Problem with Arduino code on a state machine

[AscanioFX]

Hey everybody I am attempting to build an arduino robot, the easy one you start with, which detects obstacle with a IR sensor and move right or left on the better way considering his position.
The robot has 2 dc motors controlled by an h-bridge, one IR sensor mounted on a servo (for watching right and left) and a on-off pushbutton with a LED for on or off state (to see if it is on or not)
Here is the pseudocode:

If the distance is > 25 cm (means the way is clear)
   move forward
else (checks for the best way to turn)
  watch right, save distance
  watch left, save distance
  compare distances
  choose the way (left or right) where the it is more clear.

It doesn't work. When it approaches an obstacle, it immediatly turn always left while turning the servo left. At the sme time. It doesn't even look right...
Here is the loop code:

Code: [Select]

void loop () {
  int val;                         // Ir val
  float distance;              // distance of val in cm
  float rightDistance;
  float leftDistance;
  time = millis();
 
  if ( time - previousTime > 250){
     previousTime = time;
 
  // BUTTON CODE
  buttonState = debounce(previousButtonState);
  if ( buttonState == HIGH && previousButtonState == LOW) {
       botState = !botState;
     }
  previousButtonState = buttonState;
 
     
   

  val = analogRead(sensorPin); 
  distance = 6787.0 /(val - 3.0) - 4.0;
 
   
   if ( botState == HIGH ) {            // Means if it is on
      digitalWrite(ledPin, HIGH);
      Serial.println(distance);
     if ( distance > 25 ){
       testa.write(85);
       Forward(220, 170);
   
   delay(1000);   
     } else {            // If there is an obstacle...
     STOP();
     
   
   // NOW WATCH RIGHT (0 DEGREES) AND LEFT. SAVE DISTANCES
     
     Serial.println("0 GRADI  ");
     delay(1000);
     testa.write(0);
     val = analogRead(sensorPin); 
     distance = 6787.0 /(val - 3.0) - 4.0;
     rightDistance = distance;
     Serial.print("VALORE: ");
     Serial.println(rightDistance);   
     Serial.println("180 GRADI  ");
     testa.write(180);
     val = analogRead(sensorPin); 
     distance = 6787.0 /(val - 3.0) - 4.0;
     leftDistance = distance;
     Serial.print("VALORE: ");
     Serial.println(leftDistance);   
     
     
     if (rightDistance > leftDistance){
       Serial.println("DESTRA");
       Right90();                      // More space on right then turn right
     
     } else {
       Serial.println("SINISTRA");
       Left90();                         // turn left
       
     } 
  delay(1000);
 }
   
   
  }else{
     digitalWrite(ledPin, LOW);
     STOP();
    }
  }
}


[jwatte]

I assume "testa" is a servo?
The problem is that you don't wait for the servo to reach the target position before reading the distance. Try adding a delay(400) or so after changing the servo position to make sure it has time to get there.

[AscanioFX]

Thank you. I managed to make it work, so that now it chooses for the actual best way. The loop code, by the way now looks like this:

Code: [Select]

