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);
} |
Partager