I am new to Arudino and have been trying to learn from a) the SparkFun Inventor’s kit and its programming challenges, b) reading the 54 programming pages of this forum, and c) the websites of the work of Majenko and Nick Gammon. So avoiding delay() and state machines have been a way to learn (along with avoidance of (capital “S”) Strings in the IDE). The code below is a simple variation of the “autonomous” robot which is the final project in the Inventors Kit. Links to the SparkFun docs are in comments at the bottom of the code.
I wanted to have the “robot” flash lights when too close to an object, beep a “backup warning” (like a semi-truck!) and indicate its left turn when avoiding obstacles, all the while continuing to operate the motors. So this was an exercise in avoiding delay() and implementing a simple finite-state-machine.
The code works – although I need to fine tune the timing and pre-defined distances for each state. (it really needs an “if running on carpet()” function !!). But I have two questions arising from the experience.
First, there are still two delay() lines in the code. One at the end of loop() to force a wait of 50ms before another reading of the distance sensor and one in the getDistance() function to set the delay between the outgoing and return ultrasonic pulses. The first COULD be avoided by a Millis() routine like I have used to time the TURN state. But I am not sure about the second one and whether there is anything to be gained avoiding either of these. Do these two situation show that selective use of delay() is justified?
Second, for this project I have added four LEDs and a buzzer thus approaching the limit of my Uno’s available pins and I used two analog pins for digital purposes. No problem. All works. But when the Arudino is powered up and before the program loads the two LEDs on the analog pins light up. They go off as soon as the program loads (perhaps when set to OUTPUT?). I would be interested in a simple explanation of why they light up with no code loaded (or even a simple blank dummy program loaded).
[code]
/*
Adapted from SparkFun Inventor’s Kit Circuit 5C - Autonomous Robot
Original SparkFun comments etc at bottom of code.
Mods DW june/july 2022 - This revised code is completely free for any use.
1. Style changes (mostly as per Nick Gammon)
2. Created state machine in main Loop and removed blocking delay()s,
3. condensed motor driving functions into one function w/extra parameters
4. added sounds and lights for warning when close to obstacle, backup buzzer, ..
.. and turn signal, all of which avoid delay() and operate while moving
5. added an additional state CHECK to allow more turning if still blocked
6. TBA.... more warnings using blue and green LEDs .......
*/
// **************************** Pin constants
const int RT_A1 = 13; //control pin A1 on the motor driver for the right motor
const int RT_A2 = 12; //control pin A2 on the motor driver for the right motor
const int RT_APWM = 11; //speed control pin on the motor driver for the right motor
const int LF_B1 = 8; //control pin B1 on the motor driver for the left motor
const int LF_B2 = 9; //control pin B2 on the motor driver for the left motor
const int LF_BPWM = 10; //speed control pin on the motor driver for the left motor
const int TRIG_PIN = 6; //pins on the distance sensor
const int ECHO_PIN = 5;
const int YELLOW_LED_PIN = 4;
const int RED_LED_PIN = 2;
const int BLUE_LED_PIN = A4; //future use
const int GREEN_LED_PIN = A5; //future use
const int SWITCH_PIN = 7; //switch to turn the robot on and off
const int BUZZER_PIN = 3;
// **************************** robot behaviour constants
const byte STOP = 0; //To clarify code when motor is stopped
const byte DRIVE_SPEED = 100; //motor speed to drive forwards
const int BACKUP_SPEED = -100; //motor speed to drive backwards
const byte TURN_SPEED = 75; //speed for turning
const byte ALERT_ON_DISTANCE = 10; //distance to stop when close to obstacle
const byte ALERT_OFF_DISTANCE = 15; //distance to turn off red LED alert
const byte START_TURN_DISTANCE = 20; //distance to stop backing up and turn
const int WARNING_INTERVAL = 500; //flash/buzzer interval
// **************************** robot behaviour variables
float distance = 0; //stores distance to obstacle measured by the sensor
int turnTime = 4000; //initial time to turn to avoid obstacle
float currentMillis = 0; //used to time the turn and avoid using delay()
float previousMillis = 0; //used to time the turn and avoid using delay()
bool firstTime = true; //prevents redundant calls to motor functions
bool redWarning = false; //flag to turn on red alert of close obstacle
bool buzzWarning = false; //flag to buzz when backing up
bool turnWarning = false; //flag to turn on left turn indicator
// State Machine states put in enum for clarity of code
enum testState {FORWARD, BACKUP, TURN, CHECK} state;
void setup()
{
state = FORWARD; //initialize first state at startup
pinMode(TRIG_PIN, OUTPUT); //pin to send ultrasonic pulses out from the distance sensor
pinMode(ECHO_PIN, INPUT); //pin to sense when the pulses reflect back to the distance sensor
pinMode(SWITCH_PIN, INPUT_PULLUP); //switch to turn robot on/off
//set the motor control and other pins as outputs
pinMode(RT_A1, OUTPUT);
pinMode(RT_A2, OUTPUT);
pinMode(RT_APWM, OUTPUT);
pinMode(LF_B1, OUTPUT);
pinMode(LF_B2, OUTPUT);
pinMode(LF_BPWM, OUTPUT);
pinMode(BLUE_LED_PIN, OUTPUT); //for future use
pinMode(GREEN_LED_PIN, OUTPUT); //for future use
pinMode(RED_LED_PIN, OUTPUT); //flashes when obstacle too close
pinMode(YELLOW_LED_PIN, OUTPUT); //acts as left turn flashing signal
pinMode(BUZZER_PIN, OUTPUT); //for backup warning beep
}
void loop()
{
distance = getDistance(); //get distance of any obstacle ahead
if (digitalRead(SWITCH_PIN) == LOW) //if the switch is on
{
switch (state)
{
case FORWARD:
if (distance > ALERT_ON_DISTANCE) //no close object so drive ahead
{
if (firstTime) //flag from initialization/previous state - start motors only once
{
motor(DRIVE_SPEED, RT_A1, RT_A2, RT_APWM);
motor(DRIVE_SPEED, LF_B1, LF_B2, LF_BPWM);
firstTime = false;
}
}
else //a close object is detected - stop motors
{
motor(STOP, RT_A1, RT_A2, RT_APWM);
motor(STOP, LF_B1, LF_B2, LF_BPWM);
redWarning = true; //activate red flasher function (at end of switch(state))
firstTime = true; //flag to run BACKUP state motor only once
state = BACKUP; //switch to state BACKUP
}
break;
case BACKUP:
buzzWarning = true; //activate buzzer function (at end of switch(state))
if (firstTime) //flag passed from FORWARD case to reverse motors only once
{
motor(BACKUP_SPEED, RT_A1, RT_A2, RT_APWM); //NB: BACKUP_SPEED is a negative number
motor(BACKUP_SPEED, LF_B1, LF_B2, LF_BPWM);
firstTime = false;
}
if (distance >= ALERT_OFF_DISTANCE) //turn red flasher off if backed far enough away
redWarning = false;
if (distance >= START_TURN_DISTANCE) //turn buzzer off if far enough away and stop motors..
{ // .. then change to TURN state
buzzWarning = false;
motor(STOP, RT_A1, RT_A2, RT_APWM);
motor(STOP, LF_B1, LF_B2, LF_BPWM);
turnWarning = true; //turn on left turn indicator (at end of switch(state))
firstTime = true; //flag to run TURN state motor only once
turnTime = 4000; //set time for first turn attempt
previousMillis = millis(); //start time for turn delay in TURN state
state = TURN;
}
break;
case TURN:
currentMillis = millis(); //start turn timer
if (currentMillis - previousMillis <= turnTime) //still ticking
{
if (firstTime) //just start the motors once
{
motor( TURN_SPEED, RT_A1, RT_A2, RT_APWM);
motor(-TURN_SPEED, LF_B1, LF_B2, LF_BPWM);
firstTime = false;
}
}
else //time up
{
turnWarning = false; //turn off the direction indicator and stop motors ..
motor(STOP, RT_A1, RT_A2, RT_APWM);
motor(STOP, LF_B1, LF_B2, LF_BPWM);
state = CHECK; // .. and change state
}
break;
case CHECK:
if (distance < ALERT_ON_DISTANCE) //if an(other) object is still detected after turning
{
turnWarning = true; //reset all the start conditions (as from BACKUP) ..
firstTime = true;
turnTime = 2000; //.. but with shorter turn time
previousMillis = millis();
state = TURN; //TURN and then CHECK again
}
else //no obstacle after turn
{
firstTime = true; //reset starting conditions ..
turnWarning = false; // .. turn off turn signal ..
state = FORWARD; // .. and go forward again
}
break;
} // END of switch (state)
// ********* These 3 functions operate the flashers and buzzer while robot is moving
if (redWarning) //flash the red "too close" warning LED
redLEDWarning();
else
digitalWrite(RED_LED_PIN, LOW);
if (buzzWarning) //backup warning buzzer
backupBuzzer();
else
noTone(BUZZER_PIN);
if (turnWarning) //turn-signal light
turnSignal();
else
digitalWrite(YELLOW_LED_PIN, LOW);
} //END of if switch is on
else //if the switch is off then stop the motors
{
motor(STOP, RT_A1, RT_A2, RT_APWM);
motor(STOP, LF_B1, LF_B2, LF_BPWM);
}
delay(50); //wait 50 milliseconds between readings
} // END of loop()
// ******************************* FUNCTIONS
void redLEDWarning()
{
int interval = WARNING_INTERVAL;
float CurrMillis = millis();
static float PrevMillis = 0;
if (CurrMillis - PrevMillis >= interval)
{
if (digitalRead(RED_LED_PIN) == LOW)
digitalWrite(RED_LED_PIN, HIGH);
else
digitalWrite(RED_LED_PIN, LOW);
PrevMillis = CurrMillis;
}
}
void backupBuzzer()
{
int interval = WARNING_INTERVAL;
float CurrMillis = millis();
static float PrevMillis = 0;
if (CurrMillis - PrevMillis >= interval)
{
tone(BUZZER_PIN, 700, 100);
PrevMillis = CurrMillis;
}
}
void turnSignal()
{
int interval = WARNING_INTERVAL;
float CurrMillis = millis();
static float PrevMillis = 0;
if (CurrMillis - PrevMillis >= interval)
{
if (digitalRead(YELLOW_LED_PIN) == LOW)
digitalWrite(YELLOW_LED_PIN, HIGH);
else
digitalWrite(YELLOW_LED_PIN, LOW);
PrevMillis = CurrMillis;
}
}
void motor(int motorSpeed, int pin1, int pin2, int pwm) //function for driving the motors
{
if (motorSpeed > 0) //if the motor should drive forward (positive speed)
{
digitalWrite(pin1, HIGH); //set pin 1 to high
digitalWrite(pin2, LOW); //set pin 2 to low
}
else if (motorSpeed < 0) //if the motor should drive backward (negative speed)
{
digitalWrite(pin1, LOW); //set pin 1 to low
digitalWrite(pin2, HIGH); //set pin 2 to high
}
else //the motor speed = 0, so it should stop
{
digitalWrite(pin1, LOW); //set pin 1 to low
digitalWrite(pin2, LOW); //set pin 2 to low
}
analogWrite(pwm, abs(motorSpeed)); //motor direction is set, drive it at the passed speed
}
float getDistance() //returns the distance from the sensor
{
float echoTime; //stores time for a ping to bounce off an object
float calculatedDistance; //stores the distance calculated from the echo time
digitalWrite(TRIG_PIN, HIGH); //send out an ultrasonic pulse that's 10ms long
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
echoTime = pulseIn(ECHO_PIN, HIGH); //how long it took for the pulse to bounce back to the sensor
calculatedDistance = echoTime / 148.0; //distance of the object that reflected the pulse ..
// ..(half the bounce time multiplied by the speed of sound)
return calculatedDistance; //send back the distance that was calculated
}
/* This robot will drive around on its own and react to obstacles by backing up and turning to a new direction.
This sketch was adapted from one of the activities in the SparkFun Guide to Arduino.
Check out the rest of the book at
https://www.sparkfun.com/products/14326
This sketch was written by SparkFun Electronics, with lots of help from the Arduino community.
This code is completely free for any use.
View circuit diagram and instructions at: https://learn.sparkfun.com/tutorials/sparkfun-inventors-kit-experiment-guide---v41
Download drawings and code at: https://github.com/sparkfun/SIK-Guide-Code
*/
[/code]
