File content as of revision 3:9cc530686346:

#include "mbed.h"
#include "HEXAPOD_LEG.h"
#include "structs.h"


Hexapod_Leg::Hexapod_Leg(PCA9685 Driver, calibrated_leg_centre leg_cent, calibrated_leg_1_degree leg_step, leg_outputs leg_out, bool ON_RHS) :

    a_ref(leg_cent.a),
    b_ref(leg_cent.b),
    c_ref(leg_cent.c),

    a_1deg(leg_step.a),
    b_1deg(leg_step.b),
    c_1deg(leg_step.c),

    a_pin(leg_out.a),
    b_pin(leg_out.b),
    c_pin(leg_out.c),

    RHS(ON_RHS),

    driver(Driver)

{

}



float Hexapod_Leg::angle_to_pwm(float angle, char joint)
{

    float pwm = 0;

    switch (joint) {
        case 'a':
        case 'A':
            pwm = a_ref + (angle * a_1deg);
            break;
        case 'b':
        case 'B':
            if (RHS) {
                pwm = b_ref - (angle * b_1deg);
            } else {
                pwm = b_ref + (angle * b_1deg);
            }
            break;
        case 'c':
        case 'C':
            if (RHS) {
                pwm = c_ref + (angle * c_1deg);
            } else {
                pwm = c_ref - (angle * c_1deg);
            }
            break;
        default:
            pwm = 1500;
            break;
    }

    return pwm;
}


void Hexapod_Leg::set_joint_angles(float angle_a, float angle_b, float angle_c)
{
    driver.set_pwm_pw(a_pin, angle_to_pwm(angle_a, 'a'));
    driver.set_pwm_pw(b_pin, angle_to_pwm(angle_b, 'b'));
    driver.set_pwm_pw(c_pin, angle_to_pwm(angle_c, 'c'));
}

void Hexapod_Leg::set_leg_position(leg_x_angles angles)
{
    driver.set_pwm_pw(a_pin, angle_to_pwm(angles.a, 'a'));
    driver.set_pwm_pw(b_pin, angle_to_pwm(angles.b, 'b'));
    driver.set_pwm_pw(c_pin, angle_to_pwm(angles.c, 'c'));
}