1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
|
//base code for six direction driving
int led = 13;
int m1enA = 6;
int m1enB = 5;
int m1pwm1 = 4;
int m1pwm2 = 3;
int m1encoA = 32;
int m1encoB = 31;
int m2enA = 12;
int m2enB = 11;
int m2pwm1 = 9;
int m2pwm2 = 10;
int m2encoA = 30;
int m2encoB = 29;
int m3enA = 27;
int m3enB = 26;
int m3pwm1 = 24;
int m3pwm2 = 25;
int m3encoA = 28;
int m3encoB = 23;
void setup() {
pinMode(m1enA, OUTPUT);
pinMode(m1enB, OUTPUT);
pinMode(m1pwm1, OUTPUT);
pinMode(m1pwm2, OUTPUT);
pinMode(m2enA, OUTPUT);
pinMode(m2enB, OUTPUT);
pinMode(m2pwm1, OUTPUT);
pinMode(m2pwm2, OUTPUT);
pinMode(m3enA, OUTPUT);
pinMode(m3enB, OUTPUT);
pinMode(m3pwm1, OUTPUT);
pinMode(m3pwm2, OUTPUT);
Serial.begin(9600);
Serial.print("start");
}
void drive(int speed, int direction){
//
// Direction............|.Motor 1.|.Motor 2.|.Motor 3.
// 0deg(forward)........|....0....|....-....|....+....
// 60deg................|....+....|....-....|....0....
// 120deg...............|....+....|....0....|....-....
// 180 deg (backward)...|....0....|....+....|....-....
// 240 deg..............|....-....|....+....|....0....
// 300 deg..............|....-....|....0....|....+....
//
if(speed<0){
speed = -speed;
}
digitalWrite(m1enA, HIGH);
digitalWrite(m2enA, HIGH);
digitalWrite(m2enA, HIGH);
digitalWrite(m1enB, LOW);
digitalWrite(m2enB, LOW);
digitalWrite(m3enB, LOW);
if(direction>330 && direction<=30){
//0 deg
digitalWrite(m1pwm1, LOW);
digitalWrite(m1pwm2, LOW);
digitalWrite(m2pwm1, LOW);
digitalWrite(m2pwm2, HIGH);
digitalWrite(m3pwm1, HIGH);
digitalWrite(m3pwm2, LOW);
}else if (direction>30 && direction <= 90){
//60 deg
digitalWrite(m1pwm1, HIGH);
digitalWrite(m1pwm2, LOW);
digitalWrite(m2pwm1, LOW);
digitalWrite(m2pwm2, HIGH);
digitalWrite(m3pwm1, LOW);
digitalWrite(m3pwm2, LOW);
}else if (direction>90 && direction <= 150){
//120 deg
digitalWrite(m1pwm1, HIGH);
digitalWrite(m1pwm2, LOW);
digitalWrite(m2pwm1, LOW);
digitalWrite(m2pwm2, LOW);
digitalWrite(m3pwm1, LOW);
digitalWrite(m3pwm2, HIGH);
}else if (direction > 150 && direction <= 210) {
//180 degrees
digitalWrite(m1pwm1, LOW);
digitalWrite(m1pwm2, LOW);
digitalWrite(m2pwm1, HIGH);
digitalWrite(m2pwm2, LOW);
digitalWrite(m3pwm1, LOW);
digitalWrite(m3pwm2, HIGH);
}else if (direction > 210 && direction <= 270) {
//240 degrees
digitalWrite(m1pwm1, LOW);
digitalWrite(m1pwm2, HIGH);
digitalWrite(m2pwm1, HIGH);
digitalWrite(m2pwm2, LOW);
digitalWrite(m3pwm1, LOW);
digitalWrite(m3pwm2, LOW);
}else if (direction > 270 && direction <= 330) {
//300 degrees
digitalWrite(m1pwm1, LOW);
digitalWrite(m1pwm2, HIGH);
digitalWrite(m2pwm1, LOW);
digitalWrite(m2pwm2, LOW);
digitalWrite(m3pwm1, HIGH);
digitalWrite(m3pwm2, LOW);
}else{
Serial.println("how?");
}
}
// the loop routine runs over and over again forever:
void loop() {
drive(255, 120);
}
|
