Table Cleaner Voice Controlled Arduino Robot + WiFi Camera

#include <AFMotor.h>
#include <Servo.h>
#include <NewPing.h>
#define TRIG_PIN A14 
#define ECHO_PIN A15
#define MAX_DISTANCE 200 
#define MAX_SPEED 235 
#define MAX_SPEED_OFFSET 20 
AF_DCMotor motor1(4); 
AF_DCMotor motor2(2); 
Servo myservo1, myservo2, myservo3, myservo4;
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 
boolean goesForward=false;
int distance = 100;
int speedSet = 0;
int lights = 13;
int headangle=60;
int neckangle=120;
char command;

void setup() 
{   
  pinMode(lights, OUTPUT);
  myservo1.attach(11);
  myservo2.attach(9);
  myservo3.attach(10);
  myservo4.attach(7);
  myservo4.write(135);
  myservo1.write(0);
  myservo2.write(headangle);
  myservo3.write(neckangle);
  int readPing();
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  Serial.begin(9600);  //Set the baud rate to your Bluetooth module.
}



 
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
}

void backward()
{
  motor1.setSpeed(255); 
  motor1.run(BACKWARD); //rotate the motor counterclockwise
  motor2.setSpeed(255); 
  motor2.run(BACKWARD); //rotate the motor counterclockwise
}


void left()
{
  motor1.setSpeed(255); //Define maximum velocity
  motor1.run(FORWARD); //rotate the motor clockwise
  motor2.setSpeed(255);
  motor2.run(BACKWARD);
}

void right()
{
  motor1.setSpeed(255);
  motor1.run(BACKWARD); //turn motor1 off
  motor2.setSpeed(255); //Define maximum velocity
  motor2.run(FORWARD); //rotate the motor clockwise
}

void camforward()
{
  headangle=headangle+5;
  myservo2.write(headangle);
  myservo3.write(neckangle);
  
}

void cambackward()
{
  headangle=headangle-5;
  myservo2.write(headangle);
  myservo3.write(neckangle);
}

void camleft()
{
  neckangle=neckangle+5;
  myservo2.write(headangle);
  myservo3.write(neckangle);
}

void camright()
{
  neckangle=neckangle-5;
  myservo2.write(headangle);
  myservo3.write(neckangle);
}

void lookAround(){
   myservo2.write(60);
  for (neckangle = 30; neckangle <= 180; neckangle += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo3.write(neckangle);              // tell servo to go to position in variable 'pos'
    delay(50);                       // waits 15ms for the servo to reach the position
  }
  for (neckangle = 180; neckangle >= 0; neckangle -= 1) { // goes from 180 degrees to 0 degrees
    myservo3.write(neckangle);              // tell servo to go to position in variable 'pos'
    delay(60);                       // waits 15ms for the servo to reach the position
  }
  for (neckangle = 0; neckangle <= 100; neckangle += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo3.write(neckangle);              // tell servo to go to position in variable 'pos'
    delay(50);                       // waits 15ms for the servo to reach the position
  }
}

void gripac()
{
  myservo1.write(0);

}

void gripkapa()
{
  myservo1.write(70);
}
  


int readPing() { 
  delay(70);
  int cm = sonar.ping_cm();
  if(cm==0)
  {
    cm = 250;
  }
  return cm;
}

int lookRight()
{
    myservo4.write(50); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo4.write(115); 
    return distance;
}

int lookLeft()
{
    myservo4.write(170); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo4.write(115);
    return distance;
    delay(100);
}



void moveStop() {
  motor1.run(RELEASE); 
  motor2.run(RELEASE);
  } 
  
void moveForward() {

 if(!goesForward)
  {
    goesForward=true;
    motor1.run(FORWARD);      
    motor2.run(FORWARD);   
   for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
   {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
    delay(5);
   }
  }
}


void moveBackward() {
    goesForward=false;
    motor1.run(BACKWARD);      
    motor2.run(BACKWARD);  
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
    delay(5);
  }
}  

void turnRight() {
  motor2.run(FORWARD);
  motor1.run(BACKWARD);     
  delay(300);
  motor2.run(FORWARD);      
  motor1.run(FORWARD);      
} 
 
void turnLeft() {
  motor2.run(BACKWARD);     
  motor1.run(FORWARD);     
  delay(300);
  motor2.run(FORWARD);     
  motor1.run(FORWARD);
}

void Stop()
{
  motor1.setSpeed(0); //Define maximum velocity
  motor1.run(RELEASE); //rotate the motor clockwise
  motor2.setSpeed(0);
  motor2.run(RELEASE); //turn motor2 off
}

void otoac()
{
 long duration, distance;
 while(command='A'){
 int distanceR = 0;
 int distanceL =  0;
 delay(40);
 if(distance<=24)
 {
  moveStop();
  delay(100);
  moveBackward();
  delay(300);
  moveStop();
  delay(200);
  distanceR = lookRight();
  delay(200);
  distanceL = lookLeft();
  delay(200);
  if(distanceR>=distanceL)
  {
    turnRight();
    moveStop();
  }else
  {
    turnLeft();
    moveStop();
  }
 }else
 {
  moveForward();
 }
 distance = readPing();
 }
}
 
void otokapa()
{
  motor1.setSpeed(0);
  motor2.run(RELEASE); //turn motor1 off
  motor2.setSpeed(0);
  motor2.run(RELEASE); //turn motor2 off
}

void loop(){
    
   if(Serial.available() > 0){ 
    command = Serial.read(); 
    Stop();
    switch(command){
    case 'F':  
      forward();
      break;
    case 'B':  
       backward();
      break;
    case 'L':  
      left();
      break;
    case 'R':
      right();
      break;
    case 'G':  
      camforward();
      break;
    case 'I':  
      cambackward();
      break;
    case 'H':  
      camleft();
      break;
    case 'J':
      camright();
      break;
    case 'W':  
      gripac();
      break;
    case 'w':  
      gripkapa();
      break;
     case 'P':  
      Stop();
      break;
      case 'U':  
      myservo2.write(10);
      myservo3.write(30);
      break;
     case 'D':  
      lookAround();
      break;
      case 'S':  
     digitalWrite(lights, HIGH);
      break;
    case 's':  
      digitalWrite(lights, LOW);
      break;
     case 'A':  
      otoac();
      break;
    case 'a':  
      otokapa();
      break;
    }
  } 
}