/*
* Test of Arduino control of Cybot sonar board
* 
* Author Tony Wills
* Licensed CC-BY-SA
*
* 20180211 Initial release version 1.00
* see http://www.arduino.scorchingbay.nz (or an archive.org copy) for more information
*/

const byte outPin1 = 6; 
const byte outPin2 = 7;
const byte enLeftPin= 4;
const byte enRightPin = 5;
const byte interruptPin = 2;
unsigned long startTime;
volatile unsigned long duration;
long leftDistance;
long rightDistance;

void setup() {
  Serial.begin(9600);
  Serial.print("\eSP F");                                     // tell to use 7-bit control codes
  Serial.print("\e[?25l");                                    // hide cursor
  Serial.print("\e[?12l");                                    // disable cursor highlighting
  Serial.print("\e[2J");
  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
  digitalWrite(enRightPin, 0);
  pinMode(interruptPin, INPUT_PULLUP);
  startTime = 0;
  duration = 0;
  leftDistance = 0;
  rightDistance = 0;
//  attachInterrupt(digitalPinToInterrupt(interruptPin), echoReceived, FALLING);
}

void loop() { 
  //check left

  // set level then enable as output so that we down't get stray high pulses on output as it goes form floating to low
  digitalWrite(enLeftPin, LOW);                               // Disable left
  pinMode(enLeftPin, OUTPUT);
  digitalWrite(enRightPin, LOW);                              // Disable right
  pinMode(enRightPin, OUTPUT);
  ping( 8, 26 );                                              // pulse 8 cycles at 40kHz aprox (26uS period as can't delayMicroseconds(12.5uS) half cycles)
                                                              //   receivers not very sensitive at 24uS, work fine at 26uS

  pinMode(enLeftPin, INPUT);                                  // select left sonar receiver (float high)

  // enable interupt routine which will set echo response time as "duration"
  attachInterrupt(digitalPinToInterrupt(interruptPin), echoReceived, LOW);
 
  delay(10);                                                  // wait for echo, instead this could be other processing for controlling bot
  
  if (duration > 0 ) {
    leftDistance = ((duration)*343) / 20000;                   // distance in cm, approximating speed of sound as 343m/S
  } else {
    leftDistance = 0;
  }

  digitalWrite(enLeftPin, LOW);                                // disable it
  pinMode(enLeftPin, OUTPUT);                                  // select left sonar receiver

  // for now just display distances on ANSI serial terminal
  Serial.print("\e[1;1H");Serial.print(duration);Serial.print("     ");
  if (leftDistance > 0 ) {
    Serial.print("\e[5;1HLeft : ");
    Serial.print( leftDistance-8 );                            // cludge factor to accomadate individual sensor
    Serial.println("cm    ");
  }
  
  // check right
  digitalWrite(enLeftPin, LOW);                                // Disable left
  pinMode(enLeftPin, OUTPUT);
  digitalWrite(enRightPin, LOW);                               // Disable right
  pinMode(enRightPin, OUTPUT);
  ping( 8, 26 );                                               // pulse 8 cycles at 40kHz aprox

  pinMode(enRightPin, INPUT);                                  // select right sonar receiver (float high)

  // enable interupt routine which will set echo response time as "duration"
  attachInterrupt(digitalPinToInterrupt(interruptPin), echoReceived, LOW);
 
  delay(10);
  
  if (duration > 0 ) {
    rightDistance = ((duration)*343) / 20000;
  } else {
    rightDistance = 0;
  }

  digitalWrite(enRightPin, LOW);                               // disable it
  pinMode(enRightPin, OUTPUT);                                 // select right sonar receiver

  // for now just display distances on ANSI serial terminal
  Serial.print("\e[1;1H");Serial.print(duration);Serial.print("     ");
  if (rightDistance > 0 ) {
    Serial.print("\e[5;16HRight : ");
    Serial.print( rightDistance-6 );                            // cludge factor to accomadate individual sensor
    Serial.println("cm    ");
  }

}

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;
  int 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
  duration = 0;                       //reset echo measuring
  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;
//    digitalWrite(outPin1, HIGH);   // pulse on
//    digitalWrite(outPin2, LOW);    // pulse on inverted
//    delayMicroseconds(6);
    delayMicroseconds(wavelength/2);
    PORTD= B01000000;
//    digitalWrite(outPin1, LOW);    // pulse off
//    digitalWrite(outPin2, HIGH);   // pulse off inverted
//    delayMicroseconds(5);      
    delayMicroseconds(wavelength/2);      
  }
  DDRD = saveDDR;
//  interrupts();

}

void echoReceived() {
  // interupt routine for output of sonar board
  duration = micros() - startTime;
  detachInterrupt(digitalPinToInterrupt(interruptPin));       // just want first echo
}