/*
  Test of Arduino control of Cybot sonar board

  Author Tony Wills
  Licensed CC-BY-SA

  20180211 Initial release version 1.00
  20180221 1.061
  see http://www.projects.scorchingbay.nz/dokuwiki/arduino/Controller_for_Cybot (or an archive.org copy) for more information
*/

#include <LiquidCrystal.h>
// initialize the library by associating any needed LCD interface pin
// with the arduino pin number it is connected to
const int rs = 12, en = A1, d4 = A2, d5 = A3, d6 = A4, d7 = A5;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

const byte interruptPin =  2;         // interrupt from sonar echo return
const byte stopPin =       3;         // interrupt from push button switch to halt cybot
const byte enLeftPin =     4;         // enable left side sonar return
const byte enRightPin =    5;         // enable right side sonar return
const byte outPin1 =       6;         // ultrasonic driver1
const byte outPin2 =       7;         // ultrasonic driver1'
const byte m2RRPin =       8;         // motor 2, right side, reverse
const byte m2RFPin =       9;         // motor 2, right side, forward
const byte m1LRPin =      10;         // motor 1, left side,  reverse
const byte m1LFPin =      11;         // motor 1, left side,  forward

const byte ledPin =       13;         // arduino LED
const byte batteryPin =   A0;         // motor battery voltage


int lastAction;                       // last manoeuvre performed
const byte actionForward =  0;        //    traveling forward
const byte actionLeft =     1;        //    avoided object on left by turning right
const byte actionRight =    2;        //    avoided object on right by turning left
const byte actionSpin =     3;        //    spun (one wheel forward, other back) left or right
const byte actionBack =     4;        //    backed up and chose another direction
const byte actionWheelly =  5;        //    stop then full speed ahead
const byte actionChange =   6;        //    random change after long forward
unsigned long actionStart;            // time last manoeuvre began
unsigned long runStart;               // time the last actionFoward manoeuvre began

const int LEFT =  0;                  // select left side sonar
const int RIGHT = 1;                  // select right side sonar
int side;                             // left or right side sonar receiver
const byte pingInterval =   10;       // number of milliseconds between sending pings
//   can't be too short or will still be getting echos from the last ping
unsigned long startTime;              // time in micros() of ping firing
volatile byte awaitingPing;           // we haven't received echo from latest ping yet
unsigned long lastPing;               // time in millis() of ping firing
unsigned long lastSpeedCheck;         // time in millis() of last doppler measure
const byte sonarLog = 100;            // size of sonar echo log
byte sonarIdx;                        // index into circular buffer sonar echo log
volatile unsigned long duration[2];   // duration in micros() of latest ping echo
volatile unsigned long oldDuration[2][sonarLog];  // circular buffer of previous echo times
volatile byte actionLog[sonarLog];    // circular buffer of past manoeuvres
const int followDistance =   600;     // distance at which to follow a wall
const int avoidDistance =    700;     // distance at which to take evasive action
const int reverseDistance = 1200;     // distance at which to back out and try a different path

volatile byte stopNow;                // stop push button has been pressed

volatile int wavePeriod;              // period in micros() of last echo cycles received
int batteryNoLoad;                    // motor battery voltage measured without motors running
int batteryVolts;                     // last measured motor battery voltage

byte runSpeed;                        // speed for actionForward travel

/*****************************************************************************************/

