Quadruped code

quad.arduino

#include <Servo.h>

struct ServoMove {
  char cmd[3];
  int  angle;
  int  wait;
};
 
Servo joints[8];
// Front then back; right then left; hip then leg
int pins[] = {2,3,4,5,9,10,6,7};
// min, home, max positions
int bot[] = {  0, 55, 20,135,  0,  0,  0,  0};
int med[] = { 80, 90, 85,168, 80, 80, 80, 80};
int top[] = {170,160,170,220,170,170,170,170};

int idle = 2000;

ServoMove walk[] = {
  {{'F','D','L'},0,idle},
  {{'B','G','L'},100,0},
  {{'F','D','H'},0,0},
  {{'B','D','H'},100,idle},
  {{'F','D','L'},100,0},
  {{'B','D','L'},100,idle},
  {{'B','G','H'},100,0},
  {{'F','D','H'},100,idle},
  {{'B','G','L'},0,0},
  {{'F','G','L'},0,idle},
  {{'F','G','H'},100,0},
  {{'B','G','H'},0,idle},
  {{'F','G','L'},0,0},
  {{'B','D','L'},0,idle},
  {{'B','D','H'},0,0},
  {{'F','G','H'},100,idle},//home
  {{'B','D','L'},0,0},
  {{'F','G','L'},0,idle}
};

ServoMove w1 = {{'F','D','L'},110,100};
ServoMove w2 = {{'F','D','L'},120,100};
ServoMove wiggle[] = {w1,w2,w1,w2,w1,w2};

void setup() 
{ 
  Serial.begin(115200);
  delay(200);
  for (int joint=0; joint<8; joint++) {
    joints[joint].attach(pins[joint]);
    if (joint % 2) {
      joints[joint].write(med[joint]);
    } else {
      joints[joint].write(bot[joint]);
    }
  }
} 
 
 
void loop() 
{
  ServoMove user;
  if (Serial.available() >= 3) {
    Serial.readBytes(user.cmd,3);
    if (user.cmd[0] == '*') {
      for (int i=0; i<(sizeof(walk)/sizeof(ServoMove)); i++) {
        doMove(walk[i]);
      }
    } else {
      user.angle = Serial.parseInt();
      user.wait = 0;
      doMove(user);
    }
  }
}

void doMove(struct ServoMove aMove) {
    boolean valid=true;
    int joint = 0;
    Serial.println(aMove.cmd[0]);
    switch (aMove.cmd[0]) {
      case 'f':
      case 'F':
        break;
      case 'b':
      case 'B':
        joint += 4;
        break;
      default:
        valid = false;      
    }
    Serial.println(aMove.cmd[1]);
    switch (aMove.cmd[1]) {
      case 'd':
      case 'D':
        break;
      case 'g':
      case 'G':
        joint += 2;
        break;
      default:
        valid = false;      
    }
    Serial.println(aMove.cmd[2]);
    switch (aMove.cmd[2]) {
      case 'h':
      case 'H':
        break;
      case 'l':
      case 'L':
        joint += 1;
        break;
      default:
        valid = false;      
    }
    if (valid) {
      Serial.print("joint:");
      Serial.println(joint);
      Servo *output = &joints[joint];
      int logicalAngle = aMove.angle;
      int mappedAngle = 0;
      if (logicalAngle < 50) {
        mappedAngle = map(logicalAngle,0,50,bot[joint],med[joint]);
      } else {
        mappedAngle = map(logicalAngle,50,100,med[joint],top[joint]);
      }
      Serial.println(mappedAngle);
      output->write(mappedAngle);
    }
    delay(aMove.wait);
}