State machine controlling dc motor/ buzzer/ lcd / ultrasonic

Hi dear community,

I ran into some problems here with programming. I am trying to put together a code for 4dc motors, buzzer, ultrasonic and lcd to work together,. they will together make up a small car. when the distance to the ultrasonic is smaller than 10cm, the buzzer makes a random sound, and a funny face. Meanwhile the motors will stop for 2 seconds, goes back for 2 seconds and do a turning motion for 8 seconds. However. I do not want to use the delay function for the motors since it will cause the buzzer to sound too slowly. I was directed from an earlier thread that I should use a state machine, and nicolajna has been kind enough to provide me with this structure
state machine
but when I tried to put the motor controls in, my motors does not turn, can someone help me with spotting what is the problem here?
Thank you!

#define HALT_TIME 2000
#define BACK_TIME 2000
#define TURN_TIME 8000
#define trigPin A1
#define echoPin A0
#include <LiquidCrystal.h>
#include <NewTone.h>
#include<AFMotor.h>

long duration;
int distance;
int distanceUltra;
int maximum = 100;
int buzzerPin = A2;
int dur = 100;

LiquidCrystal lcd(8, 9, 4, 7, 12, 13);
AF_DCMotor motor1(1,MOTOR34_1KHZ);
AF_DCMotor motor2(2,MOTOR34_1KHZ);
AF_DCMotor motor3(3,MOTOR34_1KHZ);
AF_DCMotor motor4(4,MOTOR34_1KHZ);

enum ApplicationState {
  BeginDrive,
  Drive,
  BeginHalt,
  Halt,
  BeginBack,
  Back,
  BeginTurn,
  Turn
};

static enum ApplicationState currentApplicationState = BeginDrive;

static unsigned long t = 0;

static bool sensorTriggered() {
  
  if (distance <= 10){return true;}
  
  else {return false;}
}

static void funnyFace() {

    lcd.setCursor(6,0);
    lcd.write("x");
    lcd.setCursor(8,0);
    lcd.write("x");
    lcd.setCursor(7,1);
    lcd.write("o");
    randSound(maximum);//random sound for buzzer
}

static void beginDriveHandler() {
    Serial.println(currentApplicationState);
    
    motor1.setSpeed(100);
    motor1.run(BACKWARD);
    motor2.setSpeed(100);
    motor2.run(BACKWARD);
    motor3.setSpeed(100);
    motor3.run(BACKWARD);
    motor4.setSpeed(100);
    motor4.run(BACKWARD);
  currentApplicationState = Drive;
  
}

static void driveHandler() {
  
  if (sensorTriggered()) {
    Serial.println(currentApplicationState);
    t = millis();
    currentApplicationState = BeginHalt;
  }
}

static void beginHaltHandler() {
  Serial.println(currentApplicationState);
    motor1.setSpeed(0);
    motor1.run(RELEASE);
    motor2.setSpeed(0);
    motor2.run(RELEASE);
    motor3.setSpeed(0);
    motor3.run(RELEASE);
    motor4.setSpeed(0);
    motor4.run(RELEASE);
 
  funnyFace();
  currentApplicationState = Halt;
}

static void haltHandler() {
  
  if (millis() - t > HALT_TIME) {
    Serial.println(currentApplicationState);
    t = millis();
    currentApplicationState = BeginBack;
  }
}

static void beginBackHandler() {
Serial.println(currentApplicationState);
    motor1.setSpeed(100);
    motor1.run(FORWARD);
    motor2.setSpeed(100);
    motor2.run(FORWARD);
    motor3.setSpeed(100);
    motor3.run(FORWARD);
    motor4.setSpeed(100);
    motor4.run(FORWARD);
  currentApplicationState = Back;
}

static void backHandler() {
  
  if (millis() - t > BACK_TIME) {
    Serial.println(currentApplicationState);
    t = millis();
    currentApplicationState = BeginTurn;
  }
}

