PROGRAM ARDUINO ROBOT GO GREEN

/*
 * ROBOT GO GREEN
 *
 * SVN Keywords
 * ----------------------------------
 * $Author:  MUTIARA DIGITEK

 * $Date:  1-1-2013
 * $
 * ----------------------------------
 */

#include <Ultrasonic.h>
#include <Servo.h>

Servo servoLeft;          // Define left servo
Servo servoRight;         // Define right servo

#define TRIGGER_PIN  12
#define ECHO_PIN     13

Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN);

void setup()
  {
    servoLeft.attach(3);  // Set left servo to digital pin 3 DAN 5
    servoRight.attach(5);  //
  Serial.begin(115200);
   pinMode(2, OUTPUT);//GERAK MAJU MUNDUR
 
   pinMode(4, OUTPUT);//GERAK MAJU MUNDUR
 
 
   pinMode(7, OUTPUT);//GERAK MAJU MUNDUR
   pinMode(8, OUTPUT);//GERAK MAJU MUNDUR
 
  }

void loop()
  {
  float cmMsec, inMsec;
  long microsec = ultrasonic.timing();

  cmMsec = ultrasonic.convert(microsec, Ultrasonic::CM);
  inMsec = ultrasonic.convert(microsec, Ultrasonic::IN);
  Serial.print("MS: ");
  Serial.print(microsec);
  Serial.print(", CM: ");
  Serial.print(cmMsec);
  Serial.print(", IN: ");
  Serial.println(inMsec);

  if ((cmMsec) < 50)  { digitalWrite(2, LOW);//GERAK MAJU MUNDUR
                        digitalWrite(8, LOW);//GERAK MAJU MUNDUR
                           delay(500);
                        digitalWrite(4, HIGH);//GERAK MAJU MUNDUR
                        digitalWrite(7, HIGH); //GERAK MAJU MUNDUR
                     
                   {     forward();         // GERAK TANGAN  Example: move forward
                        delay(500);                          }
                     
                     {   turnLeft();       // GEREAK TANGAN Example: move TURNLEFT
                        delay(500);                          }
                     
                     
                     
                     {   stopRobot();      // GERAK TANGAN Example: move STOPROBOT
                        delay(500); }
                 
                   }
                 
                     { digitalWrite(4, LOW);//GERAK MAJU MUNDUR
                       digitalWrite(7, LOW);}//GERAK MAJU MUNDUR
                     
                       delay(1500);
                      { digitalWrite(2, HIGH);//GERAK MAJU MUNDUR
                       digitalWrite(8, HIGH); }//GERAK MAJU MUNDUR
                     
                     
                       
                           



  delay(100);
  }

void forward() {
  servoLeft.write(0);
  servoRight.write(180);
}
void turnRight() {
  servoLeft.write(180);
  servoRight.write(180);
}
void turnLeft() {
  servoLeft.write(0);
  servoRight.write(0);
}
void reverse() {
  servoLeft.write(180);
  servoRight.write(0);
}

void stopRobot() {
  servoLeft.write(90);
  servoRight.write(90);
}