void loop () {
  int val;
  float distance;
  float rightDistance = 0;
  float leftDistance = 0;
  time = millis();
 
  if ( time - previousTime > 250){
     previousTime = time;
 
  // BOTTONE 
  buttonState = debounce(previousButtonState);
  if ( buttonState == HIGH && previousButtonState == LOW) {
       botState = !botState;
     }
  previousButtonState = buttonState;
  // FINE BOTTTONE
     
   

  val = analogRead(sensorPin); 
  distance = 6787.0 /(val - 3.0) - 4.0;
 
   
   if ( botState == HIGH ) {
      digitalWrite(ledPin, HIGH);
      Serial.println(distance);
     if ( distance > 25 ){
       testa.write(80);
       Forward(220, 170);
   
   delay(1000);   
     } else {     
     STOP();
     
   
   // MUOVUI LA TESTOLINA
     
     Serial.println("0 GRADI  ");
     delay(1000);
     testa.write(0);
     delay(1000);
     val = analogRead(sensorPin); 
     distance = 6787.0 /(val - 3.0) - 4.0;
     rightDistance = distance;
     Serial.print("VALORE A DESTRA: ");
     Serial.println(rightDistance);   
     Serial.println("180 GRADI  ");
     testa.write(165);
     delay(1000);
     val = analogRead(sensorPin); 
     distance = 6787.0 /(val - 3.0) - 4.0;
     leftDistance = distance;
     Serial.print("VALORE A SINISTRA: ");
     Serial.println(leftDistance);   
     // Fine muovi testolina
     delay(1000);
     
     if (rightDistance > leftDistance){
       Serial.println("DESTRA");
       Right90();
     
     } else {
       Serial.println("SINISTRA");
       Left90();
       
     }   
  testa.write(80);
  delay(1000);
 }
   
   
  }else{
     digitalWrite(ledPin, LOW);
     STOP();
    }
  }
}

The problem is that the button is really unreactive and I think I know the reason. It's becouse once it enters the watch-check-choose block it has to wait for it to finish right?
Is there a better solution for increasing it reactivity instead of simply copy-paste the button code every delay?
And, the bot doesn't go straight at all. I know it is normal becouse i don't have any encoders or anything. I was planning to add, instead of an encoder, a digital compass to meke it go straigh and for make sure it turns of the amount of degree requested. Would a digital compass be precise enough? How much precison do I need on the compass degrees? Or would it be better to set a pair of encoders?

[jwatte]

A proper embedded system doesn't use delay() at all.
Instead, it builds the software as a state machine, and runs that state machine over and over again.
For things that "take time," you use a timer / clock. Measure the time you entered a state, and then in the state, if the difference between "now" and "enter time" is greater than X, exit the state.

This is a different way of constructing software, and it's more complex than a simple "do this, wait, then do that," which is why beginner tutorials don't talk about it. If you have an Arduino, look at the "blink without delay" example for the very simplest implementation of this idea.

In general, your loop function will look like this:

Code: [Select]

void loop() {
  read all inputs;
  possibly change state based on current state and inputs;
  apply outputs based on current state;
}

This loop() will typically run in a fraction of a millisecond, so any input changing will be reacted to seemingly instantly. This includes things like buttons.

A more concrete example:

Code: [Select]

float speedleft = 0;
float speedright = 0;
float leftdistance = 0;
float rightdistance = 0;
float turn = 0;
float servoposition = 90;
unsigned int statetime = 0;
bool oldbutton = true;

enum State {
  StateStopped,
  StateForward,
  StateScanningLeft,
  StateScanningRight,
  StateTurningLeft,
  StateTurningRight
};
State curstate = StateStopped;

void loop() {
    float distance = read_sensor();
    bool button = read_button();

    if (oldbutton == false && button == true) {
        if (curstate == StateStopped) {
            curstate = StateForward;
        }
        else {
            curstate = StateStopped;
        }
    }
    switch (curstate) {
    case StateStopped:
        speedleft = 0;
        speedright = 0;
        servoposition = 90;
        statetime = millis();
        break;
    case StateForward:
        speedleft = 1;
        speedright = 1;
        if (distance < 25) {
            curstate = StateScanningLeft;
            speedleft = 0;
            speedright = 0;
        }
        statetime = millis();
        break;
    case StateScanningLeft:
        servoposition = 0;
        if (millis() - statetime > 500) {
            leftdistance = distance;
            curstate = StateScanningRight;
            statetime = millis();
        }
        break;
    case StateScanningRight:
        servoposition = 180;
        if (millis() - statetime > 500) {
            rightdistance = distance;
            if (rightdistance > leftdistance) {
                curstate = StateTurningRight;
            }
            else {
                curstate = StateTurningLeft;
            }
            statetime = millis();
        }
        break;
    case StateTurningLeft:
        speedleft = -1;
        speedright = 1;
        if (millis() - statetime > 700) {
             state = StateForward;
             statetime = millis();
        }
        break;
    case StateTurningRight:
        speedleft = 1;
        speedright = -1;
        if (millis() - statetime > 700) {
             state = StateForward;
             statetime = millis();
        }
        break;
    }

    oldbutton = button;
    set_motor_speeds(speedleft, speedright);
    set_servo_position(servoposition);
}

