Maus LearningBot - Family Challenge One (State Machine)

Let's Make Robots
, , , ,
1

MausBot.zip (3146Bytes)

Introduction

We have put together our learning robot and have switched out our Arduino stack consisting of the Arduinio, screw shield and arduino motor shield for one of Ro-Bot-X's Robot Builder's Rduino v2 boards.  Now that things are hooked up to the new board, we decided to go forth with a simple programming challenge.

The Challenge

Our first programming challenge is for each of us to program the robot to move foward towards a wall and try to stop the closest to the wall without knocking over the blocks used to create it.

Both boys are currently working out their code to solve the challenge.  I happen to have some code that works okay and thought I'd post it here for both discussion and evaluation.  I know there are some things I want to change right away, but I figure with the programming blogs, this is a good example to work with.

Hardware Issues

First off, I'd like to say that my robot does not drive straight.  It has a definate curve to the left when I power each motor at the same speed.  I "fixed" this by adding an adjustment to the motor speed which should really be adjusted by what speed the motors are moving.  I plan on modifying my code to use a PID algorithm to help straighten out this annoyance.

For reference, my constants for the motor issues are:

#define NORMAL_SPEED  128
#define CREEP_SPEED   50
#define RIGHT_ADJUST  0
#define LEFT_ADJUST   15

where the 2 speed constants are the normal and creep up to the wall speeds and the adjust constants attempt to straighten out the robot movement.

MausBot Files

Attached is the Arduino project with my code.  You'll see libraries in there for the Ping sensor (corrected from previous blogs), Encoders (which I'm not using at the moment), and a header file containing a debug macro.

I'm a little rusty with my C/C++, so be kind! :)

State Machine Mayhem

I descided to use a state magine for this challenge.  I haven't written a state machine in some time, so I thought this would be a good opportunity to jump in and get my brain working.  First up, we have the definitions of all the states, an array of functions to perform those states and a structure with the state data I wish to use.

enum Actions {
  INITIALIZE = 0,
  MOVE,
  DRIVE,
  MOVE_STOP,
  READ_SENSOR,
  WAIT,
  DONE
};

void (*mausStateAction[7])(void) = {
  initialize,
  move,
  drive,
  moveStop,
  readSensor,
  wait,
  done
};

struct mausSM {
  Actions state;
  long    cm;
  unsigned long waitUntil;
  uint8_t motor_direction;
  uint8_t motor_speed;
  uint8_t done;
} mausState;

I could have coded the state machine using C++ classes, but for this first attempt, I wanted to keep it simple.  Here's a quick summary of the states:

  • INITIALIZE: initialize the variables in the state machine
  • MOVE: start the motors in the appropriate direction
  • DRIVE: logic to determine how fast to move given the distance the sensor is providing us
  • MOVE_STOP: stop the motors
  • READ_SENSOR: get an average reading from the sensor to determine distance
  • WAIT: wait for a determined amount of time.  I was going to use this but did not.
  • DONE: all done, do nothing

The arduino loop() becomes very short when you use function pointers with my basic state machine:

void loop() {
  (*mausStateAction[mausState.state])();
}

As you can see, it's just a call to the current state function in our array which call the various state methods as shown below:

void initialize() {
  DebugCode(
    Serial.println("initialize");
  )
 
  mausState.motor_direction = MOTOR_FORWARD;
  mausState.motor_speed     = 0;
  mausState.cm              = 1000;
  mausState.waitUntil       = 0;
  mausState.done            = 0;
  mausState.state           = READ_SENSOR;
}

void move() {
  DebugCode(
    Serial.println("move");
  )
 
  mausState.state = READ_SENSOR;
 
  if (mausState.cm < STOP_DISTANCE) {
    mausState.state = MOVE_STOP;
  } else if (mausState.cm < SLOW_DISTANCE) {
    if (mausState.motor_speed != CREEP_SPEED) {
      mausState.motor_speed     = CREEP_SPEED;
      mausState.state           = DRIVE;
    }
  } else {
    if (mausState.motor_speed != NORMAL_SPEED) {
      mausState.motor_speed     = NORMAL_SPEED;
      mausState.state           = DRIVE;
    }
  }
}

