#include <AFMotor.
h>
#include <Servo.h>
//initial motors pin
AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(2, MOTOR12_1KHZ);
AF_DCMotor motor3(3, MOTOR34_1KHZ);
AF_DCMotor motor4(4, MOTOR34_1KHZ);
String readString;
char command;
Servo myservo1, myservo2;
void setup()
Serial.begin(9600); //Iniciamos buetooh
myservo1.attach(10); // pin de servo
myservo2.attach(9); // pin de servo
myservo1.write(90); // angulo de servo
delay(1000);
myservo1.write(120);
delay(1000);
void loop(){
if(Serial.available() > 0){
command = Serial.read();
Stop(); //initialize with motors stoped
//Change pin mode only if new command is different from previous.
//Serial.println(command);
switch(command){
case 'A': //F
forward();
break;
case 'R': //B
back();
break;
case 'I': //L
left();
break;
case 'D': //r
right();
break;
case 'X': //F
m1();
break;
case 'Y': //F
m2();
break;
void forward()
motor1.setSpeed(255); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(255); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
motor3.setSpeed(255);//Define maximum velocity
motor3.run(FORWARD); //rotate the motor clockwise
motor4.setSpeed(255);//Define maximum velocity
motor4.run(FORWARD); //rotate the motor clockwise
void back()
motor1.setSpeed(255); //Define maximum velocity
motor1.run(BACKWARD); //rotate the motor anti-clockwise
motor2.setSpeed(255); //Define maximum velocity
motor2.run(BACKWARD); //rotate the motor anti-clockwise
motor3.setSpeed(255); //Define maximum velocity
motor3.run(BACKWARD); //rotate the motor anti-clockwise
motor4.setSpeed(255); //Define maximum velocity
motor4.run(BACKWARD); //rotate the motor anti-clockwise
void left()
motor1.setSpeed(255); //Define maximum velocity
motor1.run(BACKWARD); //rotate the motor anti-clockwise
motor2.setSpeed(255); //Define maximum velocity
motor2.run(BACKWARD); //rotate the motor anti-clockwise
motor3.setSpeed(255); //Define maximum velocity
motor3.run(FORWARD); //rotate the motor clockwise
motor4.setSpeed(255); //Define maximum velocity
motor4.run(FORWARD); //rotate the motor clockwise
void right()
{
motor1.setSpeed(255); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(255); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
motor3.setSpeed(255); //Define maximum velocity
motor3.run(BACKWARD); //rotate the motor anti-clockwise
motor4.setSpeed(255); //Define maximum velocity
motor4.run(BACKWARD); //rotate the motor anti-clockwise
void Stop()
motor1.setSpeed(0); //Define minimum velocity
motor1.run(RELEASE); //stop the motor when release the button
motor2.setSpeed(0); //Define minimum velocity
motor2.run(RELEASE); //rotate the motor clockwise
motor3.setSpeed(0); //Define minimum velocity
motor3.run(RELEASE); //stop the motor when release the button
motor4.setSpeed(0); //Define minimum velocity
motor4.run(RELEASE); //stop the motor when release the button
void m1()
delay(10);
while (Serial.available()) {
char b = Serial.read();
readString += b;
if (readString.length() >0) {
Serial.println(readString.toInt());
myservo1.write(readString.toInt());
readString=""; // Clear string
void m2(){
delay(10);
while (Serial.available()) {
char b = Serial.read();
readString += b;
if (readString.length() >0) {
Serial.println(readString.toInt());
myservo2.write(readString.toInt());
readString="";