static void beginTurnHandler() {
Serial.println(currentApplicationState);
    motor1.setSpeed(100);
    motor1.run(FORWARD);
    motor2.setSpeed(100);
    motor2.run(FORWARD);
    motor3.setSpeed(100);
    motor3.run(BACKWARD);
    motor4.setSpeed(100);
    motor4.run(BACKWARD);
  currentApplicationState = Turn;
}

static void turnHandler() {
  
  if (millis() - t > TURN_TIME) {
    Serial.println(currentApplicationState);
    currentApplicationState = BeginDrive;
  }
}

void setup() {
  Serial.begin(9600);
  lcd.begin(16,2);
  pinMode (trigPin, OUTPUT);
  pinMode (echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
}

void loop() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  //Serial.println(sensorTriggered());
  duration = pulseIn(echoPin, HIGH);
  distance = duration*0.034/2;
  switch (currentApplicationState) {

    case BeginDrive:
    beginDriveHandler();
    break;
    
    case Drive:
    driveHandler();
    break;

    case BeginHalt:
    beginHaltHandler();
    break;

    case Halt:
    haltHandler();
    break;

    case BeginBack:
    beginBackHandler();
    break;

    case Back:
    backHandler();
    break;

    case BeginTurn:
    beginTurnHandler();
    break;

    case Turn:
    turnHandler();
    break;
  }

  //Serial.println(currentApplicationState);
}


void randSound(int maximum){
  NewTone(buzzerPin, random(maximum, 10*maximum),dur);
  delay(maximum);
  noNewTone(buzzerPin);
}

Hi,

So I am trying to put together a code for 4dc motors, buzzer, ultrasonic and lcd to work together,. they will together make up a small car. when the distance to the ultrasonic is smaller than 10cm, the buzzer makes a random sound, and a funny face. Meanwhile the motors will stop for 2 seconds, goes back for 2 seconds and do a turning motion for 8 seconds. However. I do not want to use the delay function for the motors since it will cause the buzzer to sound too slowly. I know I need to do a blink without delay code, I am trying to implement this but am having trouble right now: the motors will stop when I put in the motor stop function, but it will not go backwards.. I know there are some timing issues but how can I fix this?
please see attached the code and wiring diagram.

Thank you sooo much !!!!

#include <LiquidCrystal.h>
#include <NewTone.h>

#include<AFMotor.h>
#define trigPin A1
#define echoPin A0

AF_DCMotor motor1(1,MOTOR34_1KHZ);
AF_DCMotor motor2(2,MOTOR34_1KHZ);
AF_DCMotor motor3(3,MOTOR34_1KHZ);
AF_DCMotor motor4(4,MOTOR34_1KHZ);

LiquidCrystal lcd(8, 9, 4, 7, 12, 13);

int buzzerPin = A2;
int notes[] = {262};
int dur = 100;
int maximum = 100;
long duration;
int distance;
int distanceUltra;
unsigned long previousBuzzerMillis = 0;
unsigned long previousDCStopMillis = 0;
unsigned long previousDCBackMillis = 0;
unsigned long currentMillis = 0;
const unsigned long DCStopInterval =2000;
const unsigned long DCBackInterval =2000;


byte a[]={
  B10001,
  B10001,
  B10101,
  B10101,
  B01010,
  B00000,
  B00000,
  B00000
  };
  
byte b[]={
  B00000,
  B00010,
  B00100,
  B01000,
  B01000,
  B01000,
  B01000,
  B01000
  };

byte c[]={
  B00000,
  B01000,
  B01000,
  B01000,
  B01000,
  B01000,
  B00100,
  B00010
  };

byte h[]={
  B00000,
  B01000,
  B00100,
  B00010,
  B00010,
  B00010,
  B00010,
  B00010
  };

void setup() {
   
  Serial.begin(9600);
  lcd.begin(16,2);
  pinMode (trigPin, OUTPUT);
  pinMode (echoPin, INPUT);
  lcd.createChar(0,a);
  lcd.createChar(1,b);
  lcd.createChar(2,h);
  lcd.createChar(3,c);

  pinMode(buzzerPin, OUTPUT);
  }

