Little Walking Robot - Arduio Sketch

The software which runs on the Arduino in the transmitter is actually fairly simple.  The robot is essentially just a wireless puppet - movement of the joysticks is translated directly to movement of the legs.  All the fancy moves the robot does - walking, posing, waving at people, rolling over - is performed by the operator, moving the robot's joysticks and changing the mixing algorithm with the mode select buttons.  The software simply samples the mode buttons and analog inputs from the joysticks, performs some simple digital debouncing and filtering, calculates the desired position of each servo, and transmits the servo position commands out on the serial port.

In the robot,the serial output from the receiving XBee module is fed to the serial input of an 8-channel Pololu Serial Servo Controller.  The controller has some configuration settings that let you adjust the center points and ranges on each servo, but I'm bypassing those and setting the position directly with the Set Position, Absolute command.  I prefer to do those calculations in the transmitter.

---

int stickX = 0;
int stickY = 0;
int stickX2 = 0;
int stickY2 = 0;

long avgtemp = 0L;  //teprary long for digital filtering

//for all servos, 3000 is center

int servoFRX = 3000; 
int servoFLX = 3000; 
int servoRRX = 3000; 
int servoRLX = 3000; 
int servoFRY = 3000; 
int servoFLY = 3000; 
int servoRRY = 3000; 
int servoRLY = 3000; 

int outputFRX;
int outputFLX;
int outputRRX;
int outputRLX;
int outputFRY;
int outputFLY;
int outputRRY;
int outputRLY;

//trim the center points on each servo
//require because the MG996R servos are so damn inaccurate

#define trimFLY 0     //plus is ccw (down),        minus is cw (up)
#define trimFLX 0     //plus is ccw (back/left),   minus is cw (front/right)
#define trimRLY -100  //plus is ccw (down),        minus is cw (up)
#define trimRLX 0     //plus is ccw (back/right),  minus is cw (front/left)
#define trimRRX 30    //plus is ccw (front/right), minus is cw (back/left)
#define trimRRY 0     //plus is ccw (up),          minus is cw (down)
#define trimFRX -10   //plus is ccw (front/left),  minus is cw (back/right)
#define trimFRY 150   //plus is ccw (up),          minus is cw (down)

int mode = 0;  //miximg mode

byte button0 = 0;
byte button1 = 0;
byte button2 = 0;
byte button3 = 0;

byte button0oldstat = 0;
byte button1oldstat = 0;
byte button2oldstat = 0;
byte button3oldstat = 0;

byte buttoncount = 0;
byte invertmode = 0;

void setup()                    // run once, when the sketch starts
{
  ADMUX &= 0x3F;                // use 3.3V supply as AREF
  Serial.begin(38400);
 
  delay(500);  //give the radio time to start working
}

