Robot arm inverse kinematics on an Arduino Uno

// Robot arm inverse kinematics: say where the claw should be, in
// millimeters, and the sketch works out the shoulder and elbow angles.
//
//   arm      BASE pin 3, SHOULDER pin 5, ELBOW pin 6, GRIP pin 9
//            VCC and GND from a separate 5 V supply, GND joined to the Uno's
//   knobs    target x (how far forward) on A0, target y (how high) on A1
//
// The arm is two links seen from the side: the upper arm, L1 long, from the
// shoulder to the elbow, and the forearm, L2 long, from the elbow to the
// claw. Measured from the shoulder, x is forward and y is up.
//
// FORWARD KINEMATICS is the easy way round: given the two link angles,
// where is the claw? With both angles measured up from level,
//
//     elbow = (L1 cos(upper), L1 sin(upper))
//     claw  = elbow + (L2 cos(fore), L2 sin(fore))
//
// INVERSE KINEMATICS is the way round a robot needs: given the claw's
// (x, y), which angles? The shoulder, the elbow and the claw make a
// triangle with sides L1, L2 and d, the straight line from shoulder to
// claw. The law of cosines gives the angle at the shoulder between d and
// the upper arm:
//
//     d = sqrt(x*x + y*y)
//     a = acos((L1*L1 + d*d - L2*L2) / (2 * L1 * d))
//     upper = atan2(y, x) + a          (the elbow above the line, not below)
//
// That puts the elbow at (L1 cos(upper), L1 sin(upper)), and the forearm
// simply points from there to the target. If d is longer than L1 + L2 the
// claw cannot reach, and acos would be handed a number past 1.
//
// THIS ARM'S SERVOS. On the desktop arm the shoulder servo leans the upper
// arm: 90 is upright and 180 leans fully forward, so the servo angle is
// 180 minus the upper arm's angle above level. The elbow servo sets the
// forearm through a side linkage: 90 is level and 180 points straight up,
// so the servo angle is 90 plus the forearm's angle above level.

#include <Servo.h>
#include <math.h>

Servo base, shoulder, elbow, claw;

// The two link lengths in millimeters, shoulder to elbow and elbow to claw.
// Measure your own arm and put its numbers here.
const float L1 = 72.0;
const float L2 = 84.0;

// The servos stop short of their ends so the arm cannot fold into itself.
const int LOW_END = 40;
const int HIGH_END = 160;

const float DEGREES = 180.0 / M_PI;

// Solve for a target. Returns false, and leaves the angles alone, when the
// target is out of reach or would need a servo past its limits.
bool solve(float x, float y, int &shoulderAngle, int &elbowAngle) {
  float d = sqrt(x * x + y * y);
  if (d > L1 + L2 || d < fabs(L1 - L2) || d < 1) return false;

  // The law of cosines: the angle at the shoulder between the line to the
  // target and the upper arm.
  float a = acos((L1 * L1 + d * d - L2 * L2) / (2 * L1 * d));
  float upper = atan2(y, x) + a;

  // Where that puts the elbow, and the forearm's angle from there.
  float elbowX = L1 * cos(upper);
  float elbowY = L1 * sin(upper);
  float fore = atan2(y - elbowY, x - elbowX);

  // Link angles to this arm's servo angles.
  int s = (int)lround(180 - upper * DEGREES);
  int e = (int)lround(90 + fore * DEGREES);
  if (s < LOW_END || s > HIGH_END || e < LOW_END || e > HIGH_END) return false;
  shoulderAngle = s;
  elbowAngle = e;
  return true;
}

int lastX = -1000, lastY = -1000;

void setup() {
  base.attach(3);
  shoulder.attach(5);
  elbow.attach(6);
  claw.attach(9);
  base.write(90);
  claw.write(60);
  Serial.begin(115200);
  Serial.println("inverse kinematics arm: turn the knobs to move the target");
}

void loop() {
  // The knobs give the target: x from 20 to 150 mm, y from -60 to 120 mm.
  int x = map(analogRead(A0), 0, 1023, 20, 150);
  int y = map(analogRead(A1), 0, 1023, -60, 120);

  int s, e;
  bool reached = solve(x, y, s, e);
  if (reached) {
    shoulder.write(s);
    elbow.write(e);
  }

  // Report each new target once.
  if (x != lastX || y != lastY) {
    lastX = x;
    lastY = y;
    Serial.print("target x ");
    Serial.print(x);
    Serial.print(" mm  y ");
    Serial.print(y);
    Serial.print(" mm  ->  ");
    if (reached) {
      Serial.print("shoulder ");
      Serial.print(s);
      Serial.print("  elbow ");
      Serial.println(e);
    } else {
      Serial.println("out of reach or past a joint limit, holding");
    }
  }
  delay(20);
}