Thing-a-day, Day 22: Walking bot

#include <Servo.h>
#include <SimpleMessageSystem.h>
 
/******************************************************************************************************
 *  Walkbot: a four-legged walking bot
 *
 * by Matt Mets
 * Created 23 Feb. 2008
 *  
 * Trigger some servos in a pattern to make the robot 'walk'
 * Requires the Servo and SimpleMessageSystem libraries
 *
 * This code is released into the public domain.  Attribution is appreciated.
 *
******************************************************************************************************/
 
/***** Variable Definitions ***************************************************************************/
int statusPin =    13;                       // Status LED
Servo servoBL, servoBR, servoFL, servoFR;    // One servo per leg
 
typedef struct feetPositions_t
{
  int FrontLeft;
  int FrontRight;
  int BackLeft;
  int BackRight;
} feetPositions_t;
 
// The table of movements to go through.  Each row is a frame.  The columns correspond to servo
// positions of each feet, in the order FL, FR, BL, BR
feetPositions_t walk_forward[] = {{90, 90, 90, 90},
                                 {60, 90, 90, 90},
                                 {60, 120, 90, 90},
                                 {60, 120, 90, 120},
                                 {60, 120, 60, 120},
                                 {90, 90, 90, 90},
                                 {90, 120, 90, 90},
                                 {60, 120, 90, 90},
                                 {60, 120, 60, 90},
                                 {60, 120, 60, 120}};    
 
// An alternate movement pattern
#if 0
feetPositions_t walk_forward[] = {{90, 90, 90, 90},
                                 {60, 90, 90, 90},
                                 {60, 90, 90, 120},
                                 {60, 120, 90, 120},
                                 {60, 120, 60, 120},
                                 {90, 90, 90, 90},
                                 {90, 120, 90, 90},
                                 {90, 120, 60, 90},
                                 {60, 120, 60, 90},
                                 {60, 120, 60, 120}};
#endif
 
/***** Functions ************************************************************************************/
void setup()
{
  servoBL.attach(2);
  servoBR.attach(3);
  servoFL.attach(4);
  servoFR.attach(5);
 
  pinMode(statusPin, OUTPUT);
 
  Serial.begin(9600);
}
 
// Allow control of the bot via serial commands
// The command expected is m xxx yyy zzz aaa
// where:
//   xxx - Front Left leg position, degrees (0-180)
//   yyy - Front Right leg position, degrees (0-180)
//   zzz - Back Left leg position, degrees (0-180)
//   aaa - Back Right leg position, degrees (0-180)
void do_serial_control()
{ 
  if(messageBuild())
  {
    char firstChar = messageGetChar();
    if(firstChar == 'm')
    {
      servoFL.write(messageGetInt());
      servoFR.write(messageGetInt());
      servoBL.write(messageGetInt());
      servoBR.write(messageGetInt());
    }
  }  
  Servo::refresh();
}
 
// Walk through each position in the sequence
void play_sequence(struct feetPositions_t* sequence, int length)
{ 
  int startTime;
 
  for(int step = 0; step < length; step++)
  {
    // Set the servos to the proper positions for this step
      servoFL.write(sequence[step].FrontLeft);
      servoFR.write(sequence[step].FrontRight);
      servoBL.write(sequence[step].BackLeft);
      servoBR.write(sequence[step].BackRight);
    // delay for some time
    startTime = millis();
    while(millis() - startTime < 500)
    {
      Servo::refresh();
    }
  }
 
}
 
void loop()
{
//  do_serial_control();
 
  play_sequence(walk_forward, 10);
}