void loop() {
  
  currentMillis = millis();
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = duration*0.034/2;
  
  Serial.print("distance:");
  Serial.println(distance);
  
  lcd.clear();
currentMillis = millis();

  
  if (distance <= 10){
    
 
    stopDCMotorStop();
    //stopDCMotorBack();
    

    }
    
  else{
    motor1.setSpeed(100);
    motor1.run(BACKWARD);
    motor2.setSpeed(100);
    motor2.run(BACKWARD);
    motor3.setSpeed(100);
    motor3.run(BACKWARD);
    motor4.setSpeed(100);
    motor4.run(BACKWARD);
      }


  if (distance <= 10){
  
    lcd.setCursor(5,1);   //set lcd faces
    lcd.write((char) 3);
    lcd.setCursor(5,0);
    lcd.write((char) 1);
    lcd.setCursor(9,1);
    lcd.write((char) 201);
    lcd.setCursor(9,0);
    lcd.write(2);
    lcd.setCursor(6,0);
    lcd.write("x");
    lcd.setCursor(8,0);
    lcd.write("x");
    lcd.setCursor(7,1);
    lcd.write("o");
    }

    if (distance <= 10){
      randSound(maximum); 
    }
      
}

void randSound(int maximum){
  NewTone(buzzerPin, random(maximum, 10*maximum),dur);
  delay(maximum);
  noNewTone(buzzerPin);
}

void stopDCMotorStop(){

  if (currentMillis-previousDCStopMillis<= DCStopInterval) {
    motor1.setSpeed(0);
    motor1.run(RELEASE);
    motor2.setSpeed(0);
    motor2.run(RELEASE);
    motor3.setSpeed(0);
    motor3.run(RELEASE);
    motor4.setSpeed(0);
    motor4.run(RELEASE);
    previousDCStopMillis =currentMillis;
    }

  }

void stopDCMotorBack(){

 
  if (currentMillis-previousDCBackMillis<= DCBackInterval) {
    motor1.setSpeed(100);
    motor1.run(FORWARD);
    motor2.setSpeed(100);
    motor2.run(FORWARD);
    motor3.setSpeed(100);
    motor3.run(FORWARD);
    motor4.setSpeed(100);
    motor4.run(FORWARD);
    previousDCBackMillis =currentMillis;
    }
  }
  

You need to structure your code as tasks (actually your current code is close to this already)
see Multi-tasking in Arduino
and then use millis() (or my millisDelay class) to implement the timers.
See How to write Timers and Delays in Arduino for examples.
multitaskingDiagramSmall

Do some searching/reading about state machines. You need to keep track of which state you are in. For example, when you come near an object, you want to do your stop, reverse, turn procedure. You don't want to do that every time through loop when distance is < 10, just detect the first time. It is similar to State Change Detection example in the IDE (File->examples->02.digital->State Change Detection)

A state machine would be the proper solution for this. You should be able to do something like this:

#define HALT_TIME 2000
#define BACK_TIME 2000
#define TURN_TIME 8000

enum ApplicationState {
  Drive,
  Halt,
  Back,
  Turn
};

static enum ApplicationState currentApplicationState = Drive;

static unsigned long t = 0;

static bool sensorTriggered() {
  // TODO return true if too close, return false otherwise.
}

static void funnyFace() {
  // TODO funny face and buzzer
}

static void driveHandler() {
  // TODO do whatever the car is supposed to do normally
  
  if (sensorTriggered()) {
    t = millis();
    currentApplicationState = Halt;
  }
}

static void haltHandler() {

  static bool shouldShowFunnyFace = true;

  // TODO stop the motors
  
  if (shouldShowFunnyFace) {
    funnyFace();
    shouldShowFunnyFace = false;
  }
  
  if (millis() - t > HALT_TIME) {
    t = millis();
    currentApplicationState = Back;
    shouldShowFunnyFace = true;
  }
}

static void backHandler() {
  // TODO drive backwards

  if (millis() - t > BACK_TIME) {
    t = millis();
    currentApplicationState = Turn;
  }
}

static void turnHandler() {
  // TODO turn the car

  if (millis() - t > TURN_TIME) {
    currentApplicationState = Drive;
  }
}

