____ _ ____
| _ \ ___ | |__ ___/ ___| _ _ _ __ ___ ___
| |_) / _ \| '_ \ / _ \___ \| | | | '_ ` _ \ / _ \
| _ < (_) | |_) | (_) |__) | |_| | | | | | | (_) |
|_| \_\___/|_.__/ \___/____/ \__,_|_| |_| |_|\___/
____ _ _ _
/ ___|___ ___ | | _| |__ ___ ___ | | __
| | / _ \ / _ \| |/ / '_ \ / _ \ / _ \| |/ /
| |__| (_) | (_) | <| |_) | (_) | (_) | <
\____\___/ \___/|_|\_\_.__/ \___/ \___/|_|\_\
Simple state machine example
//
// State machine RoboSumo example
// Written by Ted Burke
// Last updated Wed 28-Mar-2023
//
int state = 1;
void setup()
{
// Motor control digital outputs
pinMode(2, OUTPUT); // LM forward
pinMode(3, OUTPUT); // LM reverse
pinMode(4, OUTPUT); // RM forward
pinMode(5, OUTPUT); // RM reverse
}
void loop()
{
int colour;
colour = analogRead(0); // read sensor voltage from A0
if (state == 1)
{
setMotorDirections(1, 1); // robot forward
if (colour > 512) state = 2;
}
else if (state == 2)
{
setMotorDirections(1, -1); // spin right for 750ms
delay(750);
state = 1;
}
}
void setMotorDirections(int left_direction, int right_direction)
{
if (left_direction > 0)
{
digitalWrite(2, HIGH); // LM forward
digitalWrite(3, LOW);
}
else if (left_direction < 0)
{
digitalWrite(2, LOW); // LM reverse
digitalWrite(3, HIGH);
}
else
{
digitalWrite(2, LOW); // LM stop
digitalWrite(3, LOW);
}
if (right_direction > 0)
{
digitalWrite(4, HIGH); // RM forward
digitalWrite(5, LOW);
}
else if (right_direction < 0)
{
digitalWrite(4, LOW); // RM reverse
digitalWrite(5, HIGH);
}
else
{
digitalWrite(4, LOW); // RM stop
digitalWrite(5, LOW);
}
}