//Work in Progress Code

//Left Motor Controls - LMSpeed Range from 0-255, LMB and 3 HIGH = coast, LMB and 3 LOW = Brake,  LMB HIGH + LM 3 LOW = Backwards,  LMB LOW + LMF HIGH = Forwards

#define LMSpeed 5
#define LMB 13   
#define LMF 12

//Right Motor Controls - RMSpeed Range from 0-255, RMF and 1 HIGH = coast, RMF and 1 LOW = Brake,  RMF HIGH + RM 1 LOW = Backwards,  RMF LOW + RMB HIGH = Forwards

#define RMSpeed 6
#define RMB 10
#define RMF 11

//Sonar Controls - sends out a signal (trig) and times how long it takes to echo back - then used that number to calculate the dstance in cm

#define trig 0
#define echo 1

int distance;

               
void setup() {
  
  pinMode(LMB, OUTPUT);  
  pinMode(LMF, OUTPUT); 
  pinMode(RMF, OUTPUT);  
  pinMode(RMB, OUTPUT);
  pinMode(trig, OUTPUT);
}


void loop(){

ping();
   
   if (distance >= 25 || distance <= 0)
   {
    forward(100,100);
    coast(10);
    
   }
      
  else
  {
    
    backward(200,50);
    coast(10);
   
    rightTurn (200,100);
    coast(10);
  }
}


void ping() //functon used to calculate sonar distance in cm and create an integer called "distance" for other functions
{
  int duration;
  digitalWrite(trig, LOW);
  digitalWrite(trig, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
  distance = (duration/2) / 29.1;
}

void forward (int sp, int ms)
{
    analogWrite(LMSpeed, sp);
    digitalWrite(LMB, HIGH);
    digitalWrite(LMF, LOW);
    
    analogWrite(RMSpeed, sp);
    digitalWrite(RMF, HIGH);
    digitalWrite(RMB, LOW);

    delay(ms);

}

void backward (int sp, int ms)
{
    analogWrite(LMSpeed, sp);
    digitalWrite(LMB, LOW);
    digitalWrite(LMF, HIGH);
    
    analogWrite(RMSpeed, sp);
    digitalWrite(RMF, LOW);
    digitalWrite(RMB, HIGH);

    delay(ms);

}

void rightTurn (int sp, int ms)
{
    analogWrite(LMSpeed, sp);
    digitalWrite(LMB, HIGH);
    digitalWrite(LMF, LOW);
    
    analogWrite(RMSpeed, sp);
    digitalWrite(RMF, LOW);
    digitalWrite(RMB, HIGH);

    delay(ms);

}
  void brake (int ms)
{

    digitalWrite(LMB, LOW);
    digitalWrite(LMF, LOW);
    
    digitalWrite(RMF, LOW);
    digitalWrite(RMB, LOW);

    delay(ms);
    

}

  void coast (int ms)
{
    analogWrite(LMSpeed, 0);
    analogWrite(RMSpeed, 0);
    digitalWrite(LMB, HIGH);
    digitalWrite(LMF, HIGH);
    digitalWrite(RMF, HIGH);
    digitalWrite(RMB, HIGH);

    delay(ms);
    

}