/* File: Land_Rover_Defender_Programmed_1
 * Arduino with motor shield - random pattern
 *
 * Hardware:
 * - Arduino Uno Rev 3 ( http://arduino.cc/en/Main/ArduinoBoardUno ) 
 * - Arduino Motor Shield R3 ( http://arduino.cc/en/Main/ArduinoMotorShieldR3 )
 * - 6V DC motor is driving the rear axle
 * - Micro servo (5V) is steering the car
 *
 * Configuration and Pins:
 * - A single 9V battery is connected to the Vin and Gnd on the
 *   motor shield screw terminal block with an on/off switch
 * - The 6V drive motor has two wires, one yellow and one blue. The
 *   blue wire is on (+) for channel A, and the yellow is (-).
 * - The servo steering motor has three pins, brown is ground,
 *   red is 5V and yellow is PWM on pin 10.
 *
 * Motor Shield Pins:
 *  D3 - (Ch A) PWM_A
 *  D5 - TinkerKit Analog Output
 *  D6 - TinkerKit Analog Output
 *  D8 - (Ch B) BRK_B, set HIGH to brake, LOW to spin down
 *  D9 - (Ch A) BRK_A, set HIGH to brake, LOW to spin down
 *  D11 - (Ch B) PWM_B 
 *  D12 - (Ch A) DIR_A, Direction, set HIGH or LOW
 *  D13 - (Ch B) DIR_B, Direction, set HIGH or LOW
 *  A0 - (Ch A) SNS0, Current sensing
 *  A1 - (Ch B) SNS1, Current sensing
 *  A2 - TinkerKit Analog Input
 *  A3 - TinkerKit Analog Input
 *  *** Note for PWM_A/PWM_B:
 *  Use analogWrite(pin,value) to generate a PWM wave. After a call
 *  to analogWrite() the pin will generate a steady square wave of
 *  the specified duty cycle until the next call to analogWrite()
 *  or a call to digitalRead() or digitalWrite() on the same pin.
 *  The frequency of the PWM signal is approximately 490 Hz. The
 *  value is between 0 (always off) and 255 (always on).
 *  *** Note for SNS0/SNS1:
 *  Measure the current going through the DC motor by reading the
 *  SNS0 and SNS1 pins. On each channel will be a voltage
 *  proportional to the measured current, which can be read as a
 *  normal analog input, through the function analogRead() on the
 *  analog input A0 and A1. It is calibrated to be 3.3V when the
 *  channel is delivering its maximum possible current (2A).
 *
 * Top Pins, Right to Left:
 *  D0  - Not Used, RX
 *  D1  - Not Used, TX
 *  D2  - Not Used
 *  D3  - PWM_A
 *  D4  - Not Used
 *  D5  - Not Used, TinkerKit Analog Output
 *  D6  - Not Used, TinkerKit Analog Output
 *  D7  - Not Used
 *  D8  - Not Used, BRK_B
 *  D9  - BRK_A, set LOW to free spin
 *  D10 - PWM_S, 180 degree servo for steering
 *  D11 - Not Used, PWM_B
 *  D12 - DIR_A, set HIGH for foward, LOW for reverse
 *  D13 - Not Used, DIR_B
 *  GND - Not Used
 *  AREF - Not Used
 *  SDA - Not Used
 *  SCL - Not Used
 *
 * Bottom Pins, Right to Left:
 *  A5 - Not Used
 *  A4 - Not Used
 *  A3 - Not Used, TinkerKit Analog Input
 *  A2 - Not Used, TinkerKit Analog Input
 *  A1 - Not Used, SNS1
 *  A0 - SNS0, used to read current on drive motor, Ch A.
 *  Vin - Not Used
 *  Gnd - Not Used
 *  Gnd - Steering servo motor ground (brown pin)
 *  5V - Steering servo motor power (middle or red pin)
 *  3.3V - Not Used
 *  RESET - Not Used
 *  IOREF - Not Used
 */

#include <Servo.h>
Servo strservo; // create servo object for steering

/* Set all constants, such as pin numbers */
const int SNS0 = 0;   // Read current on drive motor
const int PWM_A = 3;  // Drive motor speed, (0=stop, 255=max)
const int DIR_A = 12; // Drive motor direction (H = forward)
const int BRK_A = 9;  // Drive motor brake (L = free spin)
const int PWM_S = 10; // Steering servo (90 = straight)

/* Initialize all variables */
int str_pos = 90; // variable to store the steering servo position
int drv_spd = 0;  // variable to store the speed (0 to 255).
int drv_dir = LOW; // variable to store direction (H = forward).
int car_state = 0; // State machine for car

/* Function: setup()
 * The setup function is called only once and should perform
 * all initialization.
 */
