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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
| /*
This code conducts a few simple manoeuvres to illustrate the functions:
- motorDrive(motorNumber, motorDirection, motorSpeed)
- motorBrake(motorNumber)
- motorStop(motorNumber)
- motorsStandby
Connections:
- Pin 3 ---> PWMA
- Pin 8 ---> AIN2
- Pin 9 ---> AIN1
- Pin 10 ---> STBY
- Pin 11 ---> BIN1
- Pin 12 ---> BIN2
- Pin 5 ---> PWMB
- Motor 1: A01 and A02
- Motor 2: B01 and B02
*/
//Define the Pins
//Motor 1
int pinAIN1 = 9; //Direction
int pinAIN2 = 8; //Direction
int pinPWMA = 3; //Speed
//Motor 2
int pinBIN1 = 11; //Direction
int pinBIN2 = 12; //Direction
int pinPWMB = 5; //Speed
//Standby
int pinSTBY = 10;
//Constants to help remember the parameters
static boolean turnCW = 0; //for motorDrive function
static boolean turnCCW = 1; //for motorDrive function
static boolean motor1 = 0; //for motorDrive, motorStop, motorBrake functions
static boolean motor2 = 1; //for motorDrive, motorStop, motorBrake functions
void setup()
{
//Set the PIN Modes
pinMode(pinPWMA, OUTPUT);
pinMode(pinAIN1, OUTPUT);
pinMode(pinAIN2, OUTPUT);
pinMode(pinPWMB, OUTPUT);
pinMode(pinBIN1, OUTPUT);
pinMode(pinBIN2, OUTPUT);
pinMode(pinSTBY, OUTPUT);
}
void loop()
{
//Drive both motors CW, full speed
motorDrive(motor1, turnCW, 255);
motorDrive(motor2, turnCW, 255);
//Keep driving for 2 secs
delay(2000);
//Turn towards motor1: Stop Motor1, slow Motor2
motorStop(motor1);
motorDrive(motor2, turnCW, 192);
//Keep turning for 2 secs
delay(2000);
//Turn in opposite direction: Stop Motor2, slow Motor1
motorDrive(motor1, turnCW, 192);
delay(250);
motorStop(motor2);
//Keep turning for 2 secs
delay(2000);
//Straighten up
motorDrive(motor2, turnCW, 192);
delay(500);
//Put motors into Standby
motorsStandby();
delay(1000);
//Do a tight turn towards motor1: Motor2 forward, Motor1 reverse
motorDrive(motor1, turnCCW, 192);
motorDrive(motor2, turnCW, 192);
//Keep turning for 2 secs
delay(2000);
//Apply Brakes, then into Standby
motorBrake(motor1);
motorBrake(motor2);
motorsStandby();
//Stand still for 5 secs, then we do it all over again...
delay(5000);
}
void motorDrive(boolean motorNumber, boolean motorDirection, int motorSpeed)
{
/*
This Drives a specified motor, in a specific direction, at a specified speed:
- motorNumber: motor1 or motor2 ---> Motor 1 or Motor 2
- motorDirection: turnCW or turnCCW ---> clockwise or counter-clockwise
- motorSpeed: 0 to 255 ---> 0 = stop / 255 = fast
*/
boolean pinIn1; //Relates to AIN1 or BIN1 (depending on the motor number specified)
//Specify the Direction to turn the motor
//Clockwise: AIN1/BIN1 = HIGH and AIN2/BIN2 = LOW
//Counter-Clockwise: AIN1/BIN1 = LOW and AIN2/BIN2 = HIGH
if (motorDirection == turnCW)
pinIn1 = HIGH;
else
pinIn1 = LOW;
//Select the motor to turn, and set the direction and the speed
if(motorNumber == motor1)
{
digitalWrite(pinAIN1, pinIn1);
digitalWrite(pinAIN2, !pinIn1); //This is the opposite of the AIN1
analogWrite(pinPWMA, motorSpeed);
}
else
{
digitalWrite(pinBIN1, pinIn1);
digitalWrite(pinBIN2, !pinIn1); //This is the opposite of the BIN1
analogWrite(pinPWMB, motorSpeed);
}
//Finally , make sure STBY is disabled - pull it HIGH
digitalWrite(pinSTBY, HIGH);
}
void motorBrake(boolean motorNumber)
{
/*
This "Short Brake"s the specified motor, by setting speed to zero
*/
if (motorNumber == motor1)
analogWrite(pinPWMA, 0);
else
analogWrite(pinPWMB, 0);
}
void motorStop(boolean motorNumber)
{
/*
This stops the specified motor by setting both IN pins to LOW
*/
if (motorNumber == motor1)
{
digitalWrite(pinAIN1, LOW);
digitalWrite(pinAIN2, LOW);
}
else
{
digitalWrite(pinBIN1, LOW);
digitalWrite(pinBIN2, LOW);
}
}
void motorsStandby()
{
/*
This puts the motors into Standby Mode
*/
digitalWrite(pinSTBY, LOW);
} |
