#include <AFMotor.h>
AF_DCMotor motor2(2, MOTOR12_64KHZ); // create motor #2, 64KHz pwm
AF_DCMotor motor3(3, MOTOR12_64KHZ); // create motor #2, 64KHz pwm
void setup() {
motor2.setSpeed(250); // set the speed to 200/255
motor3.setSpeed(250); // set the speed to 200/255
pinMode(A0, INPUT_PULLUP); // Set pin to High state and an Input
pinMode(A1, INPUT_PULLUP); // Set pin to High state and an Input
pinMode(A4, INPUT_PULLUP); // Set pin to High state and an Input
pinMode(A5, INPUT_PULLUP); // Set pin to High state and an Input
// delay(3000); //Chill a bit before motors start
}
void f(int d) { // Call Forward
motor2.setSpeed(200); // set the speed to 150/255
motor3.setSpeed(200); // set the speed to 150/255
motor3.run(FORWARD); // turn it on going forward
motor2.run(FORWARD); // turn it on going forward
delay(d);
}
void b(int d) { // Call Backup
motor2.setSpeed(250); // set the speed to 200/255
motor3.setSpeed(250); // set the speed to 200/255
motor3.run(BACKWARD); // the other way
motor2.run(BACKWARD); // the other way
delay(d);
}
void r(int d) { // Call Turn Right
motor2.setSpeed(250); // set the speed to 200/255
motor3.setSpeed(250); // set the speed to 200/255
motor3.run(BACKWARD); // turn it on going reverse
motor2.run(FORWARD); // turn it on going forward
delay(d);
}
void l(int d) { // Call Turn Left
motor2.setSpeed(250); // set the speed to 200/255
motor3.setSpeed(250); // set the speed to 200/255
motor3.run(FORWARD); // turn it on going forward
motor2.run(BACKWARD); // turn it on going reverse
delay(d);
}
void s(int d) { // Call Stop
motor2.run(RELEASE); // stopped
motor3.run(RELEASE); // stopped
delay(d);
}
void loop() {
if (digitalRead(A1) == LOW) {
s(100); // 0.1 second Stop
b(200); // 1/2 second Backup
s(100); // 0.1 second Stop
l(200); // 1/2 Turn Left
}
if (digitalRead(A4) == LOW) {
s(100); // 0.1 second Stop
b(300); // 1/2 second Backup
s(100); // 0.1 second Stop
r(300); // 1/2 Turn Right
}
if (analogRead(A0) > 500) {
s(100); // 0.1 second Stop
r(500); // 1/2 Turn Left
}
if (analogRead(A5) > 500) {
s(100); // 0.1 second Stop
l(500); // 1/2 Turn Right
}
f(5); // go forward for 5 milliseconds
}