// communications

#include <SoftwareSerial.h>
SoftwareSerial mySerial(9, 10); // RX, TX

// definitions for the distance sensor

// defines pins numbers
const int trigPin = 6;    // MOSI - ISP Pin2
const int echoPin = 5;    // MISO - ISP Pin4
const int flashPin = 7;

// defines variables
long duration;
int distance;

// subroutines

void flashLEDs(int i,int j) {
  i = abs(i-1);
  j = abs(j-1);
  delay(100);
  digitalWrite(7,HIGH); digitalWrite(8,HIGH);
  delay(100);
  digitalWrite(7,i); digitalWrite(8,j);
}

void commandLeg(String commandString) {
    flashLEDs(0,1);
    mySerial.println(commandString);
    flashLEDs(1,0);
    delay(100);
}


int readSensor() {
  // Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance= duration*0.034/2;
  // Prints the distance on the Serial Monitor
  if (distance < 10) {
    digitalWrite(flashPin,HIGH);
    return 1;
  } else {
    digitalWrite(flashPin,LOW);
    return 0;
  } 
}

void setup() {
  // set the data rate for the SoftwareSerial port
  mySerial.begin(1200);
  pinMode(flashPin,OUTPUT);
  pinMode(echoPin,INPUT);
  pinMode(trigPin,OUTPUT);

  String commandString = "N8 speak 007";
  commandLeg(commandString);
}

void loop() { // run over and over

  int obstacle = readSensor();
//  int obstacle = 0;
   
  int leg=0;
  String commandString;

    commandString = "N7 heave 110";
    commandLeg(commandString);

    delay(1000);

    if (obstacle==1) {
      commandString = "n8 speak 005";
      commandLeg(commandString);
      delay(50);
    }
    
    if (obstacle==0) {
      commandStraing = "N8 speak 007";
      commandLeg(commandString);
      delay(50);
      commandString = "N7 sweep 80";
      commandLeg(commandString);
      delay(1000);
    }

    
for (int i=1;i<7;i++){
    commandString = 'N' + String(i) + " heave 160";
    commandLeg(commandString);
    delay(100);
    if (obstacle==0) {
      commandString = 'N' + String(i) + " sweep 100";
      commandLeg(commandString);
      delay(100);
    }
    commandString = 'N' + String(i) + " heave 120";
    commandLeg(commandString);
    delay(100);
    }

}