/*  RobotCar3SensorsPWMTB6612FNGv2.ino

     Mod to change turns, turns too much in v1

     This car uses 3 ultrasonic sensors
     Motor controller is TB6612FNG
     PWM to control speed

  This worked but seemed to have weird issues with the sensors not registering correctly...PWM pulsen() conflict?
    Issue fixed by having seperate battery for the arduino
    This version uses all the main usable pins

    Pins
    2   Echo right
    3   trigger right
    4   Echo centre
    5   PWM left
    6   PWM right
    7   Trigger centre
    8   rightForwardPin
    9   rightReversePin
    10  leftForwardPin
    11  leftReversePin
    12  Echo left
    13  Trigger left

*/
//TB6612FNG pins
/*
  const int leftForwardPin = 10;
  const int leftReversePin = 11;
  const int rightForwardPin = 9;
  const int rightReversePin = 8;
  const int rightPWMPin = 5;
  const int leftPWMPin = 6;
*/

//reconfigured after reversing bottom plate due to crash
const int leftForwardPin = 11;
const int leftReversePin = 10;
const int rightForwardPin = 8;
const int rightReversePin = 9;
const int rightPWMPin = 6;
const int leftPWMPin = 5;

//HCSR04 Ultrasonic sensors
const int triggerLeft = 13;
const int echoLeft = 12;
const int triggerCentre = 7;
const int echoCentre = 4;
const int triggerRight = 3;
const int echoRight = 2;

//set drive condition
int pwmSpeed; //0 = stop 255 is full ahead
int leftSpeed;
int rightSpeed;
int pwmAdjuster; //stores direction differential
unsigned long speedTimer;
//canning sensors
int scanDistances[3];//holds the distances scanned 0 = left, 1 = centre 2 = right
int scanDirection = 0;//0 = 0, 1, 2  1 = 2,1,0
int scanPosition = 1;//hold the step scans have got to
//speed of sound is 343m/s = 0.003 seconds per metre
//for sound to travel 20 meters = 0.06 seconds...safe gap between triggering pulses = 60 milliseconds
const int scanGap = 100;//changed to 120, seemed to stop odd readings
unsigned long scanTime;
int goBackwards;

void doScans() {
  int getDistance;
  if (millis() > scanTime) {
    switch (scanPosition) {
      case 0:
        getDistance = getDistanceHCSSR04(triggerLeft, echoLeft);
        if (getDistance > 1000) {
          getDistance = 1000;
        }
        if (getDistance < 10000) {
          scanDistances[scanPosition] = getDistance;
        }
        break;
      case 1:
        getDistance = getDistanceHCSSR04(triggerCentre, echoCentre);
        if (getDistance > 1000) {
          getDistance = 1000;
        }
        if (getDistance < 10000) {
          scanDistances[scanPosition] = getDistance;
        }
        break;
      case 2:
        getDistance = getDistanceHCSSR04(triggerRight, echoRight);
        if (getDistance > 1000) {
          getDistance = 1000;
        }
        if (getDistance < 10000) {
          scanDistances[scanPosition] = getDistance;
        }
        break;
      default:
        Serial.println("scan error default");
        break;
    }
    if (scanDirection < 1) { //if scanning left to right
      scanPosition++;
      if (scanPosition > 1) {
        scanDirection = 1;//change the scanning direction
      }
    } else { //scanning right to left
      scanPosition--;
      if (scanPosition < 1) {
        scanDirection = 0;//change the scanning direction
      }
    }


    scanTime = millis() + scanGap;
  }
}

//Distance function from HCSR04UltrasonicDistanceSensorv2Function.ino
//It receives the pins of the sensor and send back the distance
int getDistanceHCSSR04(int funcTriggerPin, int funcEchoPin) {
  //An unsigned long is needed because the numbers produced can be much larger
  //than an int can hold (max value 32,767)
  //Local variables
  unsigned long duration;  //time taken for echo
  int myDistance;
  //make sure Trigger is low to start
  digitalWrite(funcTriggerPin, LOW);
  //A tiny delay 2 millionths of a second
  delayMicroseconds(2);
  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  digitalWrite(funcTriggerPin, HIGH);
  //pin stays HIGH (5v) for 10 microseconds to trigger a pulse
  delayMicroseconds(10);
  //Pin taken LOW (0v) after pulse triggered ready for next pulse
  digitalWrite(funcTriggerPin, LOW);
  duration = pulseIn(funcEchoPin, HIGH);
  myDistance = duration * 0.34 / 2;
  return myDistance;
}


