Parallel parking rc car code needed!

Hi guys, help needed to create carry on the code for my parallel parking RC car project.

I have done the code for forward/reverse and left/right turn in the presence of an obstacle. Appreciate if anyone can help me to complete or provide guidance for the parallel parking part. I believe case coding is required but I'm not good in programming. Please help.

This is the code for moving away from obstacle.

const int pingPin = 7;
const int pingPin2 = 5;

void setup() {
    // set the switch as an input:
//    pinMode(switchPin, INPUT);
Serial.begin(9600);
//Dist.begin(A0);

    // set all the other pins you're using as outputs:


    // set enablePin high so that motor can turn on:

}

void loop() {
   long duration, cm;
   long duration2, cm2;
   
     // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  
   pinMode(pingPin2, OUTPUT);
  digitalWrite(pingPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin2, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin2, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  pinMode(pingPin2, INPUT);
  duration2 = pulseIn(pingPin2, HIGH);

  // convert the time into a distance
    cm = microsecondsToCentimeters(duration);
    cm2 = microsecondsToCentimeters(duration2);

  Serial.print(cm);
  Serial.print("cm");
  
  Serial.print("\t");    // prints a tab
  
  Serial.print(cm2);
  Serial.print("cm");
  Serial.println();
  delay(100);
  
  
}
long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

This is the code for multiple sensors (front, side and rear) which I have not integrate into the main code above:

// this constant won't change.  It's the pin number
// of the sensor's output:
const int pingPin1 = 2;
const int pingPin2 = 7;
const int pingPin3 = 8;
//const int pingPin4 = 10;

void setup() {
  // initialize serial communication:
  Serial.begin(9600);
}

void loop()
{
  // establish variables for duration of the ping, 
  // and the distance result in inches and centimeters:
  long duration1, cm;
  pinMode(pingPin1, OUTPUT);
  digitalWrite(pingPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin1, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin1, LOW);
  pinMode(pingPin1, INPUT);
  duration1 = pulseIn(pingPin1, HIGH);
    cm = microsecondsToCentimeters(duration1);
  Serial.print("A ");
  Serial.print(cm);
    Serial.print("\t");    // prints a tab
    
  long duration2, cm2;
  pinMode(pingPin2, OUTPUT);
  digitalWrite(pingPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin2, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin2, LOW);
  pinMode(pingPin2, INPUT);
  duration2 = pulseIn(pingPin2, HIGH);
    cm2 = microsecondsToCentimeters(duration2);
  Serial.print("B ");
  Serial.print(cm2);
    Serial.print("\t");    // prints a tab
  
  long duration3, cm3;
  pinMode(pingPin3, OUTPUT);
  digitalWrite(pingPin3, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin3, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin3, LOW);
  pinMode(pingPin3, INPUT);
  duration3 = pulseIn(pingPin3, HIGH);
    cm = microsecondsToCentimeters(duration3);
  Serial.print("C ");
  Serial.println(cm3);
   
 /* long duration4;
  pinMode(pingPin4, OUTPUT);
  digitalWrite(pingPin4, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin4, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin4, LOW);
  pinMode(pingPin4, INPUT);
  duration4 = pulseIn(pingPin4, HIGH);
  Serial.print("D ");
  Serial.println(duration4);
*/

 delay(10);
}
long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

I'm not good in programming.

Not great at following clearly-stated guidelines either.

Cross-post deleted.

I haven't studied the code you provided because I infer from your post (perhaps incorrectly) that you have those pieces of code working properly and I can't infer from your post what is the exact problem you want advice about.

You will have to tell us what you have tried, how it works and where it fall short of your requirement. If you haven't got as far as trying anything then tell us how you think the problem should be solved.

With no insight into your own thoughts it's impossible to help.

...R

Robin2:
I haven’t studied the code you provided because I infer from your post (perhaps incorrectly) that you have those pieces of code working properly and I can’t infer from your post what is the exact problem you want advice about.

You will have to tell us what you have tried, how it works and where it fall short of your requirement. If you haven’t got as far as trying anything then tell us how you think the problem should be solved.

With no insight into your own thoughts it’s impossible to help.

…R

Thanks for replying Robin. Actually I have a huge load of problems for this as I’m new to Arduino. Hope I can find help and solve the issues one by one. Well I can definitely make the motors turn using the codes above, but how do a create a threshold for ‘logging’ in the seconds the sensor detects the distance? For example, if the sensor returns a distance of > 15cm for 5seconds, then it will initiate the parallel parking sequence, else it will reset the counter and continue to move forward. The flow chart should help aid the understanding of my question.

Your description is still not focused as narrowly as is needed to figure out answers. What you write is a bit like the Wright brothers asking "how do you fly to Europe?"

Let's try one step at a time, but even the bit I have quoted has several uncertainties.

What do you mean by a threshold? I presume your program can tell that the distance is > 15cm.

If so then you need 2 things, a variable to record the fact that you are over 15cm and the time this first happened. So, for example

beyond15cm = true;
firstTimeBeyond15cm = millis()

Then in your loop you keep checking the distance and if it falls below 15cm you set beyond15cm to false. You also check the difference between the saved time value and millis() with something like

if (millis() - firstTimeBeyond15cm > 5000 && beyond15cm) {
    startParking;
}

...R

timlee: ... how do a create a threshold for 'logging' in the seconds the sensor detects the distance? For example, if the sensor returns a distance of > 15cm for 5seconds, then it will initiate the parallel parking sequence

Robin2:
What do you mean by a threshold? I presume your program can tell that the distance is > 15cm.

If so then you need 2 things, a variable to record the fact that you are over 15cm and the time this first happened. So, for example

beyond15cm = true;

firstTimeBeyond15cm = millis()



Then in your loop you keep checking the distance and if it falls below 15cm you set beyond15cm to false. You also check the difference between the saved time value and millis() with something like 


if (millis() - firstTimeBeyond15cm > 5000 && beyond15cm) {
    startParking;
}



Yes Robin, my ultrasonic sensors are able to tell me when it is >15cm. I need someone to point out the mistakes in my code. It seems that things are getting out of hand...
When it detects distance is < 15cm, it does not move and when distance is > 15cm, it moves continuously and does not stop, whereas what I wanted is the exact opposite!

It should be this way. when distance is < 15cm it should move and start parking (stop) when distance is > 15cm for 5 seconds. I also need to reset the timer if the distance which is > 15cm is lesser than 5 seconds.

I declared my variables:


boolean beyond15cm;
unsigned long firstTimeBeyond15cm;




my code:


if ( cm >= 15 ){
   
beyond15cm = true;
firstTimeBeyond15cm = millis();
if (firstTimeBeyond15cm > 50000 && beyond15cm == true) {
  startparking();
    }

// if distance from car is more than 15cm, car will continue to move
  else {
      beyond15cm = false;
      forward();
    }
}

You should not have “if (firstTimeBeyond15cm > 50000 && …” within the section for " if ( cm >= 15 ){ …"

…R

Robin2: You should not have "if (firstTimeBeyond15cm > 50000 && ..." within the section for " if ( cm >= 15 ){ ..."

...R

how can I stop the millis() and also reset it? I found 2 forums on these but still couldn't get it. hopefully you can help out http://forum.arduino.cc/index.php/topic,44664.0.html http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1167861718

how can I stop the millis() and also reset it?

Let's not go there again, please.

There is no need to stop the millisecond clock and no reason to reset it.

The whole purpose of millis() is that it keeps counting forever. You don't need to reset it. You just save a copy of its current value whenever you want to make a note of the time - just like using a clock in your home.

...R

I finally came up with the code. However I met with a small problem which I need your guys’ help. The car is switching from state 2 and state 1 indefinitely sometimes when it is already in the parking lot. How can I make it to remain in state 2 and not go back to state 1, and when the conditions are right, go into state 3.

My RC car uses 3 sensors, one at the side, one infront and back. An elaborate of how the code should work:

  1. At initial state=0, When sidesensor detects distance is more than 20cm for 5seconds, starts to reverse right into the parking lot (goes into state 1 from here)
  2. While doing the right reverse, backsensor will check if distance to wall is < 5cm, if so, it will stop and goes into state 2.
  3. At state 2, the car moves forleft until the distance from the backsensor = distance from the frontsensor (goes into state 3)
  4. At state 3, goes into complete stop and while(1).

Heres my code:

  if (sideDist < 20 && state ==0) {
    forward();
  }
    else {
      forward();
    currentMillis = millis();
    }
    
    if(sideDist >= 20 && state == 0 && currentMillis >= threshold || currentMillis - previousMillis >= threshold) {
     previousMillis = currentMillis;    
     stopping();
     revright();
     state=1;
  }
    
  if(backDist > 5 && state==1) {
    revright();
  }
  else if(backDist <= 5 && state==1) {
    stopping();
    state=2;
  }
  
  if(frontDist > 5 && state==2) {
    forleft();
  }
  else if(frontDist=backDist && state==2) {
    stopping();
    state=3;
  }
  
  if(state == 3) {
    while(1);
  }

What is this meant to be doing?

currentMillis >= threshold || currentMillis - previousMillis >= threshold

Please define meaningful names for your state values, either by defining const values or by defining an enum type.

Code snippets really don’t work when you’re asking questions about your code. We need to see the whole thing, because the behaviour of the part you’re showing us here is affected fundamentally by the rest of the sketch.

PeterH: What is this meant to be doing?

currentMillis >= threshold || currentMillis - previousMillis >= threshold

Please define meaningful names for your state values, either by defining const values or by defining an enum type.

Code snippets really don't work when you're asking questions about your code. We need to see the whole thing, because the behaviour of the part you're showing us here is affected fundamentally by the rest of the sketch.

This are the declarations:

int state = 0;

const int threshold = 3000;
long previousMillis = 0;
unsigned long currentMillis;

I don't think your reply has addressed any of my comments.

PeterH:
I don’t think your reply has addressed any of my comments.

Sorry I’m lost as on what I’m supposed to do. Anyway, here’s the full code:

const int sideSensor = 5;
const int backSensor = 6;
const int frontSensor = 7;

int state = 0;

const int threshold = 3000;
long previousMillis = 0;
unsigned long currentMillis;

void setup() {
  
  Serial.begin(9600);
  
  //Setup Channel A
  pinMode(12, OUTPUT); //Initiates Motor Channel A pin
  pinMode(9, OUTPUT); //Initiates Brake Channel A pin

  //Setup Channel B
  pinMode(13, OUTPUT); //Initiates Motor Channel A pin
  pinMode(8, OUTPUT);  //Initiates Brake Channel A pin
  
}

void loop(){

  long duration1, cm1;
  pinMode(sideSensor, OUTPUT);
  digitalWrite(sideSensor, LOW);
  delayMicroseconds(2);
  digitalWrite(sideSensor, HIGH);
  delayMicroseconds(5);
  digitalWrite(sideSensor, LOW);
  pinMode(sideSensor, INPUT);
  duration1 = pulseIn(sideSensor, HIGH);
  cm1 = microsecondsToCentimeters(duration1);
  int sideDist = cm1;
  delayMicroseconds(50);
  
  if (sideDist < 20 && state ==0) {
    forward();
  }
    else {
      forward();
    currentMillis = millis();
    }
    
    if(sideDist >= 20 && state == 0 && currentMillis >= threshold || currentMillis - previousMillis >= threshold) {
     previousMillis = currentMillis;    
     stopping();
     revright();
     state=1;
  }
  
  long duration2, cm2;
  pinMode(backSensor, OUTPUT);
  digitalWrite(backSensor, LOW);
  delayMicroseconds(2);
  digitalWrite(backSensor, HIGH);
  delayMicroseconds(5);
  digitalWrite(backSensor, LOW);
  pinMode(backSensor, INPUT);
  duration2 = pulseIn(backSensor, HIGH);
  cm2 = microsecondsToCentimeters(duration2);
  int backDist = cm2;
    delayMicroseconds(50);
    
  if(backDist > 5 && state==1) {
    revright();
  }
  else if(backDist <= 5 && state==1) {
    stopping();
    state=2;
  }
  
   long duration3, cm3;
  pinMode(frontSensor, OUTPUT);
  digitalWrite(frontSensor, LOW);
  delayMicroseconds(2);
  digitalWrite(frontSensor, HIGH);
  delayMicroseconds(5);
  digitalWrite(frontSensor, LOW);
  pinMode(frontSensor, INPUT);
  duration3 = pulseIn(frontSensor, HIGH);
  cm3 = microsecondsToCentimeters(duration3);
  int frontDist = cm3;
    delayMicroseconds(50);
    
  if(frontDist > 5 && state==2) {
    forleft();
  }
  else if(frontDist=backDist && state==2) {
    stopping();
    state=3;
  }
  
  if(state == 3) {
    while(1);
  }

  Serial.print("Side ");
  Serial.print(cm1);
  Serial.print(" || Back ");
  Serial.print(cm2);
  Serial.print(" || Front ");
  Serial.println(cm3);
  Serial.print("State ");
  Serial.print(state);
  Serial.print("\t"); 
  Serial.print(" || Current ");
  Serial.print(currentMillis);
  Serial.print("\t"); 
  Serial.print(" || Previous ");
  Serial.println(previousMillis);
  delay(500);
}
void forward()  {
  //Motor A forward @ full speed
  digitalWrite(12, LOW); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A at full speed
}
void stopping()  {
    
  digitalWrite(9, HIGH);  //Engage the Brake for Channel A
  digitalWrite(8, HIGH);  //Engage the Brake for Channel B
}
void revright()  {
  digitalWrite(12, HIGH); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A at full speed
  digitalWrite(13, HIGH);  //Establishes backward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 255);    //Spins the motor on Channel B at half speed
}
void forleft()  {
  digitalWrite(12, LOW); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A at full speed
  digitalWrite(13, LOW);  //Establishes backward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 255);    //Spins the motor on Channel B at half speed
}
void reverse()  {
  //Motor A forward @ full speed
  digitalWrite(12, HIGH); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A at full speed
}
long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

It would be useful to create a function called park(). Then, you can see if that function is called unnecessarily, or if the park() function itself is fundamentally flawed.

Solving problems is a matter of divide and conquer. Like sorting, you divide the code into two parts. One has the problem, one doesn't. You divide the part with the problem into two parts. Again, one has the problem, one doesn't.

As the parts get smaller, the error eventually becomes obvious.

timlee:
Sorry I’m lost as on what I’m supposed to do.

I’m looking at your code and trying to reverse-engineer your logic to work out what the different state values are intended to represent, so that I can understand how it would be sensible to transition between them and whether you have any state transitions that are not sensible.

That’s rather hard to do because you didn’t do this:

PeterH:
Please define meaningful names for your state values, either by defining const values or by defining an enum type.

I have a suspicion that state 0 is trying to cover two different states at the same time, but without understanding what you intend it to be I can’t confirm or deny that.
You also haven’t answered this question:

PeterH:
What is this meant to be doing?

currentMillis >= threshold || currentMillis - previousMillis >= threshold

A way to structure a finite state machine which I find very simple and clear is to use a switch statement:

switch(state)
{
    case INITIAL:
        // we assume there is a line of parked vehicles to our right, with an empty bay somewhere in front of us
        moveForwards();
        state = LOCATE_REAR_OF_BAY;
        break;
    case LOCATE_REAR_OF_BAY: 
        // keep moving forwards until we detect an opening beside us
        if(sideDist > 20)
        {
            // opening found
            // keep moving forward
            state = LOCATE_FRONT_OF_BAY;
        }
        break;
    case LOCATE_FRONT_OF_BAY:
        // keep moving forward until we detect a vehicle beside us i.e. the front edge of the bay we'll be parking in
        if(sideDist < 20)
        {
            // vehicle found
            // keep moving forward a bit further
           state = MOVE_TO_READY_POSITION;
           startTime = millis();
        }
        break;
    case MOVE_TO_READY_POSITION:
        // move past the bay slightly before trying to reverse into it
        // here the distance is defined by how long we move forward for
        if(millis() - startTime >= FORWARD_DURATION)
        {
            // we have moved forward far enough, start reversing into the bay
            stop();
            reverseRight();
            state = REVERSING_RIGHT;

    ... and so on

I don’t know whether the logic above is anything like the algorithm you’re actually using, but all I’m trying to show is how to structure your code so that each state corresponds to a specific part of the overall maneuver and the code for each state implements the tests to decide when to move to another state. I recommend that you aim to keep your states as simple as possible (even if that means you end up with a lot of different states).

PeterH:
Thanks Peter I think the code you got the idea of what I want. I’m not good in programming and I haven’t tried ‘switch (state)’ before. Is this supposed to be in void loop { or after void { setup?

I’m definitely going to try your code out but any extra advise would be most appreciated. I have also attached an abruptly edited flowchart. Hope it helps in understanding.

The switch statement would be inside a function. It could be inside loop(), or some other function you create that is called from loop().

The flow chart you posted is on the right lines but doesn't (yet) represent a sensible design IMO.

I'd suggest you try to break the overall process into a sequence of steps, with each step ended by some tangible condition.

For example I expect that you would want to start by finding where the parking bay starts, then finding where it ends, then move to the position that you are going to park from, then reverse while turning towards the curb, then reverse while turning away from the curb, then move forward to align the vehicle with the curb, then move back to center the vehicle in the bay. Or something like that - the algorithm is for you to define. Each discrete state should correspond to an activity which is in progress which will be ended by some event. In the switch case section for each event you would put the code to detect the events relevant to that state and respond to them.

PeterH:
The switch statement would be inside a function. It could be inside loop(), or some other function you create that is called from loop().

I’d suggest you try to break the overall process into a sequence of steps, with each step ended by some tangible condition.

For example I expect that you would want to start by finding where the parking bay starts, then finding where it ends, then move to the position that you are going to park from, then reverse while turning towards the curb, then reverse while turning away from the curb, then move forward to align the vehicle with the curb, then move back to center the vehicle in the bay.

Thanks again Peter. I’m trying to work my way into using the switch(state) but got a problem trying to define the states itself. Like “INITIAL” was not declared in the scope and so are the other states. I took a look at the tutorial http://playground.arduino.cc/code/FiniteStateMachine and got myself all confused. I’m not using ledStateMachine so what should I do? I know I can be a real pain… Appreciate if you could help further.

Heres my code where I got stuck trying to “initialise” the states. Currently it is only asking to move the car in the bay.

#include <FiniteStateMachine.h>

const int sideSensor = 5;
const int backSensor = 6;
const int frontSensor = 7;

const int FORWARD_DURATION = 3000;
const byte state = 3;
unsigned long startTime;

//initialize states
State LOCATE_REAR_OF_BAY = State(fLOCATE_REAR_OF_BAY);
State INITIAL = State(fINITIAL);
State MOVE_TO_READY_POSITION = State(fMOVE_TO_READY_POSITION);
 
FSM ledStateMachine = FSM(On);     //initialize state machine, start in state: On

void setup() {
  
  Serial.begin(9600);
  
  //Setup Channel A
  pinMode(12, OUTPUT); //Initiates Motor Channel A pin
  pinMode(9, OUTPUT); //Initiates Brake Channel A pin

  //Setup Channel B
  pinMode(13, OUTPUT); //Initiates Motor Channel A pin
  pinMode(8, OUTPUT);  //Initiates Brake Channel A pin
  
}

void loop(){

  long duration1, cm1;
  pinMode(sideSensor, OUTPUT);
  digitalWrite(sideSensor, LOW);
  delayMicroseconds(2);
  digitalWrite(sideSensor, HIGH);
  delayMicroseconds(5);
  digitalWrite(sideSensor, LOW);
  pinMode(sideSensor, INPUT);
  duration1 = pulseIn(sideSensor, HIGH);
  cm1 = microsecondsToCentimeters(duration1);
  int sideDist = cm1;
  delayMicroseconds(50);
    
  long duration2, cm2;
  pinMode(backSensor, OUTPUT);
  digitalWrite(backSensor, LOW);
  delayMicroseconds(2);
  digitalWrite(backSensor, HIGH);
  delayMicroseconds(5);
  digitalWrite(backSensor, LOW);
  pinMode(backSensor, INPUT);
  duration2 = pulseIn(backSensor, HIGH);
  cm2 = microsecondsToCentimeters(duration2);
  int backDist = cm2;
  delayMicroseconds(50);
  
  long duration3, cm3;
  pinMode(frontSensor, OUTPUT);
  digitalWrite(frontSensor, LOW);
  delayMicroseconds(2);
  digitalWrite(frontSensor, HIGH);
  delayMicroseconds(5);
  digitalWrite(frontSensor, LOW);
  pinMode(frontSensor, INPUT);
  duration3 = pulseIn(frontSensor, HIGH);
  cm3 = microsecondsToCentimeters(duration3);
  int frontDist = cm3;
  delayMicroseconds(50);
  
  switch(state)
{
    case INITIAL:
        // we assume there is a line of parked vehicles to our right, with an empty bay somewhere in front of us
        moveForwards();
        state = LOCATE_REAR_OF_BAY;
        break;
    case LOCATE_REAR_OF_BAY: 
        // keep moving forwards until we detect an opening beside us
        if(sideDist > 20)
        {
            // opening found
            // keep moving forward
            startTime = millis();  // start counting time (distance)
            state = MOVE_TO_READY_POSITION;
        }
        else if(sideDist < 20)
        {
            // vehicle found
            // keep moving forward a bit further
           state = INITIAL;     
        }
        break;
        
    case MOVE_TO_READY_POSITION:
        // move past the bay slightly before trying to reverse into it
        // here the distance is defined by how long we move forward for
        if(millis() - startTime >= FORWARD_DURATION)
        {
            // we have moved forward far enough, start reversing into the bay
            stop();
            reverseRight();
            state = REVERSING_RIGHT;
        }
        else if(millis() - startTime < FORWARD_DURATION)
        {
            // space is not big enough to park
            state = LOCATE_REAR_OF_BAY;
        }
        break;
}

/*  
  if (sideDist < 20 && state ==0) {
    forward();
  }
    else {
      forward();
    currentMillis = millis();
    }
    
    if(sideDist >= 20 && state == 0 && currentMillis >= threshold || currentMillis - previousMillis >= threshold) {
     previousMillis = currentMillis;    
     stopping();
     revright();
     state=1;
  }
    
  if(backDist > 5 && state==1) {
    revright();
  }
  else if(backDist <= 5 && state==1) {
    stopping();
    state=2;
  }
  
  if(frontDist > 5 && state==2) {
    forleft();
  }
  else if(frontDist=backDist && state==2) {
    stopping();
    state=3;
  }
  
  if(state == 3) {
    while(1);
  }
*/
  Serial.print("Side ");
  Serial.print(cm1);
  Serial.print(" || Back ");
  Serial.print(cm2);
  Serial.print(" || Front ");
  Serial.println(cm3);
  Serial.print("State ");
  Serial.print(state);
  Serial.print("\t"); 
  Serial.print(" || Current ");
  Serial.print(currentMillis);
  Serial.print("\t"); 
  Serial.print(" || Previous ");
  Serial.println(previousMillis);
  delay(500);
}
void moveForwards()  {
  //Motor A forward @ full speed
  digitalWrite(12, LOW); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A at full speed
}
void stopping()  {
    
  digitalWrite(9, HIGH);  //Engage the Brake for Channel A
  digitalWrite(8, HIGH);  //Engage the Brake for Channel B
}
void reverseRight()()  {
  digitalWrite(12, HIGH); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A at full speed
  digitalWrite(13, HIGH);  //Establishes backward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 255);    //Spins the motor on Channel B at half speed
}
void forleft()  {
  digitalWrite(12, LOW); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A at full speed
  digitalWrite(13, LOW);  //Establishes backward direction of Channel BDD
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 255);    //Spins the motor on Channel B at half speed
}
void reverse()  {
  //Motor A forward @ full speed
  digitalWrite(12, HIGH); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A at full speed
}
long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}