/**********************************************************************/
/*** WMRVersionAlbertENG                                            ***/
/*** Version: 1.0                                                   ***/
/*** Date: 21-10-2013                                               ***/
/***                                                                ***/
/*** This is the program code for the Dutch LMR Robot.              ***/
/*** Let's make Robots                                              ***/
/***                                                                ***/
/*** This version is based on the WMRVersionVonFlo.                 ***/
/*** Flo > Profile: http://wirmachenroboter.com/user/19715          ***/
/***                                                                ***/
/*** Have fun!                                                      ***/
/*** Albert.  http://letsmakerobots.com/user/21141                  ***/
/**********************************************************************/

// Useful functions for controlling the servo comes from this:
#include <Servo.h>

// Which pin defines which function:
// Function       Pin     Description
#define IR_SENSOR 2    // The Sharp IR Sensor
#define PWM_A     3    // Drivespeed A
#define SERVO     5    // Servo
#define BRAKE_B   8    // Brake B
#define BRAKE_A   9    // Brake A
#define PWM_B     11   // Drivespeed B
#define DIR_A     12   // Direction A
#define DIR_B     13   // Direction B


// Useful settings:
#define FULL_SPEED        128             // Maximumspeed can't exeed 255. A higher value isn't possible.
#define TURN_SPEED        255             // Quick turn.
#define LEFT              LOW             // Turn left on LOW.
#define RIGHT             HIGH            // Turn right on HIGH.
#define CLOSEST_DISTANCE  220             // With this value for the sensor, the robot will stop. Watch out! Adjust this value to change when the robot stops.
#define SERVO_RIGHT       55              // How far the servo turns to the right.
#define SERVO_LEFT        125             // How far the servo turns to the left.

Servo SensorServo;  // This element controls the servo.

// For whitch servo position is the distance to the danger the smallest?
int SeekingPositionWithClosestDanger()
{
  // Coming to a halt.
  digitalWrite(BRAKE_A, HIGH);
  digitalWrite(BRAKE_B, HIGH);
  int ServoPosition;
  int MinimumDistance = 0;
  int MinimumServoPosition = 0;

  // From right (SERVO_RIGHT = 55)to left (SERVO_LEFT=125)checking all servo position. With Servoposition++ is the value increased with 1 every time.
  for(ServoPosition = SERVO_RIGHT;ServoPosition <= SERVO_LEFT; ServoPosition++)
  {
    // Creating servo value.
    SensorServo.write(ServoPosition);
    delay(10);
    // Is the actual value from the sensor smaller than the minimum vaulue?
    if(analogRead( IR_SENSOR ) > MinimumDistance )
    {
      // Yes: actual value is the new minimum.
      MinimumDistance = analogRead( IR_SENSOR );
      // Secure servo position.
      MinimumServoPosition = ServoPosition;
    }
  }
  // Adjust to the found position and give back the value.
  SensorServo.write(MinimumServoPosition);
  return MinimumServoPosition;
}

byte ServoPosition = 90;
boolean TurnServo = RIGHT;
// Drive straight forward and simultaniously sweep the sensor.
void DriveForward()
{
  SensorServo.write( ServoPosition );
  // Adjust both motors to drive straight forward.
  digitalWrite( DIR_A, HIGH );
  digitalWrite( DIR_B, HIGH );

  // Use maximum speed.
  analogWrite( PWM_A, FULL_SPEED );
  analogWrite( PWM_B, FULL_SPEED );

  // Release the brakes.
  digitalWrite( BRAKE_A, LOW );
  digitalWrite( BRAKE_B, LOW );

  // Is the servo turning to the right?
  if( TurnServo == LEFT )
    ServoPosition = ServoPosition+1;  // Moving further to the right. Raise value by 1.
  if( TurnServo == RIGHT )
    ServoPosition = ServoPosition-1;  // Otherwise turn to the left. Reduce the value by 1.

  // Has the servo reached the maximum left?
  if( ServoPosition > SERVO_LEFT )
    TurnServo = RIGHT; // Turn to the right.
  if( ServoPosition < SERVO_RIGHT )
    TurnServo = LEFT;  // Otherwise turn to the left.
}

// Turn, but where to? Left for counter clockwise otherwise right.
void Turn( boolean Direction )
{
  // Brake
  digitalWrite( BRAKE_A, HIGH );
  digitalWrite( BRAKE_B, HIGH );
  delay( 500 );

  digitalWrite( DIR_A, Direction );    // Turn motor A in that "direction".
  digitalWrite( DIR_B, !Direction );   // Turn motor B in the opposite "direction".

  // Adjust speed to turn.
  analogWrite( PWM_A, TURN_SPEED );
  analogWrite( PWM_B, TURN_SPEED );

  // Release brakes.
  digitalWrite( BRAKE_A, LOW );
  digitalWrite( BRAKE_B, LOW );

  //Turn until the sensor measures 10% more distance.
  while( ( CLOSEST_DISTANCE * 1 ) < analogRead( IR_SENSOR ) )
  {
    delay( 50 );
  }
  // Stop.
  digitalWrite( BRAKE_A, HIGH );
  digitalWrite( BRAKE_B, HIGH );
  delay(1000);
}

// Before everything starts, this function will be executed once.
void setup( )
{
  //Motor A (right) initialise
  pinMode( DIR_A, OUTPUT );    // Set the pin for the direction for motor A to an output.
  pinMode( BRAKE_A, OUTPUT );  // Set the pin for the brake for motor A to an output.
  pinMode( DIR_B, OUTPUT );    // Set the pin for the direction for motor B to an output.
  pinMode( BRAKE_B, OUTPUT );  // Set the pin for the brake for motor A to an output.

  // Turn on both brakes. HIGH = braking.
  digitalWrite( BRAKE_A, HIGH );
  digitalWrite( BRAKE_B, HIGH );

  // Initialise servo and set to 90°.
  SensorServo.attach( SERVO );
  SensorServo.write( 90 );
  delay( 500 );

  // Wait until an object apears in front of the sensor.
  while( CLOSEST_DISTANCE  > analogRead( IR_SENSOR ) )
  {
    // This is the delay time.
    delay( 100 );
  }

  // Start!!
}

// The actual program sequence. After the setup( ) has been run through the funcion loop( ) will be endless repeated.
void loop( )
{
  int DangerPosition;
  int Distance;

  Distance = analogRead( IR_SENSOR );    // Measure the distance from the sensor
  if(Distance > CLOSEST_DISTANCE )       // Close enough? Is there an obstacle nearby, then a higher value will return otherwise a smaller value.
  {
    DangerPosition = SeekingPositionWithClosestDanger();  // Scan everything again to know precisely where the following danger is located.

    // Is the danger on the left?
    if(DangerPosition <= 90)
    {
      Turn( RIGHT );  // Turn to the right.
    }
    // Is the danger on the right?
    if( DangerPosition > 90 )
    {
      Turn( LEFT );   // Turn to the left.
    }
  }
  DriveForward();  // Straight forward!!
  delay( 10 );
}