Messing around with inverse kinematics,

robot_arm.ino

#include <Servo.h>

#define P(m) (Serial.print(m));
#define PLN (Serial.println(" "))

const int BASE_PIN = 3,
          ARM1_R_PIN = 5, ARM1_L_PIN = 6,
          ARM2_R_PIN = 9, ARM2_L_PIN = 10;

enum ServoID {
  S_BASE = 0,
  S_ARM1 = 1,
  S_ARM2 = 2
};

struct Rotation {
  float q1, q2, q3;
  Rotation(float q1, float q2, float q3)
    : q1(q1), q2(q2), q3(q3) {}
};

const int MAX_LEN = 7.5, MAX_LEN2 = MAX_LEN * MAX_LEN;
const int ARM_LEN = 4; // inches

int sTarget[3] = {0};
int sPos[3] = {0};
int sDelay = 0;
Servo sServos[5] = {};

const int arm1_cal = -12, arm2_cal = -4;

void sMoveTo(int val, ServoID id, int servoSpeed = 0) {


  if (id == 0) val = constrain(val, 2, 152);
  if (id == 1) val = constrain(val, 20, 150);
  if (id == 2) val = constrain(val, 50, 175);

  sTarget[id] = val;
  if (servoSpeed == 0) sDelay = 0;
  else sDelay = 1000 / servoSpeed;
}

void sMove(int val, ServoID id, int servoSpeed = 0) {
  sMoveTo(sTarget[id] + val, id, servoSpeed);
}

int calib[5] = {0};
void calibrateServo(int val, int id) {
  switch (id) {
    case S_BASE: calib[0] = val + 24; break;
    case S_ARM1: calib[1] = val + 32 + arm1_cal;
      calib[2] = 180 - (val + arm1_cal); break;
    case S_ARM2: calib[3] = 180 - (val + 32 + arm2_cal);
      calib[4] = val - 4 + arm2_cal; break;
  }
}

int cmp(int x, int y) {
  if (x > y) return 1;
  else if (x == y) return 0;
  else return -1;
}

void printVec(int vec[3]) {
  P(vec[0])P(", ")P(vec[1])P(", ")P(vec[2])PLN;
}

bool change[5] = {0};
void updateServos() {
  for (int i = 0; i < 3; i++) {
    int v = cmp(sPos[i], sTarget[i]);
    sPos[i] -= v;
    if (v != 0) {
      if (i == 0) change[0] = true;
      else if (i == 1) change[1] = change[2] = true;
      else if (i == 2) change[3] = change[4] = true;
    }
    calibrateServo(sPos[i], i);
  }

  //printVec(sPos);
  bool doDelay = false;
  for (int i = 0; i < 5; i++) {
    if (change[i]) {
      doDelay = true;
      //P("Changing ")P(i)P(" to ")P(calib[i])PLN;
      sServos[i].write(calib[i]);
      change[i] = false;
    }
  }
  if (sDelay > 0 && doDelay) delay(sDelay);
}

float kin_base(float x, float z) {
  if (x == 0) return PI/2;
  float v = atan(z / x);
  if (x < 0) v += PI;
  return v;
}

Rotation kin_arm(float x, float y) {
  float q3 = acos((x * x + y * y - 2 * ARM_LEN * ARM_LEN) /
                  (2 * ARM_LEN * ARM_LEN));
  
  float q2 = atan(y / x) - atan((ARM_LEN * sin(q3)) /
                                (ARM_LEN + ARM_LEN * cos(q3)));

  return Rotation(0, q2, q3);
}

Rotation kinematics(float x, float y, float z) {
  y *= -1; //Why??

  float dist = sqrt(x * x + z * z);
  Rotation rot = kin_arm(dist, y);
  rot.q1 = kin_base(x, z);
  
  
  rot.q1 *= RAD_TO_DEG;
  //rot.q1 -= 45;
  
  rot.q2 *= -RAD_TO_DEG;
  
  rot.q3 *= RAD_TO_DEG;
  rot.q3 = 180 - rot.q3;
  
  return rot;
}

float fixPos[3] = {0};
void sMoveToInvKin(float x, float y, float z) {
  float mag = x * x + y * y + z * z;

  if (mag > MAX_LEN2) {
    mag = sqrt(mag);
    x = (x / mag) * MAX_LEN;
    y = (y / mag) * MAX_LEN;
    z = (z / mag) * MAX_LEN;
  }
  Rotation r = kinematics(x, y, z);
  if (isnan(r.q1) || isnan(r.q2) || isnan(r.q3)) return;
  sMoveTo(r.q1, S_BASE);
  sMoveTo(r.q2, S_ARM1);
  sMoveTo(r.q3, S_ARM2);

}

void setup() {
  Serial.begin(9600);

  sServos[0].attach(BASE_PIN);
  sServos[1].attach(ARM1_R_PIN);
  sServos[2].attach(ARM1_L_PIN);
  sServos[3].attach(ARM2_R_PIN);
  sServos[4].attach(ARM2_L_PIN);

  sPos[0] = 89;
  sPos[1] = 89;
  sPos[2] = 89;
  sMoveTo(90, S_BASE);
  sMoveTo(90, S_ARM1);
  sMoveTo(90, S_ARM2);
}

float  tick = 0;
float stp = 0.01;
void loop() {
  // put your main code here, to run repeatedly:
  if (Serial.available() == 3) {
    Serial.println("Reading...");
    sMoveToInvKin(Serial.parseFloat(), Serial.parseFloat(), Serial.parseFloat());
  }



    tick += stp;

    P(cos(tick)*2 + 2)P(",")P(sin(tick)*2+2)PLN;
    sMoveToInvKin(0, cos(tick)*2 + 2, sin(tick)*2 + 4.3);


  updateServos();
}