/*
  Wheeled-Robot Arduino Code
  version 2012.12.12.09.32

  Created 2012 08 21
  by Basile Laderchi

  Binary sketch size: 13654 bytes
 
  controls:
  motors    arm     leds  analog  test  presets  autoDrive
    w      y u i    1 2     ~      v     p1-4        0
  a s d    h j k
    x       n m
   + -       ,

  Needed:
  NewPing library: http://code.google.com/p/arduino-new-ping/

  Useful things:
  readAccelerometer code from: http://learn.parallax.com/kickstart/28017
  readVcc code from: http://provideyourown.com/2012/secret-arduino-voltmeter-measure-battery-voltage/
  longSharp_AnalogToCm code from: http://luckylarry.co.uk/arduino-projects/arduino-using-a-sharp-ir-sensor-for-distance-calculation/
*/

#include <Servo.h>
#include <NewPing.h>

const int centerMotorSpeed = 1500;
const int incdecMotorSpeed = 100;
const int minMotorSpeed = 300;
const int maxMotorSpeed = 500;
const int motorEnabledStatusDelay = 100;

const int minHeadLRPosition = 0;
const int maxHeadLRPosition = 180;
const int centerHeadLRPosition = 100;
const int incdecHeadLR = 10;

const int minHeadS1Position = 50;
const int maxHeadS1Position = 180;
const int centerHeadS1Position = 180;
const int incdecHeadS1 = 10;

const int minHeadS2Position = 0;
const int maxHeadS2Position = 180;
const int centerHeadS2Position = 150;
const int incdecHeadS2 = 10;

const int minHeadS3Position = 0;
const int maxHeadS3Position = 180;
const int centerHeadS3Position = 150;
const int incdecHeadS3 = 10;

const int maxSonarRange = 300;

const double headSweepSpeed = 0.001;

const int minDistance = 25;

const byte USB_PIN_D0 = 0;
const byte USB_PIN_D1 = 1;
const byte pinLed1 = 2;
const byte pinHeadS2 = 3;
const byte pinLed2 = 4;
const byte pinHeadS3 = 5;
const byte pinHeadLR = 6;
const byte pinAccelX = 7;
const byte pinAccelY = 8;
const byte pinMotorL = 9;
const byte pinMotorR = 10;
const byte pinHeadS1 = 11;
const byte pinMotorStop = 12;
const byte NOT_USED_D13 = 13;

const byte pinLeftSharp = A0;
const byte pinMiddleSharp = A1;
const byte pinRightSharp = A2;
const byte pinBackSharp = A3;
const byte pinSonar = A4;
const byte NOT_USED_A5 = A5;

const int armServos = 3;
/*
  {140, 110, 120},
  { 60,  60, 170},
  { 90,  60, 140},
  { 60,  10, 130}
*/
const int armPresets[][armServos] = {
  {140,  80, 120},
  { 60,   0, 140},
  { 90,  60, 110},
  {160,  70, 110}
};
const int numArmPresets = (sizeof(armPresets) / armServos) / sizeof(int);

Servo motorL;
Servo motorR;
Servo headLR;
Servo headS1;
Servo headS2;
Servo headS3;
NewPing sonar = NewPing(pinSonar, pinSonar, maxSonarRange);

unsigned long headLRTimer = 0;

char dir = 's';
char preset = '_';
char lastMotorMovement = 's';

int headLRLastPosition = centerHeadLRPosition;
byte headLRSweepEnabled = 0;
int headLRRotation;
int headS1LastPosition = centerHeadS1Position;
int headS2LastPosition = centerHeadS2Position;
int headS3LastPosition = centerHeadS3Position;
double headLRSpeed = 0;
double headLRRange = 0;

int motorLLastSpeed = centerMotorSpeed;
int motorRLastSpeed = centerMotorSpeed;
int motorLastSpeed = minMotorSpeed;
int lastMotorEnabledStatus = LOW;

int motorEasingSpeed;

int lastLed1Status = LOW;
int lastLed2Status = LOW;
int lastLeftSharpValue = 0;
int lastMiddleSharpValue = 0;
int lastRightSharpValue = 0;
int lastBackSharpValue = 0;
unsigned int lastSonarValue = 0;
int lastAccelXValue = 0;
int lastAccelYValue = 0;

byte autoDriveEnabled = 0;

double Vcc;

