Hexapod robot on an Arduino Uno

// Hexapod: a six-legged walker, twelve servos on a PCA9685 servo board.
//
//   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 twelve servos
//   button   pin 2 to ground (internal pull-up): press to walk or stand
//   legs     left front  hip on channel 0,  knee on 1
//            left middle hip on 2,          knee on 3
//            left rear   hip on 4,          knee on 5
//            right front hip on 6,          knee on 7
//            right middle hip on 8,         knee on 9
//            right rear  hip on 10,         knee on 11
//
// # The tripod gait
//
// A robot is stable when its feet on the ground make a triangle around its
// middle. Six legs split into two triangles that can take turns:
//
//   tripod A  left front, right middle, left rear
//   tripod B  right front, left middle, right rear
//
// Each step is three moves on one tripod while the other stays planted:
//
//   1. the tripod's knees lift its feet off the ground
//   2. its hips swing the feet forward, while the planted tripod's hips
//      slide back together, which pushes the body forward
//   3. the knees put the feet down
//
// Then the two tripods swap jobs. At no time are fewer than three feet on the
// ground, and the three are always spread front, middle and rear, so the
// robot never has to balance. This is how most insects walk, and it is the
// fastest gait a hexapod has.
//
// # Mirrored servos
//
// The servos on the right side face the other way from the ones on the left,
// 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 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 PCA9685 counts a 20 ms frame in 4096
// ticks, so that window is 111 to 492 ticks.
const int MIN_TICKS = 111;
const int MAX_TICKS = 492;

const int BUTTON = 2;

// Legs 0 to 5: left front, left middle, left rear, right front, right middle,
// right rear.
const char *LEG_NAMES[6] = {"left front", "left middle", "left rear", "right front", "right middle", "right rear"};
const int SIDE[6] = {+1, +1, +1, -1, -1, -1};
// Which tripod each leg is in: 0 is A, 1 is B.
const int TRIPOD[6] = {0, 1, 0, 1, 0, 1};

const int SWING = 20;  // how far a foot swings from its middle, degrees
const int LIFT = 35;   // how far a knee raises its foot, degrees
const int STEP_MS = 15;

// Where each leg is now: degrees forward of square and degrees lifted.
int forward[6] = {-SWING, -SWING, -SWING, -SWING, -SWING, -SWING};
int lifted[6] = {0, 0, 0, 0, 0, 0};

bool walking = false;
bool lastButton = HIGH;
int stepNumber = 0;

// Look at the button. A step takes most of a second, so this is called from
// inside glide() as well as from loop(): a press that came and went while a
// step was under way would otherwise be missed. Pressing never cuts a step
// short, because a robot stopped with three feet in the air would fall over;
// the walk ends when the step in progress does.
void checkButton() {
  bool button = digitalRead(BUTTON);
  if (lastButton == HIGH && button == LOW) {
    walking = !walking;
    Serial.println(walking ? "walking" : "standing");
  }
  lastButton = button;
}

void setAngle(int channel, int degrees) {
  degrees = constrain(degrees, 0, 180);
  pwm.setPWM(channel, 0, map(degrees, 0, 180, MIN_TICKS, MAX_TICKS));
}

void writeLegs() {
  for (int leg = 0; leg < 6; leg++) {
    setAngle(leg * 2, 90 + SIDE[leg] * forward[leg]);
    setAngle(leg * 2 + 1, 90 + SIDE[leg] * lifted[leg]);
  }
}

// Move every joint from where it is to the target, together, in ten small
// steps.
void glide(const int toForward[6], const int toLifted[6]) {
  int fromForward[6], fromLifted[6];
  for (int leg = 0; leg < 6; leg++) {
    fromForward[leg] = forward[leg];
    fromLifted[leg] = lifted[leg];
  }
  for (int step = 1; step <= 10; step++) {
    for (int leg = 0; leg < 6; 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);
    checkButton();
  }
}

// One step by one tripod: lift, swing forward while the other tripod slides
// back, put down.
void step(int tripod) {
  int toForward[6], toLifted[6];
  for (int leg = 0; leg < 6; leg++) {
    toForward[leg] = forward[leg];
    toLifted[leg] = lifted[leg];
  }
  for (int leg = 0; leg < 6; leg++) {
    if (TRIPOD[leg] == tripod) toLifted[leg] = LIFT;
  }
  glide(toForward, toLifted);

  for (int leg = 0; leg < 6; leg++) {
    toForward[leg] = TRIPOD[leg] == tripod ? SWING : -SWING;
  }
  glide(toForward, toLifted);

  for (int leg = 0; leg < 6; leg++) toLifted[leg] = 0;
  glide(toForward, toLifted);

  Serial.print("tripod ");
  Serial.print(tripod == 0 ? "A" : "B");
  Serial.println(" stepped");
}

// Stand: every foot down, every hip in the middle of its swing.
void stand() {
  int toForward[6] = {-SWING, -SWING, -SWING, -SWING, -SWING, -SWING};
  int toLifted[6] = {0, 0, 0, 0, 0, 0};
  glide(toForward, toLifted);
}

void setup() {
  pinMode(BUTTON, INPUT_PULLUP);
  Serial.begin(115200);
  if (!pwm.begin()) {
    Serial.println("no PCA9685 at 0x40: check SDA, SCL and VCC");
  }
  pwm.setPWMFreq(50);
  writeLegs();
  Serial.println("hexapod: standing. press the button to walk");
  delay(500);
}

void loop() {
  checkButton();

  if (walking) {
    step(stepNumber % 2);
    stepNumber++;
  } else if (stepNumber != 0) {
    // The step that was under way has finished with every foot down: settle
    // into the stand.
    stand();
    stepNumber = 0;
  }
}