Arduino quadruped robot on an Arduino Uno

// Quadruped robot: a four-legged walker with eight servos on a PCA9685
// servo board, walking a creep gait. Each leg has a hip that swings it
// forward and back and a knee that lifts the foot.
//
//   PCA9685  SDA to A4, SCL to A5, VCC to the Uno's 5V, GND to GND
//            V+ from a separate 5 V supply for the eight servos
//   legs     front left   hip on channel 0, knee on 1
//            front right  hip on 2, knee on 3
//            rear left    hip on 4, knee on 5
//            rear right   hip on 6, knee on 7
//
// THE CREEP GAIT. A robot on four legs stands safely on any three of them,
// so this gait only ever lifts one leg at a time. Each step is two moves:
//
//   1. one leg lifts, swings 40 degrees forward and comes down, while the
//      other three feet stay planted and hold the body still;
//   2. all four feet slide 10 degrees back together, which pushes the body
//      forward over them.
//
// The legs take turns: rear left, front left, rear right, front right. The
// leg that steps is always the one furthest back, so after four steps every
// leg is where it started and the robot has moved one stride forward.
//
// MIRRORED SERVOS. The servos on the left side face the other way from the
// ones on the right, so "forward" is a bigger angle on the left and a
// smaller one on the right. SIDE[] holds that +1 or -1, and the gait itself
// only thinks in "degrees forward" and "degrees up".

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();  // address 0x40

// 544 to 2400 us is 0 to 180 degrees, the Arduino Servo library's window,
// which is what the servos here answer to. The PCA9685 counts each 20 ms
// frame in 4096 ticks, so in ticks that window is 111 to 492 (us * 4096 /
// 20000). Setting ticks with setPWM() is one write on the I2C bus, where
// writeMicroseconds() also reads the board's prescaler back every time.
const int MIN_TICKS = 111;
const int MAX_TICKS = 492;

// The four legs, in the order their channels are wired.
const int FRONT_LEFT = 0, FRONT_RIGHT = 1, REAR_LEFT = 2, REAR_RIGHT = 3;
const char *LEG_NAMES[4] = {"front left", "front right", "rear left", "rear right"};
const int HIP_CHANNEL[4] = {0, 2, 4, 6};
const int KNEE_CHANNEL[4] = {1, 3, 5, 7};
const int SIDE[4] = {+1, -1, +1, -1};  // left legs +1, right legs -1

// Calibration: on a real robot no servo horn sits exactly square. Stand the
// robot up, then nudge these (in degrees) until each leg points straight
// out and each foot just touches the floor.
int HIP_TRIM[4] = {0, 0, 0, 0};
int KNEE_TRIM[4] = {0, 0, 0, 0};

const int SWING = 40;  // how far a lifted leg swings forward, degrees
const int SHIFT = 10;  // how far the body moves per step: SWING / 4
const int LIFT = 40;   // how far a knee raises its foot, degrees
const int STEP_MS = 20;  // the pause after each tenth of a move

// Where each leg is now: degrees forward of square, and degrees lifted.
// The rear left leg is furthest back, so it steps first.
int forward[4] = {-5, 15, -15, 5};
int lifted[4] = {0, 0, 0, 0};

// The order the legs step in.
const int GAIT[4] = {REAR_LEFT, FRONT_LEFT, REAR_RIGHT, FRONT_RIGHT};

// One servo to an angle, 0 to 180 degrees.
void setAngle(int channel, int degrees) {
  degrees = constrain(degrees, 0, 180);
  pwm.setPWM(channel, 0, map(degrees, 0, 180, MIN_TICKS, MAX_TICKS));
}

int hipAngle(int leg) { return 90 + SIDE[leg] * forward[leg] + HIP_TRIM[leg]; }
int kneeAngle(int leg) { return 90 + SIDE[leg] * lifted[leg] + KNEE_TRIM[leg]; }

void writeLegs() {
  for (int leg = 0; leg < 4; leg++) {
    setAngle(HIP_CHANNEL[leg], hipAngle(leg));
    setAngle(KNEE_CHANNEL[leg], kneeAngle(leg));
  }
}

// Move every leg from where it is to the target, together, in ten small
// steps. Each one writes all eight servos over the I2C bus, then pauses
// STEP_MS. A smaller STEP_MS walks faster.
void glide(const int toForward[4], const int toLifted[4]) {
  int fromForward[4], fromLifted[4];
  for (int leg = 0; leg < 4; leg++) {
    fromForward[leg] = forward[leg];
    fromLifted[leg] = lifted[leg];
  }
  for (int step = 1; step <= 10; step++) {
    for (int leg = 0; leg < 4; leg++) {
      forward[leg] = fromForward[leg] + (toForward[leg] - fromForward[leg]) * step / 10;
      lifted[leg] = fromLifted[leg] + (toLifted[leg] - fromLifted[leg]) * step / 10;
    }
    writeLegs();
    delay(STEP_MS);
  }
}

void printHips(const char *what) {
  Serial.print(what);
  Serial.print("  hips");
  for (int leg = 0; leg < 4; leg++) {
    Serial.print(' ');
    Serial.print(hipAngle(leg));
  }
  Serial.println();
}

// Step 1: one leg up, forward and down.
void stepLeg(int leg) {
  int toForward[4], toLifted[4];
  for (int i = 0; i < 4; i++) {
    toForward[i] = forward[i];
    toLifted[i] = lifted[i];
  }
  toLifted[leg] = LIFT;  // up
  glide(toForward, toLifted);
  toForward[leg] += SWING;  // forward
  glide(toForward, toLifted);
  toLifted[leg] = 0;  // down
  glide(toForward, toLifted);
  Serial.print(LEG_NAMES[leg]);
  printHips(" steps");
}

// Step 2: every foot slides back, so the body moves forward.
void shiftBody() {
  int toForward[4];
  for (int leg = 0; leg < 4; leg++) toForward[leg] = forward[leg] - SHIFT;
  glide(toForward, lifted);
  printHips("body forward");
}

void setup() {
  Serial.begin(115200);
  if (!pwm.begin()) {
    Serial.println("no PCA9685 at 0x40: check SDA, SCL and VCC");
  }
  pwm.setPWMFreq(50);  // the servo frame, 50 times a second
  writeLegs();
  Serial.println("quadruped: standing. hips are FL FR RL RR");
  printHips("stand");
  delay(1000);
}

void loop() {
  for (int i = 0; i < 4; i++) {
    stepLeg(GAIT[i]);
    shiftBody();
  }
}