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
126
127
128
129
130
131
132
133
134
135
|
//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....|....+....
//
digitalWrite(m1enA, HIGH);
digitalWrite(m2enA, HIGH);
digitalWrite(m3enA, HIGH);
digitalWrite(m1enB, LOW);
digitalWrite(m2enB, LOW);
digitalWrite(m3enB, LOW);
Serial.println("debug2");
Serial.println(direction);
if(direction>330 || direction<=30){
//0 deg
digitalWrite(m1pwm1, LOW);
digitalWrite(m1pwm2, HIGH);
digitalWrite(m2pwm1, LOW);
digitalWrite(m2pwm2, LOW);
digitalWrite(m3pwm1, LOW);
digitalWrite(m3pwm2, HIGH);
Serial.println("0 deg");
}else if (direction>30 && direction <= 90){
//60 deg
Serial.println("60deg");
digitalWrite(m1pwm1, LOW);
digitalWrite(m1pwm2, LOW);
digitalWrite(m2pwm1, HIGH);
digitalWrite(m2pwm2, LOW);
digitalWrite(m3pwm1, LOW);
digitalWrite(m3pwm2, HIGH);
}else if (direction>90 && direction <= 150){
//120 deg
Serial.println("120 deg");
digitalWrite(m1pwm1, HIGH);
digitalWrite(m1pwm2, LOW);
digitalWrite(m2pwm1, HIGH);
digitalWrite(m2pwm2, LOW);
digitalWrite(m3pwm1, LOW);
digitalWrite(m3pwm2, LOW);
}else if (direction > 150 && direction <= 210) {
//180 degrees
Serial.println("on 180");
digitalWrite(m1pwm1, HIGH);
digitalWrite(m1pwm2, LOW);
digitalWrite(m2pwm1, LOW);
digitalWrite(m2pwm2, LOW);
digitalWrite(m3pwm1, HIGH);
digitalWrite(m3pwm2, LOW);
}else if (direction > 210 && direction <= 270) {
//240 degrees
digitalWrite(m1pwm1, LOW);
digitalWrite(m1pwm2, LOW);
digitalWrite(m2pwm1, LOW);
digitalWrite(m2pwm2, HIGH);
digitalWrite(m3pwm1, HIGH);
digitalWrite(m3pwm2, LOW);
}else if (direction > 270 && direction <= 330) {
//300 degrees
digitalWrite(m1pwm1, LOW);
digitalWrite(m1pwm2, HIGH);
digitalWrite(m2pwm1, LOW);
digitalWrite(m2pwm2, HIGH);
Serial.println("300deg");
digitalWrite(m3pwm1, LOW);
digitalWrite(m3pwm2, LOW);
}else{
Serial.println("how?");
}
}
// the loop routine runs over and over again forever:
void loop() {
drive(255, 320);
delay(250);
}
|