void setup()
{
  // Set pin modes
  pinMode(BRK_A, OUTPUT);
  pinMode(DIR_A, OUTPUT);
  pinMode(PWM_A, OUTPUT);

  // One time initializations
  strservo.attach(PWM_S); // attach steering servo
  digitalWrite(BRK_A, LOW); // set brake to free spin
  analogWrite(PWM_A, drv_spd); // set speed
  digitalWrite(DIR_A, drv_dir); // set direction
  randomSeed(analogRead(0)); // init random numbers

  /* To kickoff (and perform a limited power-on self-test) we will
   * rotate the steering left and right, then drive backward and
   * forward for a few centimeters.
   */

  // First we try out the steering servo
  for (str_pos = 90; str_pos < 180; str_pos += 1)
  { // go from 90 to 180 degrees in steps of 1 degree
    strservo.write(str_pos);
    delay(10); // wait 10 ms for the servo to reach position
  }
  delay(1000); // wait for a second
  for (str_pos = 180; str_pos > 0; str_pos -= 1)
  { // go from 180 to 0 degrees in steps of 1 degree
    strservo.write(str_pos);
    delay(10); // wait 10 ms for the servo to reach position
  }
  delay(1000); // wait for a second
  // Return to pointing the wheels straight ahead
  strservo.write(90);
  delay(1000); // wait for a second

  // Now we try out the drive motor
  drv_spd = 64; // 256/4 = 64 or one-quarter speed
  analogWrite(PWM_A, drv_spd);
  delay(2000); // drive two seconds
  drv_spd = 0; // Full stop
  analogWrite(PWM_A, drv_spd);
  delay(1000); // wait one second
  drv_dir = HIGH; // go forward
  digitalWrite(DIR_A, drv_dir);
  drv_spd = 64; // all ahead one quarter speed
  analogWrite(PWM_A, drv_spd);
  delay(2000); // drive two seconds
  drv_spd = 0; // Full stop
  analogWrite(PWM_A, drv_spd);
  delay(1000); // wait one second
}

/* Function: loop()
 * The loop function is the main loop, called repeatedly until
 * the Arduino is powered off.
 */
void loop()
{
  // Do something different depending on the state
  switch (car_state) {
  case 0:  // Drive forward
    drv_dir = HIGH; // go forward
    digitalWrite(DIR_A, drv_dir);
    drv_spd = 192; // (256/4)*3 = three quarter speed
    analogWrite(PWM_A, drv_spd);
    delay(5000); // do this for five seconds
    break;
  case 1: // Drive forward left
    str_pos = 1; // 1 = left
    strservo.write(str_pos);
    drv_dir = HIGH; // go forward
    digitalWrite(DIR_A, drv_dir);
    drv_spd = 192; // (256/4)*3 = three quarter speed
    analogWrite(PWM_A, drv_spd);
    delay(5000); // do this for five seconds
    str_pos = 90; // 90 = straight
    strservo.write(str_pos);
    // Without the following delay the subsequent servo write
    // has no effect.
    delay(10); // wait for servo to finish
    break;
  case 2: // Drive forward right
    str_pos = 179; // 179 = right
    strservo.write(str_pos);
    drv_dir = HIGH; // go forward
    digitalWrite(DIR_A, drv_dir);
    drv_spd = 192; // (256/4)*3 = three quarter speed
    analogWrite(PWM_A, drv_spd);
    delay(5000); // do this for five seconds
    str_pos = 90; // 90 = straight
    strservo.write(str_pos);
    delay(10); // wait for servo to finish
    break;
  case 3: // Go backward
    // Without a full stop before going backward the motor draws
    // too much current and the board resets.
    drv_spd = 0; // Full stop
    analogWrite(PWM_A, drv_spd);
    delay(3000); // wait three seconds
    drv_dir = LOW; // Go backward
    digitalWrite(DIR_A, drv_dir);
    drv_spd = 192; // (256/4)*3 = three quarter speed
    analogWrite(PWM_A, drv_spd);
    delay(5000); // do this for five seconds
    break;
  case 4: // Stop
    drv_spd = 0; // Full stop
    analogWrite(PWM_A, drv_spd);
    delay(3000); // do this for three seconds
    break;
  case 5:
    // 90 is straight, but for this platform it pulls left.
    // Using 95 it still drifts a little left, but it's better
    str_pos = 95;
    strservo.write(str_pos);
    drv_dir = HIGH; // go forward
    digitalWrite(DIR_A, drv_dir);
    drv_spd = 192; // (256/4)*3 = three quarter speed
    analogWrite(PWM_A, drv_spd);
    delay(5000); // do this for five seconds
  }
  if (car_state < 5)
  {
    car_state += 1;
  }
}