void setup() {
  
}

void loop() {

  switch (currentApplicationState) {
    
    case Drive:
    driveHandler();
    break;

    case Halt:
    haltHandler();
    break;

    case Back:
    backHandler();
    break;

    case Turn:
    turnHandler();
    break;
  }
}

For more info on state machines see:

1 Like

hey Nicola,

thank you, this is very helpful, however, I tried to implement it and I havnt been able to make it work.. so right now. the motors does not work at all... can you help me to look at my code?
Thank you..

#define HALT_TIME 2000
#define BACK_TIME 2000
#define TURN_TIME 8000
#define trigPin A1
#define echoPin A0
#include <LiquidCrystal.h>
#include <NewTone.h>
#include<AFMotor.h>

long duration;
int distance;
int distanceUltra;
int maximum = 100;
int buzzerPin = A2;
int dur = 100;

LiquidCrystal lcd(8, 9, 4, 7, 12, 13);
AF_DCMotor motor1(1,MOTOR34_1KHZ);
AF_DCMotor motor2(2,MOTOR34_1KHZ);
AF_DCMotor motor3(3,MOTOR34_1KHZ);
AF_DCMotor motor4(4,MOTOR34_1KHZ);

enum ApplicationState {
  Drive,
  Halt,
  Back,
  Turn
};

static enum ApplicationState currentApplicationState = Drive;

static unsigned long t = 0;

static bool sensorTriggered() {
  // TODO return true if too close, return false otherwise.
  
  
  if (distance <= 10){return true;}
  
  else {return false;}


}

static void funnyFace() {
  // TODO funny face and buzzer
    lcd.setCursor(6,0);
    lcd.write("x");
    lcd.setCursor(8,0);
    lcd.write("x");
    lcd.setCursor(7,1);
    lcd.write("o");
    randSound(maximum);
}

static void driveHandler() {
  // TODO do whatever the car is supposed to do normally
    motor1.setSpeed(100);
    motor1.run(BACKWARD);
    motor2.setSpeed(100);
    motor2.run(BACKWARD);
    motor3.setSpeed(100);
    motor3.run(BACKWARD);
    motor4.setSpeed(100);
    motor4.run(BACKWARD);
  if (sensorTriggered()) {
    t = millis();
    currentApplicationState = Halt;
  }
}

static void haltHandler() {

  static bool shouldShowFunnyFace = true;

  // TODO stop the motors
    motor1.setSpeed(0);
    motor1.run(RELEASE);
    motor2.setSpeed(0);
    motor2.run(RELEASE);
    motor3.setSpeed(0);
    motor3.run(RELEASE);
    motor4.setSpeed(0);
    motor4.run(RELEASE);
  
  if (shouldShowFunnyFace) {
    funnyFace();
    shouldShowFunnyFace = false;
  }
  
  if (millis() - t > HALT_TIME) {
    t = millis();
    currentApplicationState = Back;
    shouldShowFunnyFace = true;
  }
}

static void backHandler() {
  // TODO drive backwards
    motor1.setSpeed(100);
    motor1.run(FORWARD);
    motor2.setSpeed(100);
    motor2.run(FORWARD);
    motor3.setSpeed(100);
    motor3.run(FORWARD);
    motor4.setSpeed(100);
    motor4.run(FORWARD);
  if (millis() - t > BACK_TIME) {
    t = millis();
    currentApplicationState = Turn;
  }
}

static void turnHandler() {
  // TODO turn the car
    motor1.setSpeed(100);
    motor1.run(FORWARD);
    motor2.setSpeed(100);
    motor2.run(FORWARD);
    motor3.setSpeed(100);
    motor3.run(BACKWARD);
    motor4.setSpeed(100);
    motor4.run(BACKWARD);
  if (millis() - t > TURN_TIME) {
    currentApplicationState = Drive;
  }
}

