Arduino Robot

#include <Arduino.h>
#include <Servo.h>

// for servo motor
Servo servo; // PWM on pin 9,10 are taken away, use pin 3,5,6 and 11 instead 
int SERVO_PIN = 2; // can be any digital pin

// for ultrasonic sensor
const float SPEED_OF_SOUND = 0.0345;
int TRIG_PIN = 13;
int ECHO_PIN = 12;

// for motor
int MOTOR_FRONT_FORWARD_PIN = 11;
int MOTOR_BACK_FORWARD_PIN = 10;

int MOTOR_FRONT_BACKWARD_PIN = 9;
int MOTOR_BACK_BACKWARD_PIN = 8;

// launch the ball
void launch() {
  Serial.println("Launching the ball");
  for(int i = 0; i < 130; i++)
  {
    servo.write(i);
    delay(5);
  }
}

/**
 * @brief Measures distance using ultrasonic sensor
 * @return Distance in centimeters, or -1 if reading failed
 */
float distanceAway() {
  // tell the sensor to send the signal to obtain the distance away
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10); // delay 10 micro-seconds for the sensor to send the signal
  digitalWrite(TRIG_PIN, LOW);

  // read the signal from the sensor to calculate the distance
  unsigned long microSecs = pulseIn(ECHO_PIN, HIGH);
  float cms = microSecs * SPEED_OF_SOUND / 2;


  return cms;
}

void moveForward() {
  // move the motor forward
  digitalWrite(MOTOR_FRONT_FORWARD_PIN, HIGH);
  digitalWrite(MOTOR_BACK_FORWARD_PIN, HIGH);

  // turn off the backward motor
  digitalWrite(MOTOR_FRONT_BACKWARD_PIN, LOW);
  digitalWrite(MOTOR_BACK_BACKWARD_PIN, LOW);
}
void moveBackward() {
  // move the motor backward
  digitalWrite(MOTOR_FRONT_BACKWARD_PIN, HIGH);
  digitalWrite(MOTOR_BACK_BACKWARD_PIN, HIGH);

  // turn off the forward motor
  digitalWrite(MOTOR_FRONT_FORWARD_PIN, LOW);
  digitalWrite(MOTOR_BACK_FORWARD_PIN, LOW);
}
void stopMotor() {
  // turn off the forward motor
  digitalWrite(MOTOR_FRONT_FORWARD_PIN, LOW);
  digitalWrite(MOTOR_BACK_FORWARD_PIN, LOW);

  // turn off the backward motor
  digitalWrite(MOTOR_FRONT_BACKWARD_PIN, LOW);
  digitalWrite(MOTOR_BACK_BACKWARD_PIN, LOW);
}

void setup() {
  // for servo motor
  servo.attach(SERVO_PIN); // bigger than 500 and less than 2500
  servo.write(0); // start from 0 degree

  // for ultrasonic sensor
  // the smallest distance the sensor can measure is 3cm
  pinMode(TRIG_PIN, OUTPUT);
  digitalWrite(TRIG_PIN, LOW); // to make sure the sensor is off
  pinMode(ECHO_PIN, INPUT); // to read the signal from the sensor

  // for motor 
  pinMode(MOTOR_FRONT_FORWARD_PIN, OUTPUT); // to move the motor forward
  digitalWrite(MOTOR_FRONT_FORWARD_PIN, LOW); // to make sure the motor is off

  pinMode(MOTOR_BACK_FORWARD_PIN, OUTPUT); // to move the motor forward
  digitalWrite(MOTOR_BACK_FORWARD_PIN, LOW); // to make sure the motor is off

  pinMode(MOTOR_FRONT_BACKWARD_PIN, OUTPUT); // to move the motor backward
  digitalWrite(MOTOR_FRONT_BACKWARD_PIN, LOW); // to make sure the motor is off

  pinMode(MOTOR_BACK_BACKWARD_PIN, OUTPUT); // to move the motor backward
  digitalWrite(MOTOR_BACK_BACKWARD_PIN, LOW); // to make sure the motor is off

  moveForward(); // Start moving forward!!!

  Serial.begin(9600); // for debugging purposes
}

boolean launched = false;
void loop() {
  float distance = distanceAway();

  // we should stop robot to launch the ball
  if (distance < 10 && !launched) { // this 10cm is just a placeholder, we need to adjust it
    // stop & launch the ball
    stopMotor();
    launch();
    delay(1000); // wait for the ball to be launched
    launched = true; // mission accomplished!
  }  

  if (launched) {
    moveBackward(); // keep moving forward
  }
}