void drive() {
  DebugCode(
    Serial.print("drive speed=");
    Serial.print(mausState.motor_speed);
    Serial.print(", dir=");
    Serial.println(mausState.motor_direction);
  )
 
  digitalWrite(PIN_MOTOR_R_DIR, mausState.motor_direction);
  digitalWrite(PIN_MOTOR_L_DIR, mausState.motor_direction);
 
  analogWrite(PIN_MOTOR_R_PWM, mausState.motor_speed + RIGHT_ADJUST);
  analogWrite(PIN_MOTOR_L_PWM, mausState.motor_speed + LEFT_ADJUST);

  mausState.state = READ_SENSOR;
}

void moveStop() {
  DebugCode(
    Serial.println("moveStop");
  )
 
  digitalWrite(PIN_MOTOR_R_DIR, MOTOR_FORWARD);
  digitalWrite(PIN_MOTOR_L_DIR, MOTOR_FORWARD);
 
  analogWrite(PIN_MOTOR_R_PWM, 0);
  analogWrite(PIN_MOTOR_L_PWM, 0);
 
  mausState.state = DONE;
}

void readSensor() {
  long total = 0;
 
  for (uint8_t x = 0; x < 3; x++) {
    sensor.pulse();
    total += sensor.getCentimeters();
    delay(10);
  }
 
  mausState.cm = total / 3;
 
  DebugCode(
    Serial.print("readSensor cm=");
    Serial.println(mausState.cm);
  )
 
  mausState.state = MOVE;
}

void wait() {
  DebugCode(
    Serial.print("Wait until=");
    Serial.print(mausState.waitUntil);
    Serial.print(", millis=");
    Serial.println(millis());
  )
 
  if (millis() >= mausState.waitUntil) {
    mausState.state = READ_SENSOR;
  }
}

void done() {
  DebugCode(
    if (!mausState.done) {
      Serial.println("Done");
      mausState.done = 1;
    }
  )
}

The code in each state function does one small task and was relatively simple to code.  I had to take the average of 3 readings in the READ_SENSOR state because I would occasionally get a really short reading from the Ping sensor and the robot would stop before it was time.

Conclusion

So there you have it.  I'll try to post a video of the robot in action as soon as I can.

I welcome any and all input, questions and discusion of all flaws and poor programming on my part as a way to get back into real programming instead of the stuff I have to do at work.  Sometimes it's best to have an example instead of some quick tip or technique.

Thanks for your time!

Maus

2

If you use the term state

If you use the term state machine I would expect terms like State, Transition, Input, Output and other elements that make all together a state machine. 

Every transition from one state to the other is triggered i.e. by a specific input event. If no trigger event occurs no state change occurs.

In your example the Arduino loop would trigger the sensor readings. Depending on the reading i.e. from the Ping))) the input events MOVE or MOVE_STOP are calculated and feeded into the state machine. This must lead to a transition from STOPPED to MOVING or from MOVING to STOPPED.

The transition from STOPPED to MOVING emits the MOTOR_FORWARD while the transition from MOVING to STOPPED emits the MOTOR_STOP event. In the Arduino code you listen to these emitted events and act accordingly - turn motors on or off.

STOPPED > [ in:MOVE, out:MOTOR_FORWARD ] > MOVING

|                                         |

<      [in:MOVE_STOP, out:MOTOR_STOP]     <

It is important that you leave the states unless the physical circumstances have changed. As soon as your Ping))) sees an obstacle it sends an MOVE_STOP. This remains until the Ping))) sees nothing anymore. Then MOVE is sent. So you need to cache the event. 
In your loop there is no caching like that. So you make state changes that effectively are not nessesary.

Using state machines I find it a good approach for a lot of LMR robots. So I find your Blog entry useful. You could translate it into a Tutorial/Tip one day…