void setup() {

  pinMode(batteryPin, INPUT);         // analog input to read motor battery voltage
  pinMode(ledPin, OUTPUT);            // arduino LED or Cybot antennas
  pinMode(outPin1, OUTPUT);           // sonar driver high
  pinMode(outPin2, OUTPUT);           // sonar driver low
  pinMode(enLeftPin, OUTPUT);         // select-left sonar receiver (float high to enable)
  pinMode(enRightPin, OUTPUT);        // select-right sonar receiver
  digitalWrite(enLeftPin, 0);         // disable both sonar receivers
  digitalWrite(enRightPin, 0);
  pinMode(interruptPin, INPUT_PULLUP);// sonar echo receiver
  pinMode(stopPin, INPUT_PULLUP);     // push button stop signal
  pinMode(m1LFPin, OUTPUT);           // "m1" motor1 Left Forward PWM
  pinMode(m1LRPin, OUTPUT);           // "m2" motor1 Left Reverse digital
  pinMode(m2RFPin, OUTPUT);           // "m3" motor2 Right Forward PWM
  pinMode(m2RRPin, OUTPUT);           // "m4" motor2 Right Reverse digital
  analogWrite(m1LFPin, 0);            // stop all motors
  digitalWrite(m1LRPin, LOW);
  analogWrite(m2RFPin, 0);
  digitalWrite(m2RRPin, LOW);

  lcd.begin(16, 2);                   // inialise LCD
  lcd.clear();
  lcd.print("Cybot");

  lastAction = 255;
  actionStart = 0;
  runStart = 0;

  duration[LEFT] = 88888;
  duration[RIGHT] = 88888;

  // initialise log with distinctive values
  for (int i = 0; i < sonarLog; i++) {
    oldDuration[LEFT][i] = 99999;
    oldDuration[RIGHT][i] = 99999;
    actionLog[i] = 255;
  }
  sonarIdx = 0;

  // do initial sonar ranging
  side = LEFT;
  awaitingPing = 0;
  startTime = micros();
  sonarCheck();
  interrupts();
  lastPing = millis();
  lastSpeedCheck = millis();
  delay(10);
  startTime = micros();
  sonarCheck();
  lastPing = millis();
  delay(10);

  runSpeed = 127;
  stopNow = 0;

  batteryVolts = analogRead(batteryPin);
  batteryNoLoad = batteryVolts;

  lcd.setCursor(5, 1);
  lcd.print(duration[LEFT]); lcd.print("  ");
  lcd.setCursor(11, 1);
  lcd.print(duration[RIGHT]); lcd.print("  ");
  lcd.setCursor(0, 1);
  lcd.print(batteryVolts); lcd.print("  ");
  // ready to go
  flashAntenna(100, 10);
}

/*****************************************************************************************/