void setup() {
  Serial.begin(9600);
  pinMode(pinLed1, OUTPUT);
  pinMode(pinLed2, OUTPUT);
  pinMode(pinAccelX, INPUT);
  pinMode(pinAccelY, INPUT);
  pinMode(pinMotorStop, OUTPUT);
  motorL.attach(pinMotorL);
  motorR.attach(pinMotorR);
  headLR.attach(pinHeadLR);
  headS1.attach(pinHeadS1);
  headS2.attach(pinHeadS2);
  headS3.attach(pinHeadS3);
  motorStop();
  headCenter();

  Vcc = readVcc() / 1000.0;
}

void loop() {
  if (Serial.available()) {
    dir = Serial.read();
    disableAutoDrive();
    move(dir);
  }
  //oscillatingAutopilot();
  readAccelerometer();
  autoDrive();
}

void move(char dir) {
  switch (dir) {
    case 'w':  // Forward
      motorForward();
      lastMotorMovement = 'w';
      break;
    case 'x':  // Back
      motorReverse();
      lastMotorMovement = 'x';
      break;
    case 's':  // Stop
      motorStop();
      lastMotorMovement = 's';
      break;
    case 'a':  // Turn Left
      motorTurnLeft();
      lastMotorMovement = 'a';
      break;
    case 'd':  // Turn Right
      motorTurnRight();
      lastMotorMovement = 'd';
      break;
    case 'y':  // Head S1 Up
      headS1Up();
      break;
    case 'h':  // Head S1 Down
      headS1Down();
      break;
    case 'u':  // Head S2 Up
      headS2Up();
      break;
    case 'j':  // Head S2 Down
      headS2Down();
      break;
    case 'i':  // Head S3 Up
      headS3Up();
      break;
    case 'k':  // Head S3 Down
      headS3Down();
      break;
    case 'n':  // Head Left
      headLeft();
      break;
    case 'm':  // Head Right
      headRight();
      break;
    case ',':  // Head Center
    case '<':
      headCenter();
      break;
    case '+':
    case '=':
      increaseSpeed();
      break;
    case '-':
    case '_':
      decreaseSpeed();
      break;
    case '1':
      led1();
      break;
    case '2':
      led2();
      break;
    case '0':
      enableAutoDrive();
    case 'p':
      presets();
      break;
    case '~':
    case '`':
      analog();
      break;
    case 'v':
      // testHeadSweep();
      break;
  }
}

/*
void oscillatingAutopilot() {
  if (headLRSweepEnabled == 1) {
    headLRTimer++;
    headLRRotation = sin(headLRTimer * headLRSpeed) * headLRRange;
    Serial.print("Sweep: ");
    Serial.print(headLRTimer);
    Serial.print(", ");
    Serial.print(headLRRotation);
    if (headLR.read() != (centerHeadLRPosition + headLRRotation)) {
      headLR.write(centerHeadLRPosition + headLRRotation);
      Serial.print(", 1");
    } else {
      Serial.print(", 0");
    }
    Serial.println();
  }
}
*/

void readAccelerometer() {
  lastAccelXValue = pulseIn(pinAccelX, HIGH);
  lastAccelYValue = pulseIn(pinAccelY, HIGH);
  /*
  Serial.print("Accelerometer: ");
  Serial.print(lastAccelXValue);
  Serial.print(",");
  Serial.println(lastAccelYValue);
  */
}

void motorForward() {
  motorChangeSpeed(motorLastSpeed, motorLastSpeed, LOW);
}

void motorReverse() {
  motorChangeSpeed(- motorLastSpeed, - motorLastSpeed, LOW);
}

void motorStop() {
  motorChangeSpeed(0, 0, HIGH);
}

void motorTurnLeft() {
  motorChangeSpeed(- motorLastSpeed, motorLastSpeed, LOW);
}

void motorTurnRight() {
  motorChangeSpeed(motorLastSpeed, - motorLastSpeed, LOW);
}

void motorChangeSpeed(int motorLSpeed, int motorRSpeed, int motorEnabledStatus) {
  if (motorEnabledStatus == LOW) {
    if (lastMotorEnabledStatus != motorEnabledStatus) {
      digitalWrite(pinMotorStop, motorEnabledStatus);
      delay(motorEnabledStatusDelay);
    }
  }

  motorLLastSpeed = centerMotorSpeed + motorLSpeed;
  motorRLastSpeed = centerMotorSpeed + motorRSpeed;
  motorL.writeMicroseconds(motorLLastSpeed);
  motorR.writeMicroseconds(motorRLastSpeed);

  if (motorEnabledStatus == HIGH) {
    if (lastMotorEnabledStatus != motorEnabledStatus) {
      delay(motorEnabledStatusDelay);
      digitalWrite(pinMotorStop, motorEnabledStatus);
    }
  }

  lastMotorEnabledStatus = motorEnabledStatus;
}

