#include <AFMotor.h>
#include <SoftwareSerial.h>
SoftwareSerial mySerial(A1, A0); // RX, TX
char order; //Variable to hold Serial Command
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);
mySerial.begin(9600);
// mySerial.println("Hello, world?");
}
void loop() {
if (mySerial.available())
order = mySerial.read();
if (order == 'f') {
forward(200);
}
if (order == 'b') {
reverse(100);
}
if (order == 'r') {
tRight(100);
}
if (order == 'l') {
tLeft(100);
}
if (order == 's') {
stopBot(100);
}
if (order == 'd') {
lDown(100);
}
if (order == 'u') {
lUp(100);
}
stopBot(10); // go forward for 10 milliseconds
}
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);
}