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 know I should use a 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? could it be that the motor library is locked?
Thank you!
Code: Select all
#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>
static long duration;
static int distance;
static int distanceUltra;
static int maximum = 100;
static int buzzerPin = A2;
static int dur = 100;
LiquidCrystal lcd(8, 9, 4, 7, 12, 13);
AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(2, MOTOR12_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;
//t = millis();
}
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;
//t = millis();
}
static void haltHandler() {
if (millis() - t > HALT_TIME) {
Serial.println(currentApplicationState);
t = millis();
currentApplicationState = BeginBack;
}
//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;
//t = millis();
}
static void backHandler() {
if (millis() - t > BACK_TIME) {
Serial.println(currentApplicationState);
t = millis();
currentApplicationState = BeginTurn;
}
//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;
//t = millis();
}
static void turnHandler() {
if (millis() - t > TURN_TIME) {
//t = millis();
Serial.println(currentApplicationState);
currentApplicationState = BeginDrive;
}
//currentApplicationState = BeginDrive;
}
void setup() {
Serial.begin(9600);
lcd.begin(16,2);
// pinMode (trigPin, OUTPUT);
// pinMode (echoPin, INPUT);
pinMode(buzzerPin, OUTPUT);
currentApplicationState = BeginDrive;
t = 0;
}
void loop() {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
//Serial.println(sensorTriggered());
duration = pulseIn(echoPin, HIGH);
distance = (float)duration*0.017;
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);
}