void loop() {

  digitalWrite(ledPin, HIGH);

  if ((millis() - lastPing) > pingInterval) {  //every 10mS or so
    if ((millis() - lastSpeedCheck) > 200) {
      speedCheck();
      lastSpeedCheck = millis();
    } else {
      sonarCheck();
      lastPing = millis();
    }
  }

  if ( (batteryVolts < (0.60 * batteryNoLoad)) ) { // || (wavePeriod > 100)) {
    // excessive load on batteries or wildly varying pings, probably stalled
    lcd.setCursor(6, 0);
    lcd.print("STALL     ");
    // all stop
    driveForward(0);
    flashAntenna(50, 5);
    lcd.setCursor(0, 1);
    lcd.print(batteryVolts); lcd.print("    ");
    myDelay(500);
    driveBack(250);
    driveSpin( RIGHT, 250 );
    lastAction = actionBack;
    runSpeed = 100;
    runStart = millis();
  }

  if ((millis() - runStart) > 10000) {
    // seem to have been running forward for a long time, try something else
    lcd.setCursor(6, 0);
    lcd.print("change    ");
    // all stop
    driveForward(0);
    myDelay(2000);
    flashAntenna(100, 5);
    driveBack(250);
    driveSpin( LEFT, random(100, 500));
    myDelay(500);
    lastAction = actionChange;
    runSpeed = 100;
    runStart = millis();
  }


  if (((duration[LEFT] < reverseDistance) && (duration[RIGHT] < reverseDistance)) ) { //&& (lastAction != actionSpin)) {
    lcd.setCursor(6, 0);
    lcd.print("backup    ");
    actionStart = millis();
    lastAction = actionSpin;
    digitalWrite(ledPin, LOW);
    // backup then rotate 180 degrees
    driveBack( 250 );

    if (duration[LEFT] < duration[RIGHT]) {
      //turn right 180 degrees
      driveSpin( RIGHT, 250 );
      while (duration[LEFT] < reverseDistance) {
        driveSpin( RIGHT, 25 );
        if ((millis() - actionStart) > 300 ) break;
      }
    } else {
      // rotate other way
      driveSpin(LEFT, 250);
      while (duration[RIGHT] < reverseDistance) {
        driveSpin( LEFT, 25 );
        if ((millis() - actionStart) > 300 ) break;
      }
    }
    runSpeed = 127;
    runStart = millis();
  }

  if (duration[RIGHT] < (avoidDistance + runSpeed)) {
    actionStart = millis();
    lcd.setCursor(6, 0);
    lcd.print("^right    ");
    lastAction = actionRight;
    runSpeed = 127;
    while (duration[RIGHT] < avoidDistance) {
      // keep turning left until away from object to right
      driveLeft(50);
      if ((millis() - actionStart) > 900 ) {
        // if we've been turning away for too long, maybe we are stuck
        driveForward(0);
        lcd.setCursor(6, 0);
        lcd.print("rStall    ");
        myDelay(500);
        driveBack(250);
        driveSpin(LEFT, 100);
        break;
      }
    }
    runStart = millis();
  }


  if (duration[LEFT] < (avoidDistance + runSpeed)) {
    actionStart = millis();
    lcd.setCursor(6, 0);
    lcd.print("^left     ");
    lastAction = actionLeft;
    runSpeed = 127;
    while (duration[LEFT] < avoidDistance) {
      // keep turning right until away from object to left
      driveRight(50);
      if ((millis() - actionStart) > 900 ) {
        // if we've been turning away for too long, maybe we are stuck
        driveForward(0);
        lcd.setCursor(6, 0);
        lcd.print("lStall    ");
        myDelay(500);
        driveBack(250);
        driveSpin(RIGHT, 100);
        break;
      }
    }
    runStart = millis();
  }

  if ((duration[LEFT] > 9999) && (duration[RIGHT] > 9999) ) {
    lcd.setCursor(6, 0);
    lcd.print("free      ");
    runSpeed = 255;
    lastAction = actionWheelly;
    driveForward( 0 );
    runStart = millis();
  }

  lcd.setCursor(13, 0);
  lcd.print(runSpeed); lcd.print(" ");
  driveForward( runSpeed );   // forward both
  myDelay(100);
  lastAction = actionForward;
}

/*****************************************************************************************/

void ping(int cycles, int wavelength) {
  // the transducer appears as a capacitively coupled device so that we can drive one side high and the other side low at the same time
  // to effectively get 10V peak-to-peak across it
  // originally used digitalWrite() but this was slow and only needed pulse delays of 5 & 6 uS to get about 40kHz output
  // but because of the slow execution of digitalWrite there was a significant difference in when one pin goes high and the other low
  // so changed to writing to the port directly so that we can drive them in sync
  int i;
  byte saveDDR;
  noInterrupts();
  saveDDR = DDRD;                     // save I/O designation of bits on this port
  DDRD = B11000000;                   // don't want to write data to any other bits than 6 & 7
  startTime = micros();
  for (i = 0; i < cycles; i++) { //min period with 0 delays is 15.6uS
    //do one cycle on 2 pins, each the inverse of the other
    PORTD = B10000000;
    delayMicroseconds(wavelength / 2);
    PORTD = B01000000;
    delayMicroseconds(wavelength / 2);
  }
  PORTD = B00000000;
  DDRD = saveDDR;
  interrupts();

}