void setup() {
  Serial.begin(9600);
  Serial.println("RobotCar3SensorsPWMTB6612FNGv2");
  //Motor driver pins
  pinMode(leftForwardPin, OUTPUT);
  pinMode(leftReversePin, OUTPUT);
  pinMode(rightForwardPin, OUTPUT);
  pinMode(rightReversePin, OUTPUT);
  pinMode(rightPWMPin, OUTPUT);
  pinMode(leftPWMPin, OUTPUT);

  digitalWrite(leftForwardPin, HIGH);
  digitalWrite(leftReversePin, LOW);
  digitalWrite(rightForwardPin, HIGH);
  digitalWrite(rightReversePin, LOW);

  //ultrasonic pins
  pinMode(triggerLeft, OUTPUT);
  pinMode(echoLeft, INPUT);
  pinMode(triggerCentre, OUTPUT);
  pinMode(echoCentre, INPUT);
  pinMode(triggerRight, OUTPUT);
  pinMode(echoRight, INPUT);
  //allow 3 sec to get initial readings and to let go of car
  speedTimer = millis() + 3000;
}

unsigned long testTimer;


void loop() {
  doScans();
  if (millis() > testTimer) {
    Serial.print(scanDistances[0]);
    Serial.print(" : ");
    Serial.print(scanDistances[1]);
    Serial.print(" : ");
    Serial.println(scanDistances[2]);

    Serial.print("pwmSpeed: ");
    Serial.println(pwmSpeed);
    Serial.print("leftSpeed: ");
    Serial.println(leftSpeed);
    Serial.print("rightSpeed: ");
    Serial.println(rightSpeed);
    testTimer = millis() + 500;
  }
  if (millis() > speedTimer) {
    //speedTimer = millis() + 3;//larger number slows direction changes
    speedTimer = millis() + 6;//larger number slows direction changes
    pwmSpeed = scanDistances[1] - 255;//This should cause car to decelerate below 500mm
    if (pwmSpeed > 255) {
      pwmSpeed = 255;//above 500 mm go flat out
    }
    if (scanDistances[0] < 200 && scanDistances[1] < 200 && scanDistances[2] < 200) { //stuck
      goBackwards = 1; //try reverse spin
    }
    //v2 mod
    //if (scanDistances[0] < 800 || scanDistances[2] < 800) {
    if (scanDistances[0] < 500 || scanDistances[2] < 500) {//don't react until closer

      if (scanDistances[0] > scanDistances[2]) {
        pwmAdjuster++;
      } else {
        pwmAdjuster--;
      }
    } else {
      pwmAdjuster = 0;
    }



    if (pwmAdjuster > 200) {
      pwmAdjuster = 200;
    }
    if (pwmAdjuster < -200) {
      pwmAdjuster = -200;
    }




    leftSpeed = pwmSpeed + pwmAdjuster;
    rightSpeed = pwmSpeed - pwmAdjuster;

    if (leftSpeed > 255) {
      leftSpeed = 255;
    }
    if (rightSpeed > 255) {
      rightSpeed = 255;
    }
    if (leftSpeed < 120) {
      leftSpeed = 0;
    }
    if (rightSpeed < 120) {
      rightSpeed = 0;
    }

    if (leftSpeed == 0 && rightSpeed == 0) {
      goBackwards = 1;
    }
    if (goBackwards < 1) {//forwards
      digitalWrite(leftForwardPin, HIGH);
      digitalWrite(leftReversePin, LOW);
      digitalWrite(rightForwardPin, HIGH);
      digitalWrite(rightReversePin, LOW);
      analogWrite(leftPWMPin,  leftSpeed);
      analogWrite(rightPWMPin, rightSpeed);
    } else {//stuck manouvere
      digitalWrite(leftForwardPin, LOW);
      digitalWrite(leftReversePin, HIGH);
      digitalWrite(rightForwardPin, LOW);
      digitalWrite(rightReversePin, HIGH);
      analogWrite(leftPWMPin,  200);
      analogWrite(rightPWMPin, 200);
      delay(200); //reverse
      digitalWrite(leftForwardPin, HIGH);
      digitalWrite(leftReversePin, LOW);
      delay(800); //spin
      analogWrite(leftPWMPin,  0);
      analogWrite(rightPWMPin, 0);
      goBackwards = 0;
      speedTimer = millis() + 1000;
    }
  }
}