void loop()                     // run over and over again
{ 
  button0 = digitalRead(2);  //D2 - brown wire
  button1 = digitalRead(3);  //D3 - green wire
  button2 = digitalRead(4);  //D4 - yellow wire
  button3 = digitalRead(5);  //D5 - orange wire
 
  //check to see if the button inputs have changed
  //we use a timer variable for digital debouncing
 
  if (  (button0oldstat != button0)||
        (button1oldstat != button1)||
        (button2oldstat != button2)||
        (button3oldstat != button3)||
        ((button0 == 1)&&
         (button1 == 1)&&
         (button2 == 1)&&
         (button3 == 1))) {
      buttoncount = 0;
  }
  else {
    if (buttoncount == 20) {
      if ((button0 == 0)&&
         (button1 == 1)&&
         (button2 == 1)&&
         (button3 == 1)) {
           invertmode = 1 - invertmode;
      }
      else if (button0 == 1) {
             mode = (1 - button1) + (1 - button2) * 2 + (1 - button3) * 4;
         }
    }
    if (buttoncount < 21) {
      buttoncount = buttoncount + 1;
    }
  }
 
  button0oldstat = button0;
  button1oldstat = button1;
  button2oldstat = button2;
  button3oldstat = button3;
 
  //read analog inputs
  //we do some digital noise filtering here

  avgtemp = stickX * 3L + (analogRead(0) - 512) * 256L;  //A0 - right X - orange wire
  stickX = avgtemp / 4; // right joystick x

  avgtemp = stickY * 3L - (analogRead(1) - 512) * 256L;  //A1 - right Y - yellow wire
  stickY = avgtemp / 4; // right joystick y

  avgtemp = stickX2 * 3L + (analogRead(2) - 512) * 256L;  //A2 - left X - green wire
  stickX2 = avgtemp / 4; // left joystick x

  avgtemp = stickY2 * 3L + (analogRead(3) - 512) * 256L;  //A3 - left Y - brown wire
  stickY2 = avgtemp / 4; // left joystick y
 
  //determine the servo positions based on the joystick inputs
  //there is no sequencing here, it's all just direct one-to-one mapping of joystick inputs
  //to servo outputs, with the mode variable controlling which mixing algorithm to use.
 
  switch (mode) {
    case 1:  //basic walk algorithm
      servoFRY = 2000 - (stickY / 32);
      servoRRY = 2000 + (stickY / 32);
      servoFLY = 4000 - (stickY / 32);
      servoRLY = 4000 + (stickY / 32);
      servoFRX = 3000 - (stickX / 24) - (stickY2 / 30) - (stickX2 / 30);
      servoRRX = 3000 + (stickX / 24) + (stickY2 / 30) - (stickX2 / 30);
      servoFLX = 3000 + (stickX / 24) - (stickY2 / 30) + (stickX2 / 30);
      servoRLX = 3000 - (stickX / 24) + (stickY2 / 30) + (stickX2 / 30);
      break;
    case 2:  //wave front legs
      servoFRY = 2500 + (stickY / 16);
      servoFLY = 3500 + (stickY2 / 16);
      servoFRX = 3000 + (stickX / 24);
      servoFLX = 3000 + (stickX2 / 24);
      servoRRY = 2000;  //down
      servoRLY = 4000;  //down
      servoRLX = 4375;  //forward/left
      servoRRX = 1625;  //forward/right
      break;
    case 3:  //all servos center - used for alignment checks
      servoFRY = 3000;
      servoFLY = 3000;
      servoFRX = 3000;
      servoFLX = 3000;
      servoRRY = 3000;
      servoRLY = 3000;
      servoRRX = 3000;
      servoRLX = 3000;
      break;
    case 4:  //body lean/shift/dance
      servoFRY = 2000 + (stickY / 32) + (stickX / 32);
      servoRRY = 2000 - (stickY / 32) + (stickX / 32);
      servoFLY = 4000 - (stickY / 32) + (stickX / 32);
      servoRLY = 4000 + (stickY / 32) + (stickX / 32);
      servoFRX = 3000 + (stickY2 / 32) + (stickX2 / 32);
      servoRRX = 3000 + (stickY2 / 32) - (stickX2 / 32);
      servoFLX = 3000 - (stickY2 / 32) + (stickX2 / 32);
      servoRLX = 3000 - (stickY2 / 32) - (stickX2 / 32);
      break;
  }
 
  if (invertmode == 1) {  //inverted?
    outputRRY = servoRLY;
    outputRLY = servoRRY;
    outputFRY = servoFLY;
    outputFLY = servoFRY;
    outputRRX = servoRLX;
    outputRLX = servoRRX;
    outputFRX = servoFLX;
    outputFLX = servoFRX;
  }
  else {
    outputFRX = 6000 - servoFRX;
    outputFLX = 6000 - servoFLX;
    outputRRX = 6000 - servoRRX;
    outputRLX = 6000 - servoRLX;
    outputFRY = servoFRY;
    outputFLY = servoFLY;
    outputRRY = servoRRY;
    outputRLY = servoRLY;
  }
     
  //tweak nonzero servo centerpoints
 
  outputFRX = outputFRX + trimFRX;
  outputFLX = outputFLX + trimFLX;
  outputRRX = outputRRX + trimRRX;
  outputRLX = outputRLX + trimRLX;
  outputFRY = outputFRY + trimFRY;
  outputFLY = outputFLY + trimFLY;
  outputRRY = outputRRY + trimRRY;
  outputRLY = outputRLY + trimRLY;
 
  //constrain to actual servo travel limits
 
  outputFRY = constrain(outputFRY,1000,5000);
  outputRRY = constrain(outputRRY,1000,5000);
  outputFLY = constrain(outputFLY,1000,5000);
  outputRLY = constrain(outputRLY,1000,5000);
  outputFRX = constrain(outputFRX,1000,5000);
  outputRRX = constrain(outputRRX,1000,5000);
  outputFLX = constrain(outputFLX,1000,5000);
  outputRLX = constrain(outputRLX,1000,5000);
 
  //transmit to robot
 
  Serial.write((byte)0x80);
  Serial.write((byte)0x01);
  Serial.write((byte)0x04);
  Serial.write((byte)0x00);
  Serial.write((byte)(outputFRY / 128));
  Serial.write((byte)(outputFRY % 128));
 
  Serial.write((byte)0x80);
  Serial.write((byte)0x01);
  Serial.write((byte)0x04);
  Serial.write((byte)0x01);
  Serial.write((byte)(outputFRX / 128));
  Serial.write((byte)(outputFRX % 128));
 
  Serial.write((byte)0x80);
  Serial.write((byte)0x01);
  Serial.write((byte)0x04);
  Serial.write((byte)0x02);
  Serial.write((byte)(outputRRY / 128));
  Serial.write((byte)(outputRRY % 128));
 
  Serial.write((byte)0x80);
  Serial.write((byte)0x01);
  Serial.write((byte)0x04);
  Serial.write((byte)0x03);
  Serial.write((byte)(outputRRX / 128));
  Serial.write((byte)(outputRRX % 128));
 
  Serial.write((byte)0x80);
  Serial.write((byte)0x01);
  Serial.write((byte)0x04);
  Serial.write((byte)0x04);
  Serial.write((byte)(outputRLX / 128));
  Serial.write((byte)(outputRLX % 128));
 
  Serial.write((byte)0x80);
  Serial.write((byte)0x01);
  Serial.write((byte)0x04);
  Serial.write((byte)0x05);
  Serial.write((byte)(outputRLY / 128));
  Serial.write((byte)(outputRLY % 128));
 
  Serial.write((byte)0x80);
  Serial.write((byte)0x01);
  Serial.write((byte)0x04);
  Serial.write((byte)0x06);
  Serial.write((byte)(outputFLX / 128));
  Serial.write((byte)(outputFLX % 128));
 
  Serial.write((byte)0x80);
  Serial.write((byte)0x01);
  Serial.write((byte)0x04);
  Serial.write((byte)0x07);
  Serial.write((byte)(outputFLY / 128));
  Serial.write((byte)(outputFLY % 128));
 
}