State machine example – Golf ball collector

State table whiteboard snapshots:

Code:

//
// State table example for Arduino Nano - Golf Ball Collector
// Written by Ted Burke - last updated 22-11-2018
//
 
int state = 1;
int step_delay = 10;
 
// Remember which winding is active on each stepper
int active_winding_left = 1;
int active_winding_right = 1;
 
// Function prototypes
void left_step_forward();
void right_step_forward();
double distance_front();
double distance_left();
double distance_right();
 
void setup()
{
    // Actuator control pins
    pinMode(2, OUTPUT); // LM winding 1
    pinMode(3, OUTPUT); // LM winding 2
    pinMode(4, OUTPUT); // LM winding 3
    pinMode(5, OUTPUT); // LM winding 4
     
    pinMode(6, OUTPUT); // RM winding 1
    pinMode(7, OUTPUT); // RM winding 2
    pinMode(8, OUTPUT); // RM winding 3
    pinMode(9, OUTPUT); // RM winding 4
     
    // Sensor pins
    pinMode(10, OUTPUT); // trigger pin for front rangefinder
    pinMode(11, INPUT);  // echo pin for front rangefinder
     
    //
    // FILL IN PINS FOR LEFT AND RIGHT RANGEFINDERS
    //
}
 
void loop()
{
    int steps;
    int rff, rfl, rfr;
     
    while(state == 1) // Forward (up the field)
    {
        // Actuators
        left_step_forward();
        right_step_forward();
         
        // Read sensors
        rff = distance_front();
        rfl = distance_left();
        rfr = distance_right();
         
        // Change state?
        if (rff < 2.0 && rfr > 1.5) state = 2;
        if (rff < 2.0 && rfr <= 1.5) state = 4;
    }
     
    steps = 0;
    while(state == 2) // Turn right
    {
        // Actuators
        left_step_forward(); // left motor forward, right motor stop
        delay(step_delay);
         
        // Count steps
        steps = steps + 1;
         
        // Change state?
        if (steps == 805) state = 3;
    }
     
    while(state == 3) // Return (down the field)
    {
        // Actuators
        left_step_forward();
        right_step_forward();
         
        // Read sensors
        rff = distance_front();
        rfl = distance_left();
        rfr = distance_right();
         
        // Change state?
        if (rff < 2.0 && rfl > 1.5) state = 4;
        if (rff < 2.0 && rfl <= 1.5) state = 2;
    }
     
    steps = 0;
    while(state == 4) // Turn left
    {
        // Actuators
        right_step_forward(); // left motor forward, right motor stop
        delay(step_delay);
         
        // Count steps
        steps = steps + 1;
         
        // Change state?
        if (steps == 805) state = 1;
    }   
}
 
void set_left_windings(int a, int b, int c, int d)
{
    digitalWrite(2, a);
    digitalWrite(3, b);
    digitalWrite(4, c);
    digitalWrite(5, d);
}
 
void left_step_forward()
{
    // Select next winding
    active_winding_left = active_winding_left + 1;
    if (active_winding_left == 5) active_winding_left = 1;
     
    // Set the state of all four windings
    if (active_winding_left == 1) set_left_windings(1, 0, 0, 0);
    if (active_winding_left == 2) set_left_windings(0, 1, 0, 0);
    if (active_winding_left == 3) set_left_windings(0, 0, 1, 0);
    if (active_winding_left == 4) set_left_windings(0, 0, 0, 1);
}
 
void set_right_windings(int a, int b, int c, int d)
{
    digitalWrite(6, a);
    digitalWrite(7, b);
    digitalWrite(8, c);
    digitalWrite(9, d);
}
 
void right_step_forward()
{
    // Select next winding
    active_winding_right = active_winding_right + 1;
    if (active_winding_right == 5) active_winding_right = 1;
     
    // Set the state of all four windings
    if (active_winding_right == 1) set_right_windings(1, 0, 0, 0);
    if (active_winding_right == 2) set_right_windings(0, 1, 0, 0);
    if (active_winding_right == 3) set_right_windings(0, 0, 1, 0);
    if (active_winding_right == 4) set_right_windings(0, 0, 0, 1);
}
 
double distance_front()
{
    // Declare variables
    double d;
    unsigned long t1, t2, duration;
     
    // Trigger pulse to front rangefinder
    digitalWrite(10, HIGH);
    delayMicroseconds(20);
    digitalWrite(10, LOW);
     
    // Measure the duration of the echo pulse
    while(digitalRead(11) == 0); // Wait for start of echo pulse
    t1 = micros();               // Remember start time of pulse
    while(digitalRead(11) == 1); // Wait while echo remains high
    t2 = micros();
    duration = t2 - t1;
     
    // Convert duration into distance in metres
    d = (duration * 1e-6 * 340.0) / 2.0;
     
    return d;
}
 
double distance_left()
{
    // Fill these in yourself
}
 
double distance_right()
{
    // Fill these in yourself
}
Advertisements
This entry was posted in Uncategorized. Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google+ photo

You are commenting using your Google+ account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s