ik.ino

#include <Servo.h>


const int coxa_pin = 2;
const int femur_pin = 3;
const int tibia_pin = 4;
const int delayt = 250;

const double length_coxa = 40;
const double length_femur = 85;
const double length_tibia = 115;

enum Side
{
    LEFT = 0,
    RIGHT = 1
};

class Leg
{
private:
    Side side;
    struct
    {
        Servo alpha;
        Servo beta;
        Servo gamma;
    } servos;

    struct
    {
        int alpha;
        int beta;
        int gamma;
    } angles;

    struct
    {
        int coxa;
        int tibia;
        int femur;
    } length;


    double calcL(double z_offset, double y)
    {
      return sqrt(z_offset * z_offset + (y - length.coxa) * (y - length.coxa));
    }


    double calcAlpha(double L, double z_offset)
    {
      double alpha_1 = acos(z_offset / L);
      double alpha_2 = acos((length.tibia * length.tibia - length.femur * length.femur - L * L) / (-2.0 * length.femur * L));
      return (alpha_1 + alpha_2) * 180.0 / 3.14;
    }


    double calcBeta(double L)
    {
      return acos((L * L - length.tibia * length.tibia - length.femur * length.femur) / (-2.0 * length.tibia * length.femur)) * 180.0 / 3.14;
    }


    double calcGamma(double x, double y)
    {
      return atan(x / y) * 180.0 / 3.14;
    }


public:
    Leg()
    {
        angles.alpha = 90;
        angles.beta = 90;
        angles.gamma = 90;
    }


    void init(int gamma_pin, int beta_pin, int alpha_pin, Side leg_side, int length_coxa, int length_femur,int length_tibia)
    {
        side = leg_side;
        length.coxa = length_coxa;
        length.femur = length_femur;
        length.tibia = length_tibia;
        servos.gamma.attach(gamma_pin);
        servos.beta.attach(beta_pin);
        servos.alpha.attach(alpha_pin);
        servos.gamma.write(angles.gamma);
        servos.beta.write(angles.beta);
        servos.alpha.write(angles.alpha);
    }


    void write(int alpha, int beta, int gamma)
    {
        Serial.print(gamma);
        Serial.print(" ");
        Serial.print(beta);
        Serial.print(" ");
        Serial.print(alpha);
        if (side == Side::LEFT)
        {
            angles.alpha = alpha;
            angles.beta = 180 - beta;
            angles.gamma = 90 - gamma;
        }

        servos.gamma.write(angles.gamma);
        servos.beta.write(angles.beta);
        servos.alpha.write(angles.alpha);
    }


    void reach(int x, int y, int z)
    {
        double L = calcL(z, y);
        double alpha = calcAlpha(L, z);
        double beta =  calcBeta(L);
        double gamma = calcGamma(x, y);
        write(alpha, beta, gamma);
    }
};


Leg leg;
void setup()
{
  leg.init(coxa_pin, femur_pin, tibia_pin, Side::LEFT, length_coxa, length_femur, length_tibia);
  Serial.begin(9600);
}

void loop()
{
  while(Serial.available() >= 3)
  {
    int x = Serial.parseInt();
    int y = Serial.parseInt();
    int z = Serial.parseInt();
    Serial.print(x);
    Serial.print(" ");
    Serial.print(y);
    Serial.print(" ");
    Serial.print(z);
    Serial.print(" | ");
    leg.reach(x, y, z);
    Serial.println();
  }
}