File content as of revision 0:ead365546ca5:

#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'));
       
}