File content as of revision 6:a91d18a26ba3:

/*
@file Leg.cpp

@brief Member functions implementations

*/
#include "mbed.h"
#include "Leg.h"

Leg::Leg(PinName coxa_pin, PinName femur_pin, PinName tibia_pin)
{
    // set up pins as required
    coxa_servo = new PwmOut(coxa_pin);
    femur_servo = new PwmOut(femur_pin);
    tibia_servo = new PwmOut(tibia_pin);
    // PWM objects have 50 Hz frequency by default so shouldn't need to set servo frequency
    // can do it anyway to be sure. All channels share same period so just need to set one of them
    coxa_servo->period(1.0/50.0);  // servos are typically 50 Hz
    // setup USB serial for debug/comms
    //serial = new Serial(USBTX,USBRX);
    
    coxa_length_ = 28.0;
    femur_length_ = 58.0;
    tibia_length_ = 125.0;
    
}

vector_t Leg::solve_inverse_kinematics(float x, float y, float z)
{
    // Equations from Quadrupedal Locomotion - An Introduction to Control of Four-legged Robot
    // Gonzalez de Santos, Garcia and Estremera, Springer-Verlag London, 2006
    
    vector_t angles;
    
    angles.x = atan2(y,x);  // coxa

    float A = -z;
    float E = x*cos(angles.x) + y*sin(angles.x);
    float B = coxa_length_ - E;
    float D = (2*coxa_length_*E + tibia_length_*tibia_length_ - femur_length_*femur_length_ - coxa_length_*coxa_length_ - z*z - E*E)/(2*femur_length_);

    angles.y = -atan2(B,A) + atan2(D,sqrt(A*A+B*B-D*D));
    angles.z = atan2(z - femur_length_*sin(angles.y),E-femur_length_*cos(angles.y)-coxa_length_) - angles.y;

    //serial->printf("coxa_angle = %f\n",RAD2DEG*angles.x);
    //serial->printf("femur_angle = %f\n",RAD2DEG*angles.y);
    //serial->printf("tibia_angle = %f\n",RAD2DEG*angles.z);
    
    return angles;
}

void Leg::move(vector_t angles)
{
    // formulas to convert angle to pulse-width
    // these need calibrating for each servo
    // the base value is the pulse width that aligns the joint along the common normal
    // the gradient value comes from measuring the pulse that moves the joint to 90 degree
    // i.e. gradient = (pulse_90 - pulse_normal)/90
    // TODO implement calibration over serial
    float coxa_pulse = 1510.0 + 10.44*angles.x*RAD2DEG;
    float femur_pulse = 1480.0 + 10.88*angles.y*RAD2DEG;
    float tibia_pulse = 490.0 - 10.33*angles.z*RAD2DEG;

    //serial.printf("Servo Pulses (us): c = %f f =%f t = %f\n",coxa_pulse,femur_pulse,tibia_pulse);
    
    // update servo pulse widths
    coxa_servo->pulsewidth_us(int(coxa_pulse));
    femur_servo->pulsewidth_us(int(femur_pulse));
    tibia_servo->pulsewidth_us(int(tibia_pulse));

}

void Leg::set_link_lengths(float coxa_length, float femur_length, float tibia_length)
{
    coxa_length_ = coxa_length;
    femur_length_ = femur_length;
    tibia_length_ = tibia_length;
}