This is typed into the browser -- it may have typos and stuff, but should illustrate how to go about this.

Note that it may seem as if this program does things redundantly -- setting the motor and servo positions/speeds each time through the iteration. However, that's fine -- in general, setting a value to what it already is is a no-op, and if it makes the code simpler to reason about and more robust, then that's a good thing!

[AscanioFX]

Thank you very much for your reply. It really made my day and opened me new way of organizing my code. Still something doesn't work. I've got rid of all the delays, exept for a debounce function for button reading whoose code is:

Code: [Select]

boolean debounce(boolean last){
  boolean current = digitalRead(buttonPin);
  if (last != current){
    delay(5);
    current = digitalRead(buttonPin);
  }
  return current;
} 

Now when it starts it do things randomly, for example:
Sometimes it starts but obly watches right, and therefore only turn right.
sometimes it starts watching left but it doesn't give the servo the time to complete the rotation and immesiatly goes watching right. sometimes it works. I can't figure out any reason for this apparently random behaviour.
Here's my loop code:

Code: [Select]

void loop () {
  float distance;
  time = millis();

 
  // BOTTONE 
  buttonState = debounce(previousButtonState);
  if ( buttonState == HIGH && previousButtonState == LOW) {
       botState = !botState;
       if ( currentState == StateStopped){
         currentState = StateForward;
       } else {
         currentState = StateStopped;
       }
     }
  // FINE BOTTTONE

  distance = 6787.0 / (analogRead(sensorPin) - 3.0) - 4.0;


  if ( botState == HIGH ) {
    digitalWrite(ledPin, HIGH);
    switch (currentState){
      case StateForward:       
        FORWARD(220, 170);
        if ( distance < 25 && prevDistance < 25){
          currentState = StateScanningLeft;
          STOP();
          tone(piezoPin, 3000, 1000);
        }
        break;
      case StateScanningLeft:
        testa.write(165);
        if (millis() - stateTime > 1000) {
            leftDistance = distance;
            currentState = StateScanningRight;
            stateTime = millis();
        }
        break;
      case StateScanningRight:
       testa.write(0);
        if (millis() - stateTime > 1000) {                       
            rightDistance = distance;   
            currentState = StateChoosingDirection;
            stateTime =millis();
        }         
        break;
      case StateChoosingDirection:
        testa.write(80);
        if (millis() - stateTime > 1000){
          if(rightDistance > leftDistance){
            currentState = StateTurningRight;
            leftDistance = 0;
            rightDistance = 0;
          } else {
            currentState = StateTurningLeft;
            leftDistance = 0;
            rightDistance = 0;
          }
          stateTime =millis();
        } 
        break;
      case StateTurningLeft:       
        LEFT90();
        if (millis() - stateTime > 1000) {
           currentState = StateForward;
           stateTime =millis();
        }
        break;
      case StateTurningRight:
        RIGHT90();
        testa.write(80);
        if (millis() - stateTime > 1000) {
           currentState = StateForward;
           stateTime =millis();
        }
        break;   
    }
       
  } else {
    digitalWrite(ledPin, LOW);
    testa.write(80);
    STOP();
  }
   previousButtonState = buttonState;
   prevDistance = distance;
   
}
I had to to a prevDistance variable to sort of "clean up" the signal from the IR becouse it appeard to get some noise and therefore sometimes it read wrong distances and saw inexisting obstacle.
Any Idea?