void headS1Up() {
  headS1LastPosition += incdecHeadS1;
  headS1LastPosition = min(headS1LastPosition, maxHeadS1Position);
  headS1.write(headS1LastPosition);
}

void headS1Down() {
  headS1LastPosition -= incdecHeadS1;
  headS1LastPosition = max(headS1LastPosition, minHeadS1Position);
  headS1.write(headS1LastPosition);
}

void headS2Up() {
  headS2LastPosition -= incdecHeadS2;
  headS2LastPosition = max(headS2LastPosition, minHeadS2Position);
  headS2.write(headS2LastPosition);
}

void headS2Down() {
  headS2LastPosition += incdecHeadS2;
  headS2LastPosition = min(headS2LastPosition, maxHeadS2Position);
  headS2.write(headS2LastPosition);
}

void headS3Up() {
  headS3LastPosition += incdecHeadS3;
  headS3LastPosition = min(headS3LastPosition, maxHeadS3Position);
  headS3.write(headS3LastPosition);
}

void headS3Down() {
  headS3LastPosition -= incdecHeadS3;
  headS3LastPosition = max(headS3LastPosition, minHeadS3Position);
  headS3.write(headS3LastPosition);
}

void headCenter() {
  headLRLastPosition = centerHeadLRPosition;
  headLR.write(headLRLastPosition);
  headS1LastPosition = centerHeadS1Position;
  headS1.write(headS1LastPosition);
  headS2LastPosition = centerHeadS2Position;
  headS2.write(headS2LastPosition);
  headS3LastPosition = centerHeadS3Position;
  headS3.write(headS3LastPosition);
}

void headLeft() {
  headLRSweepEnabled = 0;
  headLRLastPosition -= incdecHeadLR;
  headLRLastPosition = max(headLRLastPosition, minHeadLRPosition);
  headLR.write(headLRLastPosition);
}

void headRight() {
  headLRSweepEnabled = 0;
  headLRLastPosition += incdecHeadLR;
  headLRLastPosition = min(headLRLastPosition, maxHeadLRPosition);
  headLR.write(headLRLastPosition);
}

void increaseSpeed() {
  motorLastSpeed += incdecMotorSpeed;
  motorLastSpeed = min(motorLastSpeed, maxMotorSpeed);
  move(lastMotorMovement);
}

void decreaseSpeed() {
  motorLastSpeed -= incdecMotorSpeed;
  motorLastSpeed = max(motorLastSpeed, minMotorSpeed);
  move(lastMotorMovement);
}

void led1() {
  lastLed1Status = ~lastLed1Status;
  digitalWrite(pinLed1, lastLed1Status);
}

void led2() {
  lastLed2Status = ~lastLed2Status;
  digitalWrite(pinLed2, lastLed2Status);
}

void presets() {
  if (Serial.available()) {
    preset = Serial.read();
    headPreset(preset - '0');
  }
}

void headPreset(int preset) {
  if (preset > 0 && preset <= numArmPresets) {
    headLRLastPosition = centerHeadLRPosition;
    headLR.write(headLRLastPosition);
    headS1LastPosition = armPresets[preset][0];
    headS1.write(headS1LastPosition);
    headS2LastPosition = armPresets[preset][1];
    headS2.write(headS2LastPosition);
    headS3LastPosition = armPresets[preset][2];
    headS3.write(headS3LastPosition);
  }
}

void enableAutoDrive() {
  autoDriveEnabled = 1;
}

void disableAutoDrive() {
  autoDriveEnabled = 0;
}