void setup() {
  Serial.begin(9600);
  lcd.begin(16,2);
  pinMode (trigPin, OUTPUT);
  pinMode (echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
}

void loop() {
  Serial.print("distance:");
  Serial.println(distance);
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  Serial.println(sensorTriggered());
  duration = pulseIn(echoPin, HIGH);
  distance = duration*0.034/2;

  
  switch (currentApplicationState) {
    
    case Drive:
    driveHandler();
    break;

    case Halt:
    haltHandler();
    break;

    case Back:
    backHandler();
    break;

    case Turn:
    turnHandler();
    break;
  }
}


void randSound(int maximum){
  NewTone(buzzerPin, random(maximum, 10*maximum),dur);
  delay(maximum);
  noNewTone(buzzerPin);
}

Try to print the currentApplicationState variable to check if it's stuck in a case for some reason. Otherwise it might be that the AFMotor library doesn't like the repeated calls. If that's the case you can modify the code like this:

#define HALT_TIME 2000
#define BACK_TIME 2000
#define TURN_TIME 8000

enum ApplicationState {
  BeginDrive,
  Drive,
  BeginHalt,
  Halt,
  BeginBack,
  Back,
  BeginTurn,
  Turn
};

static enum ApplicationState currentApplicationState = BeginDrive;

static unsigned long t = 0;

static bool sensorTriggered() {
  // TODO return true if too close, return false otherwise.
}

static void funnyFace() {
  // TODO funny face and buzzer
}

static void beginDriveHandler() {
  // TODO move motor handling code here

  currentApplicationState = Drive;
}

static void driveHandler() {
  if (sensorTriggered()) {
    t = millis();
    currentApplicationState = BeginHalt;
  }
}

static void beginHaltHandler() {
  // TODO move motor handling code here

  // We can now move funnyFace() here, since it should only be invoked once
  funnyFace();
  currentApplicationState = Halt;
}

static void haltHandler() {
  if (millis() - t > HALT_TIME) {
    t = millis();
    currentApplicationState = BeginBack;
  }
}

static void beginBackHandler() {
  // TODO move motor handling code here

  currentApplicationState = Back;
}

static void backHandler() {
  if (millis() - t > BACK_TIME) {
    t = millis();
    currentApplicationState = BeginTurn;
  }
}

static void beginTurnHandler() {
  // TODO move motor handling code here

  currentApplicationState = Turn;
}

static void turnHandler() {
  if (millis() - t > TURN_TIME) {
    currentApplicationState = BeginDrive;
  }
}

void setup() {
  
}

void loop() {

  switch (currentApplicationState) {

    case BeginDrive:
    beginDriveHandler();
    break;
    
    case Drive:
    driveHandler();
    break;

    case BeginHalt:
    beginHaltHandler();
    break;

    case Halt:
    haltHandler();
    break;

    case BeginBack:
    beginBackHandler();
    break;

    case Back:
    backHandler();
    break;

    case BeginTurn:
    beginTurnHandler();
    break;

    case Turn:
    turnHandler();
    break;
  }
}

We add states that are only called once every cycle to set the speed. If you prefer you can also do what I did with the funnyFace function in my previous post.

Hi Nicola,

I am sorry to bother you again.. I tried this code but the motor still does not turn.. I also tried to print out currentApplicationState, and the states are switched correctly, however, the motor just does not turn... what could be the problem here?

#define HALT_TIME 2000
#define BACK_TIME 2000
#define TURN_TIME 8000
#define trigPin A1
#define echoPin A0
#include <LiquidCrystal.h>
#include <NewTone.h>
#include<AFMotor.h>

long duration;
int distance;
int distanceUltra;
int maximum = 100;
int buzzerPin = A2;
int dur = 100;

LiquidCrystal lcd(8, 9, 4, 7, 12, 13);
AF_DCMotor motor1(1,MOTOR34_1KHZ);
AF_DCMotor motor2(2,MOTOR34_1KHZ);
AF_DCMotor motor3(3,MOTOR34_1KHZ);
AF_DCMotor motor4(4,MOTOR34_1KHZ);

enum ApplicationState {
  BeginDrive,
  Drive,
  BeginHalt,
  Halt,
  BeginBack,
  Back,
  BeginTurn,
  Turn
};

static enum ApplicationState currentApplicationState = BeginDrive;

static unsigned long t = 0;

static bool sensorTriggered() {
  // TODO return true if too close, return false otherwise.
  if (distance <= 10){return true;}
  
  else {return false;}
}

static void funnyFace() {
  // TODO funny face and buzzer
    lcd.setCursor(6,0);
    lcd.write("x");
    lcd.setCursor(8,0);
    lcd.write("x");
    lcd.setCursor(7,1);
    lcd.write("o");
    randSound(maximum);
}

static void beginDriveHandler() {
  // TODO move motor handling code here
    motor1.setSpeed(100);
    motor1.run(BACKWARD);
    motor2.setSpeed(100);
    motor2.run(BACKWARD);
    motor3.setSpeed(100);
    motor3.run(BACKWARD);
    motor4.setSpeed(100);
    motor4.run(BACKWARD);
  currentApplicationState = Drive;
  
}

static void driveHandler() {
  if (sensorTriggered()) {
    t = millis();
    currentApplicationState = BeginHalt;
  }
}

static void beginHaltHandler() {
  // TODO move motor handling code here
    motor1.setSpeed(0);
    motor1.run(RELEASE);
    motor2.setSpeed(0);
    motor2.run(RELEASE);
    motor3.setSpeed(0);
    motor3.run(RELEASE);
    motor4.setSpeed(0);
    motor4.run(RELEASE);
  // We can now move funnyFace() here, since it should only be invoked once
  funnyFace();
  currentApplicationState = Halt;
}

static void haltHandler() {
  if (millis() - t > HALT_TIME) {
    t = millis();
    currentApplicationState = BeginBack;
  }
}

static void beginBackHandler() {
  // TODO move motor handling code here
    motor1.setSpeed(100);
    motor1.run(FORWARD);
    motor2.setSpeed(100);
    motor2.run(FORWARD);
    motor3.setSpeed(100);
    motor3.run(FORWARD);
    motor4.setSpeed(100);
    motor4.run(FORWARD);
  currentApplicationState = Back;
}

static void backHandler() {
  if (millis() - t > BACK_TIME) {
    t = millis();
    currentApplicationState = BeginTurn;
  }
}

static void beginTurnHandler() {
  // TODO move motor handling code here
    motor1.setSpeed(100);
    motor1.run(FORWARD);
    motor2.setSpeed(100);
    motor2.run(FORWARD);
    motor3.setSpeed(100);
    motor3.run(BACKWARD);
    motor4.setSpeed(100);
    motor4.run(BACKWARD);
  currentApplicationState = Turn;
}

static void turnHandler() {
  if (millis() - t > TURN_TIME) {
    currentApplicationState = BeginDrive;
  }
}

void setup() {
  Serial.begin(9600);
  lcd.begin(16,2);
  pinMode (trigPin, OUTPUT);
  pinMode (echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
}

void loop() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  //Serial.println(sensorTriggered());
  duration = pulseIn(echoPin, HIGH);
  distance = duration*0.034/2;
  switch (currentApplicationState) {

    case BeginDrive:
    beginDriveHandler();
    break;
    
    case Drive:
    driveHandler();
    break;

    case BeginHalt:
    beginHaltHandler();
    break;

    case Halt:
    haltHandler();
    break;

    case BeginBack:
    beginBackHandler();
    break;

    case Back:
    backHandler();
    break;

    case BeginTurn:
    beginTurnHandler();
    break;

    case Turn:
    turnHandler();
    break;
  }

  Serial.println(currentApplicationState);
}
void randSound(int maximum){
  NewTone(buzzerPin, random(maximum, 10*maximum),dur);
  delay(maximum);
  noNewTone(buzzerPin);
}

@precisioncollective
Please stop opening new topics for the same problem. 2 of them have been merged into this one

Please do NOT cross post / duplicate as it wastes peoples time and efforts to have more than one post for a single topic.

Continued cross posting could result in a time out from the forum.

Could you also take a few moments to Learn How To Use The Forum.

Other general help and troubleshooting advice can be found here.
It will help you get the best out of the forum in the future.

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.