void echoReceived() {
  // interrupt routine for output of sonar board to measure distance
  detachInterrupt(digitalPinToInterrupt(interruptPin));  // just want first echo
  oldDuration[side][sonarIdx] = duration[side];
  duration[side] = micros() - startTime;
  actionLog[sonarIdx] = lastAction;
  awaitingPing = 0;
}


void echoDoppler() {
  // interrupt routine for output of sonar board to measure speed
  byte saveDDR;
  wavePeriod = 0;                     // period of received echos over multiple cycles
  saveDDR = DDRD;                     // save I/O designation of bits on this port
  DDRD = B00000000;                   // don't want to write data

  detachInterrupt(digitalPinToInterrupt(interruptPin));  // don't need anymore echo interrupts
  while ( digitalRead(interruptPin) == 0) {
    // wait for end of current wave cycle in echo signal
  }
  // read register directly for maximum resolution
  for (int i = 0; i < 3; i++) {
    // measure period of recieved echos over multiple cycles
    while ( (PIND & B00000100) == B00000100 ) {
      wavePeriod++;
    }
    while ( (PIND & B00000100) == B00000000 ) {
      wavePeriod++;
    }
  }
  DDRD = saveDDR;                     // restore direction register
  awaitingPing = 0;
}

void sonarCheck() {
  // check for obstructions in range
  detachInterrupt(digitalPinToInterrupt(interruptPin));  // too late to receive echo now
  if (awaitingPing) {                 // no echo received before this new ping request
    oldDuration[side][sonarIdx] = duration[side];  // as though we received the same ping as last time
    actionLog[sonarIdx] = 254;
  }

  if (side == LEFT) {
    side = RIGHT;
  } else {
    side = LEFT;
  }
  // set level then enable as output so that we don't get stray high pulses on output as it goes from floating to low
  digitalWrite(enLeftPin, LOW);       // disable left comparator
  pinMode(enLeftPin, OUTPUT);
  digitalWrite(enLeftPin, LOW);
  digitalWrite(enRightPin, LOW);      // disable right
  pinMode(enRightPin, OUTPUT);
  digitalWrite(enRightPin, LOW);
  ping( 8, 26 );                      // pulse 8 cycles at 40kHz approx
  awaitingPing = 1;
  // select left/right sonar receiver (float high)
  if (side == LEFT ) {
    pinMode(enLeftPin, INPUT);
    sonarIdx++;
    if (sonarIdx >= sonarLog) sonarIdx = 0;
  } else {
    pinMode(enRightPin, INPUT);
  }

  // enable interrupt routine which will set echo response time as "duration[side]"
  attachInterrupt(digitalPinToInterrupt(interruptPin), echoReceived, LOW);
  attachInterrupt(digitalPinToInterrupt(stopPin), stopReceived, LOW);
}

void speedCheck() {
  // use doppler shift to check whether we're moving
  detachInterrupt(digitalPinToInterrupt(interruptPin));  // kill off any pending ping reception
  digitalWrite(enLeftPin, LOW);       // disable left comparator
  pinMode(enLeftPin, OUTPUT);
  digitalWrite(enLeftPin, LOW);
  digitalWrite(enRightPin, LOW);      // disable right
  pinMode(enRightPin, OUTPUT);
  digitalWrite(enRightPin, LOW);
  ping( 8, 26 );                      // pulse 8 cycles at 40kHz approx
  awaitingPing = 1;
  // enable whichever receiver is current sonar side
  if (side == LEFT ) {
    pinMode(enLeftPin, INPUT);
  } else {
    pinMode(enRightPin, INPUT);
  }
  attachInterrupt(digitalPinToInterrupt(interruptPin), echoDoppler, LOW);
}