void autoDrive() {
  if (autoDriveEnabled == 0) {
    return;
  }
  
  int leftDistance,
      middleDistance,
      rightDistance;
  byte autoDriveMode,
       rnd;

  readAnalogSensors();

  leftDistance = longSharp_AnalogToCm(lastLeftSharpValue);
  middleDistance = longSharp_AnalogToCm(lastMiddleSharpValue);
  rightDistance = longSharp_AnalogToCm(lastRightSharpValue);
  autoDriveMode = 0;

  if (leftDistance >= minDistance &&
      middleDistance >= minDistance &&
      rightDistance >= minDistance) {
    autoDriveMode = 1;
  }
  if (leftDistance < minDistance &&
      middleDistance >= minDistance &&
      rightDistance >= minDistance) {
    autoDriveMode = 2;
  }
  if (leftDistance >= minDistance &&
      middleDistance < minDistance &&
      rightDistance >= minDistance) {
    autoDriveMode = 4;
  }
  if (leftDistance >= minDistance &&
      middleDistance >= minDistance &&
      rightDistance < minDistance) {
    autoDriveMode = 3;
  }
  if (leftDistance < minDistance &&
      middleDistance < minDistance &&
      rightDistance < minDistance) {
    autoDriveMode = 4;
  }

  Serial.print("autoDrive =>");
  Serial.print("  left: ");
  Serial.print(lastLeftSharpValue);
  Serial.print(" - ");
  Serial.print(leftDistance);
  Serial.print("  middle: ");
  Serial.print(lastMiddleSharpValue);
  Serial.print(" - ");
  Serial.print(middleDistance);
  Serial.print("  right: ");
  Serial.print(lastRightSharpValue);
  Serial.print(" - ");
  Serial.print(rightDistance);
  Serial.print("  mode: ");
  Serial.println(autoDriveMode);
  delay(100);

  switch (autoDriveMode) {
    case 1:
      motorForward();
      break;
    case 2:
      motorStop();
      motorTurnRight();
      break;
    case 3:
      motorStop();
      motorTurnLeft();
      break;
    case 4:
      motorStop();
      motorReverse();
      delay(500);
      motorStop();
      rnd = random(0, 2);
      switch (rnd) {
        case 0:
          motorTurnLeft();
          break;
        case 1:
          motorTurnRight();
          break;
      }
      delay(100);
    default:
      motorStop();
      break;
  }
}

void analog() {
  readAnalogSensors();
  Serial.print("Left Sharp: ");
  Serial.print(lastLeftSharpValue);
  Serial.print(",");
  Serial.println(longSharp_AnalogToCm(lastLeftSharpValue));
  Serial.print("Middle Sharp: ");
  Serial.print(lastMiddleSharpValue);
  Serial.print(",");
  Serial.println(longSharp_AnalogToCm(lastMiddleSharpValue));
  Serial.print("Right Sharp: ");
  Serial.print(lastRightSharpValue);
  Serial.print(",");
  Serial.println(longSharp_AnalogToCm(lastRightSharpValue));
  Serial.print("Back Sharp: ");
  Serial.print(lastBackSharpValue);
  Serial.print(",");
  Serial.println(longSharp_AnalogToCm(lastBackSharpValue));
  Serial.print("Sonar: ");
  Serial.print(lastSonarValue);
  Serial.print(",");
  Serial.println(sonar.convert_cm(lastSonarValue));
}

void readAnalogSensors() {
  lastLeftSharpValue = analogRead(pinLeftSharp);
  lastMiddleSharpValue = analogRead(pinMiddleSharp);
  lastRightSharpValue = analogRead(pinRightSharp);
  lastBackSharpValue = analogRead(pinBackSharp);
  lastSonarValue = sonar.ping_median();
}

/*
void testHeadSweep() {
  // set speed to 0 and it stops sweeping
  // or set rot to 0 and speed to 0 and it centers
  // sweep_speed should be about 0.01 for a start
  if (headLRSweepEnabled == 0) {
    headLRSweepEnabled = 1;
    headLRTimer = 0;
    headLRSpeed = headLRSweepSpeed;
    headLRRange = 10;
    Serial.println("Sweep Start");
  } else {
    headLRSweepEnabled = 0;
    headLRSpeed = 0;
    headLRRange = 0;
    Serial.println("Sweep Stop");
  }
}
*/

double longSharp_AnalogToCm(int valueToConvert) {
  double valueVoltage;
  double result;

  valueVoltage = (valueToConvert / 1023.0) * Vcc;
  result = 65 * pow(valueVoltage, -1.10);
  return result;
}

long readVcc() {
  // Read 1.1V reference against AVcc
  // set the reference to Vcc and the measurement to the internal 1.1V reference
#if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  ADMUX = _BV(REFS0) | _BV(MUX4) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
#elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__)
  ADMUX = _BV(MUX5) | _BV(MUX0);
#elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
  ADMUX = _BV(MUX3) | _BV(MUX2);
#else
  ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
#endif

  delay(2); // Wait for Vref to settle
  ADCSRA |= _BV(ADSC); // Start conversion
  while (bit_is_set(ADCSRA, ADSC)); // measuring

  int low  = ADCL; // must read ADCL first - it then locks ADCH
  int high = ADCH; // unlocks both

  long result = (high<<8) | low;

  result = 1125300L / result; // Calculate Vcc (in mV); 1125300 = 1.1*1023*1000
  return result; // Vcc in millivolts
}