State Machines with ultrasonic sensor

OpampInverting · July 6, 2020, 12:38pm

I am trying to run a motor for X seconds if the HC-SR04 sensor gives a distance reading which is less than Y cm. For getting the distance from HC-SR04 following is my code.

class Sonar{
  
  int TrigPin;
  int EchoPin;
  int MaxDistance;                // Maximum range allowed in mm
  float TrustFactor;
  int PreviousDistance;
  
  //These will change
  int CurrentDistance;

  public:
  Sonar(int trigpin, int echopin, int maxdistance, float trustfactor){
    
    TrigPin = trigpin;
    EchoPin = echopin;
    MaxDistance = maxdistance;
    TrustFactor = trustfactor;
    pinMode(TrigPin,OUTPUT);
    pinMode(EchoPin,INPUT);
    digitalWrite(TrigPin,LOW);
    }

  int Measure(){
    digitalWrite(TrigPin,HIGH);
    delayMicroseconds(10);
    digitalWrite(TrigPin,LOW);
    int Time = pulseIn(EchoPin,HIGH,5000);
    CurrentDistance = Time*0.1715;
    if (CurrentDistance == 0 || CurrentDistance > 100){
      CurrentDistance = MaxDistance;
      }
    return CurrentDistance;
    }
    
  int Filtered(){
    CurrentDistance = constrain(PreviousDistance*(1-TrustFactor) + Measure()*TrustFactor,0,MaxDistance);
    PreviousDistance = CurrentDistance;
    return CurrentDistance;
    }
};

I am using Sonar.Measure() function to get my measurements. I am not using the Filtered() function.
Now I can just use delays to do this job but I also want to monitor the sensor data so if the distance for some reason is greater than Y cm before the end of X seconds then motor needs to turn of. For sake of simplicity we will assume LED pin 13 is the motor and let Y be 6cm and X be 3 seconds.

Here is a state machine I drew, I am not an expert so please bear with me. I might learn something new.

Here is what I want to accomplish.

In the first step I want to read the distance from sensor using Measure() function.
If distance<6, then I want to wait for 300ms and read the distance again to confirm if the distance is really <6.
If within in 300 ms the distance > 6 then I want to go to step one treat it as a bounce.
If after 300ms the distance is still <6 I finally want to turn on the motor and start a timer for 3 seconds.
While the motor is running check if distance is still <6 and if not turn off the motor and go to step one.
After the motor has finished running check if distance > 6 and if not keep checking until it is.
If the distance > 6 then got to step one which will arm the trigger again.

While doing all this i thought I also need to have enough delay between the ultrasonic measurements so that I do not get garbage data. So i am running state machine code at 100ms delay.
I have given it a try but no success. I am not really good at programming. Here is the code,

Sonar sonar1(12,11,100,0.120);            //The last argument is the "Trust Factor" it is a value between 0 and 1

void setup() {
  Serial.begin(9600);
  pinMode(13, OUTPUT);
}

void loop() {
  unsigned long previousMillis = 0;
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= 100){
  previousMillis = currentMillis;
  sm_s1();
  if(state_s1 == 4){
    digitalWrite(13, HIGH);
  }
  if (state_s1 != state_prev_s1){
    Serial.print("state = ");
    Serial.println(state_s1);
    
  }
  }

}

//statemachine
void sm_s1(){
  state_prev_s1 = state_s1;
  switch(state_s1){
    case 0: //RESET
    state_s1 =1;
    break;
    case 1: //START
    distance = sonar1.Measure();
    if(distance < 60){state_s1 = 2;}
    break;
    case 2: //GO
    t_0_s1 = millis();
    state_s1 = 3;
    break;
    case 3: //WAIT
    distance = sonar1.Measure();
    t_s1 = millis();
    if(distance > 60) {state_s1 = 0;}
    if(t_s1 - t_0_s1 > 200){
      state_s1 = 4;
    }
    break;
    case 4: //TRIGGRED
    state_s1 = 5;
    break;
    case 5: //WAIT
    distance = sonar1.Measure();
    if(distance > 60) {state_s1 = 0;}
    state_s1 = 5;
    break;
  }
}

What do I need to learn to get this working? What is the best way to accomplish this? Please guide me.
Thank You

johnwasser · July 6, 2020, 1:07pm

  unsigned long previousMillis = 0;

You forgot to mark this variable 'static' so it will be re-initialized to 0 each time through loop(). Alternatively, you could move the declaration out of loop() to make the variable global. Since the variable is only used in one function (loop()), 'static' is the preferred method:

  static unsigned long previousMillis = 0;

StefanL38 · July 6, 2020, 1:19pm

Hi opampinverting,

you seem to have knowledge about electronic.
First off all karma for posting such a well documented question. and the question at the end

I have taken a quick look at your statemachine. from this quick view it looks good. But I haven't analysed it in detail
One thing catched my eye

If I understand right your state 5

case 5: //WAIT
    distance = sonar1.Measure();
    if(distance > 60) {state_s1 = 0;}
    state_s1 = 5;
    break;
  }

whenever state 5 is reached and the if-condition is false = (distance < 60)
it stays in state 5 until the condition becomes true. Are you sure you want it that way?

you should give the states constant-names

const int sm_Reset     = 0;
const int sm_Start     = 1;
const int sm_Go        = 2;
const int sm_Wait      = 3;
const int sm_triggered = 4;
const int sm_Wait_dist_smaller60 = 5;

This makes you code selfcommenting and the constants are selfexplaining.

best regards Stefan

wildbill · July 6, 2020, 1:22pm

Better yet, use an enum for the state names.

StefanL38 · July 6, 2020, 2:36pm

wildbill:
Better yet, use an enum for the state names.

that's a very good idea! Would you mind giving an example how it is written, including what variable-type the stat-variable has if "enum"-values are assigned to the state-variable?
best regards Stefan

wildbill · July 6, 2020, 2:50pm

Example:

// system states
enum EngineStatus {STOPPED, STARTING, CRANKING, RUNNING, ALARM, PRECRANK, POSTCRANKFAILURE};

system · July 6, 2020, 2:52pm

(deleted)

OpampInverting · July 6, 2020, 4:56pm

StefanL38:
Hi opampinverting,

you seem to have knowledge about electronic.
First off all karma for posting such a well documented question. and the question at the end

I have taken a quick look at your statemachine. from this quick view it looks good. But I haven't analysed it in detail
One thing catched my eye

If I understand right your state 5
case 5: //WAIT

distance = sonar1.Measure();
if(distance > 60) {state_s1 = 0;}
state_s1 = 5;
break;
}

whenever state 5 is reached and the if-condition is false = (distance < 60) 
it stays in state 5 until the condition becomes true. Are you sure you want it that way?


you should give the states constant-names
const int sm_Reset = 0;
const int sm_Start = 1;
const int sm_Go = 2;
const int sm_Wait = 3;
const int sm_triggered = 4;
const int sm_Wait_dist_smaller60 = 5;
This makes you code selfcommenting and the constants are selfexplaining.

best regards Stefan

Hi stefan you are correct. I want it to be in state 5 until distance > 6 condition is meet. I want this because I want to essentially rearm the system for next trigger. The user of the system needs to remove his/hand from the trigger and bring it back to retrigger the system.

But I am having a problem associated with state 5. System gets stuck in state 5. It seems it is not capable of getting out of it. What could be the problem. Thanks

OpampInverting · July 6, 2020, 5:01pm

Hello stefan, I removed state_s1 = 5; from state 5 and it seems to work fine for now. I will keep you guys updated Thank you so much Everyone!!

system · May 6, 2021, 12:38am