void myDelay(unsigned long period ) {
  unsigned long start;
  start = millis();
  while ((millis() - start) < period ) {  //delay(period);

    if ((millis() - lastPing) > pingInterval) {  //every 10mS or so
      if ((millis() - lastSpeedCheck) > 200) {
        speedCheck();
        lastSpeedCheck = millis();
      } else {
        sonarCheck();
        lastPing = millis();
      }
      batteryVolts = analogRead(batteryPin);
    }

    lcd.setCursor(5, 1);
    lcd.print(duration[LEFT]); lcd.print("  ");
    lcd.setCursor(11, 1);
    lcd.print(duration[RIGHT]); lcd.print("  ");
  }
  lcd.setCursor(0, 1);
  lcd.print(batteryVolts); lcd.print("  ");
}

void driveBack(unsigned long howLong) {
  analogWrite(m1LFPin, 0);            // make sure not setting forward & reverse simultaneously
  analogWrite(m2RFPin, 0);            // make sure not setting forward & reverse simultaneously
  digitalWrite(m1LRPin, HIGH );       // back both full
  digitalWrite(m2RRPin, HIGH );
  myDelay( howLong );
  digitalWrite(m1LRPin, LOW );        // stop
  digitalWrite(m2RRPin, LOW );
}

void driveRight(unsigned long howLong) {
  digitalWrite(m1LRPin, LOW );       // make sure not setting forward & reverse simultaneously
  digitalWrite(m2RRPin, LOW );
  analogWrite(m1LFPin, 255);
  analogWrite(m2RFPin, 0);
  myDelay(howLong);
}

void driveLeft(unsigned long howLong) {
  digitalWrite(m1LRPin, LOW );
  digitalWrite(m2RRPin, LOW );       // make sure not setting forward & reverse simultaneously
  analogWrite(m2RFPin, 255);
  analogWrite(m1LFPin, 0);
  myDelay(howLong);
}

void driveForward(int howFast) {
  digitalWrite(m1LRPin, LOW );       // make sure not setting forward & reverse simultaneously
  digitalWrite(m2RRPin, LOW );       // make sure not setting forward & reverse simultaneously
  analogWrite(m1LFPin, howFast);
  analogWrite(m2RFPin, howFast);

}

void driveSpin(byte direction, unsigned long howLong) {
  if (direction == RIGHT) {
    //spin right
    analogWrite(m2RFPin, 0);
    digitalWrite(m1LRPin, LOW );     // make sure not setting forward & reverse simultaneously
    analogWrite(m1LFPin, 255);
    digitalWrite(m2RRPin, HIGH);
    myDelay(howLong);
    digitalWrite(m2RRPin, LOW);
  } else {
    // spin other way
    analogWrite(m1LFPin, 0);
    digitalWrite(m1LRPin, HIGH);
    digitalWrite(m2RRPin, LOW );     // make sure not setting forward & reverse simultaneously
    analogWrite(m2RFPin, 255);
    myDelay(howLong);
    digitalWrite(m1LRPin, LOW);
  }
}

void stopReceived() {
  // interrupt routine for stop button
  detachInterrupt(digitalPinToInterrupt(stopPin));  // just want first press
  stopNow = 1;
}

void flashAntenna(int period, int cycles) {
  for (int i = 0; i < cycles; i++) {
    digitalWrite(ledPin, LOW);
    delay(period);
    digitalWrite(ledPin, HIGH);
    delay(period);
  }
}

void dumpLog() {
  byte i;
  i = ((sonarIdx + 1) % sonarLog);
  Serial.println(); Serial.println(); Serial.println(); Serial.println(); Serial.println();
  Serial.println(sonarIdx);
  do {
    Serial.print(i); Serial.print("\t"); Serial.print(oldDuration[LEFT][i]); Serial.print("\t"); Serial.print(oldDuration[RIGHT][i]); Serial.print("\t"); Serial.println(actionLog[i]);
    i = (i + 1) % sonarLog;
  } while (!(i == (sonarIdx)));
  Serial.print(i); Serial.print("\t"); Serial.print(oldDuration[LEFT][i]); Serial.print("\t"); Serial.print(oldDuration[RIGHT][i]); Serial.print("\t"); Serial.println(actionLog[i]);
}