#include <AFMotor.h>
AF_DCMotor rMotor(1, MOTOR12_64KHZ); // create motor #1, 64KHz pwm
AF_DCMotor lMotor(2, MOTOR12_64KHZ); // create motor #2, 64KHz pwm
AF_DCMotor liftMotor(3, MOTOR12_64KHZ); // create motor #2, 64KHz pwm
void setup() {
Serial.begin(9600);
pinMode(13,OUTPUT);
delay(3000);
digitalWrite(13,HIGH);
delay(1000);
digitalWrite(13,LOW);
}
void loop() {
forward(500);
reverse(300);
tRight(1000);
forward(500);
tLeft(500);
stopBot(1000);
lUp(500);
forward(1000);
lDown(300);
stopBot(400);
while (true);
}
void forward(int dlay) {
rMotor.setSpeed(255);
lMotor.setSpeed(255);
rMotor.run(FORWARD); // turn it on going forward
lMotor.run(FORWARD); // turn it on going forward
delay(dlay);
}
void reverse(int dlay) {
rMotor.setSpeed(255);
lMotor.setSpeed(255);
rMotor.run(BACKWARD); // turn it on going forward
lMotor.run(BACKWARD); // turn it on going forward
delay(dlay);
}
void tLeft(int dlay) { //Turn Right
rMotor.setSpeed(255);
lMotor.setSpeed(255);
rMotor.run(FORWARD); // turn it on going forward
lMotor.run(BACKWARD); // turn it on going forward
delay(dlay);
}
void tRight(int dlay) { //Turn Right
rMotor.setSpeed(255);
lMotor.setSpeed(255);
rMotor.run(BACKWARD); // turn it on going forward
lMotor.run(FORWARD); // turn it on going forward
delay(dlay);
}
void stopBot(int dlay) { //Turn Right
rMotor.run(RELEASE); // Stop Motor
lMotor.run(RELEASE); // Stop Motor
liftMotor.run(RELEASE); // Stop Motor
delay(dlay);
}
void lUp(int dlay) { // Lift Upish
liftMotor.setSpeed(255);
liftMotor.run(FORWARD); // turn it on going forward
delay(dlay);
}
void lDown(int dlay) { // Lift Downish
liftMotor.setSpeed(255);
liftMotor.run(BACKWARD); // turn it on going forward
delay(dlay);
}