robot.ino

#include <AFMotor.h>
#include <Servo.h>

Servo name_servo;
AF_DCMotor front_left(2);
AF_DCMotor back_left(1);
AF_DCMotor back_right(3);
AF_DCMotor front_right(4);

char data = 0;
int servo_position = 0 ;

void forward(){
  Serial.println("All Motors go forward up..");
  front_left.run(FORWARD);
  back_left.run(FORWARD);
  back_right.run(FORWARD);
  front_right.run(FORWARD);
  front_left.setSpeed(100);
  back_left.setSpeed(100);
  back_right.setSpeed(100);
  front_right.setSpeed(100);
}

void backward(){
  Serial.println("All Motors go backward..");
  front_left.run(BACKWARD);
  back_left.run(BACKWARD);
  back_right.run(BACKWARD);
  front_right.run(BACKWARD);
  front_left.setSpeed(100);
  back_left.setSpeed(100);
  back_right.setSpeed(100);
  front_right.setSpeed(100);
}

void left(){
  Serial.println("Moving left....");
  front_left.run(BACKWARD);
  front_left.setSpeed(200);
  back_left.run(BACKWARD);
  back_left.setSpeed(200);
  front_right.run(FORWARD);
  front_right.setSpeed(200);
  back_right.run(FORWARD);
  back_right.setSpeed(200);
  delay(200);
  stop_motors();
}

void right(){
  Serial.println("Moving right...");
  front_left.run(FORWARD);
  front_left.setSpeed(200);
  back_left.run(FORWARD);
  back_left.setSpeed(200);
  front_right.run(BACKWARD);
  front_right.setSpeed(200);
  back_right.run(BACKWARD);
  back_right.setSpeed(200);
  delay(200);
  stop_motors();
}

void stop_motors(){
  Serial.println("stopping...");
  front_left.run(RELEASE);
  front_right.run(RELEASE);
  back_left.run(RELEASE);
  back_right.run(RELEASE);
}

void setup() {
  // put your setup code here, to run once:
  pinMode(13, OUTPUT);
  Serial.begin(9600);
  name_servo.attach(10);
}

void loop() {
  data = 0;
  if(Serial.available() > 0){
    data = Serial.read();
    Serial.println(data);
  }
  if(data=='1'){
    forward();
    delay(500);
    stop_motors();
    name_servo.write(0);
  }
  else if(data=='2'){
    backward();
    delay(500);
    stop_motors();
    name_servo.write(0);
  }
  else if(data == '3'){
    servo_position += 4;
    name_servo.write(servo_position);
    delay(10);
    right();
  }else if(data == '4'){
    servo_position -= 4;
    name_servo.write(servo_position);
    delay(10);
    left();
  }  
}