#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
// delay(3000); //Chill a bit before motors start
}
void f(int d) { // Call Forward
motor3.run(FORWARD); // turn it on going forward
motor2.run(FORWARD); // turn it on going forward
delay(d);
}
void b(int d) { // Call Backup
motor3.run(BACKWARD); // the other way
motor2.run(BACKWARD); // the other way
delay(d);
}
void r(int d) { // Call Turn Right
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
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(A0) == LOW) {
b(500); // 1/2 second stop
r(500); // 1/2 Turn Left
}
if (digitalRead(A1) == LOW) {
b(500); // 1/2 second stop
l(500); // 1/2 Turn Right
}
if (analogRead(A2) > 400) {
b(500); // 1/2 second stop
r(400); // 1/2 Turn Right
}
if (analogRead(A3) > 400) {
b(500); // 1/2 second stop
l(400); // 1/2 Turn Left
}
s(10); // go forward for 10 milliseconds
}