berginer bot V2

/*
 this coad put to gether by Charlie Watson
 thank you to Geir Andersen servo code help
 joe the maker of homer
 */
 #include <Servo.h>
 Servo myservo; 
 Servo cam;
int leftspeed = 0;
int rightspeed = 0;
const int maxspeed = 255;
int pos = 0;    // variable to store the servo position
const int IRdis = 200; // inferred bufferzone
 const int pingPin = 5; // constant for PING pin
int val = 0;    //  UNUSED
int val2 = 0;   //left value,check,ping
int val3 = 0;  //right value,check, ping
int val4 = 0;  //used for ping
int val5 = 0; //IR 0
int val6 =0; //IR 1
void setup() 
{
  myservo.attach(4);    //servo on pin4
  cam.attach(10);       // flycamone v2 pin10
  pinMode(5,INPUT);     //Ping Ultrasonick  pin5
  pinMode(7,OUTPUT);    // enable pin
  pinMode(8,OUTPUT);    //left REV
  pinMode(3, OUTPUT);   //enable pin
  pinMode(12, OUTPUT);   //left Fwd
  pinMode(13, OUTPUT);   //right rev
  pinMode(11, OUTPUT);   //right fwd
  Serial.begin(9600);
  cam.write(0);           //set up for taking pohtos with a flycam1 v2 i read the manual
  delay(5000);             // let cam power up
  cam.write(180);          //set cam to cockpit view flip 180deg
   delay(10000);           // hold at 180 for  10seconds this will set the flip
   cam.write(0);            //0 posion for off
   delay(5000);              //wate to see red ,red, green from flycam1 v2 LED
  
   cam.write(180);           //the flycan1 v2 defult startup mode is video  set mode to serial picturs >(photo every 2 seconds) from video
delay(3000);                 //hold on (180deg)for 3seconds switchs from vidio to serial pictures
cam.write(0);                // off for a second
delay(1000);                 //wate the second
cam.write(180);              //set to next mode (photo)
delay(3000);                 //hold on (180deg) for 3seconds, switch's from serial picturs to photo(1 photo on comand )                  
cam.write(0);                // off and go to the main
}
//MAIN LOOP TRY TO KEEP SIMPLE
void loop(){
                               
    myservo.write(90);              
    delay(375); 
  OVOID();
  }

 void PING()                     //from the arduino exampls
 {
    long duration, inches, cm;
    pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
    pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

  // convert the time into a distance
  cm = microsecondsToCentimeters(duration);
  val4 = cm;
 }
  
long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

void YELD_LR()
{ 
  analogWrite(3,0);analogWrite(11,0);//stop all motors
  takepick();     // take a photo
}

void REVERS()
{
  digitalWrite(8,LOW);
  digitalWrite(7,HIGH);
   digitalWrite(13,HIGH);
  digitalWrite(12,LOW);
  analogWrite(11,leftspeed);
  analogWrite(3,rightspeed);
   CHECK();
   if(val2  && val3 < 30 ){REVERS();}
   IRLR();
   if (val5 && val6 > IRdis  ){REVERS();}
}

void FWARD()
{
  IRLR();
  digitalWrite(7,LOW);
  digitalWrite(8,HIGH);
   digitalWrite(12,HIGH);
  digitalWrite(13,LOW);
  analogWrite(11,rightspeed);
  analogWrite(3,leftspeed);
}

void CHECK(){                                // in steps of 1 degree
  myservo.write(0);              // tell servo to go to position in variable 'pos'
  delay(375);     
  PING();
 val2 = val4; //left value                              
  myservo.write(180);              //
  delay(700);                       //   
PING();
val3 = val4;                              
    myservo.write(90);              //
    delay(375);                       //right value
}

void IRLR(){
val5 = analogRead(0);
val6 = analogRead(1);
if(val5 && val6 > IRdis){REVERS();}
if (val6 - val5 >0){leftspeed = maxspeed -(val6 - val5) * leftspeed / 3.14;rightspeed = maxspeed; }
else if(val6 - val5 <0){rightspeed = maxspeed - (val5 - val6) * rightspeed / 3.14 ;leftspeed = maxspeed;}
else if(val6 - val5 ==0){leftspeed = maxspeed; rightspeed = maxspeed;}
}

void RIGHT_1()
{
  digitalWrite(7,HIGH);
  digitalWrite(8,LOW);
   digitalWrite(12,HIGH);
  digitalWrite(13,LOW);
  analogWrite(11,rightspeed);
  analogWrite(3,leftspeed);
}

void LEFT_1()
{
 
  digitalWrite(7,LOW);
  digitalWrite(8,HIGH);
   digitalWrite(12,LOW);
  digitalWrite(13,HIGH);
  analogWrite(11,rightspeed);
  analogWrite(3,leftspeed);
}

void OVOID(){                          
 PING();
   if (val4 <= 30 ){
   YELD_LR();CHECK();
        if (val2 > val3){LEFT_1();}
            else {
                 if (val3 > val2){RIGHT_1();}else{if(val2 < 15 && val3 < 15 ){REVERS();}}}
                  
   }else{FWARD();}}
 
 void takepick(){       // this is were i triger the flycam1 v2
   cam.write(180);     // no (180deg)
   delay(200);         // small hold to short the cam will not triger 
   cam.write(0);      // and off again
 }