HEXAPOD_LEG.cpp

00001 //#include "mbed.h"
00002 #include "HEXAPOD_LEG.h"
00003 //#include "structs.h"
00004 
00005 
00006 Hexapod_Leg::Hexapod_Leg(PCA9685 Driver, calibrated_leg_centre leg_cent, calibrated_leg_1_degree leg_step, leg_outputs leg_out, bool ON_RHS) :
00007 
00008     a_ref(leg_cent.a),
00009     b_ref(leg_cent.b),
00010     c_ref(leg_cent.c),
00011 
00012     a_1deg(leg_step.a),
00013     b_1deg(leg_step.b),
00014     c_1deg(leg_step.c),
00015 
00016     a_pin(leg_out.a),
00017     b_pin(leg_out.b),
00018     c_pin(leg_out.c),
00019 
00020     RHS(ON_RHS),
00021 
00022     driver(Driver)
00023 
00024 {
00025 
00026 }
00027 
00028 
00029 
00030 float Hexapod_Leg::angle_to_pwm(float angle, char joint)
00031 {
00032 
00033     float pwm = 0;
00034 
00035     switch (joint) {
00036         case 'a':
00037         case 'A':
00038             pwm = a_ref + (angle * a_1deg);
00039             break;
00040         case 'b':
00041         case 'B':
00042             if (RHS) {
00043                 pwm = b_ref - (angle * b_1deg);
00044             } else {
00045                 pwm = b_ref + (angle * b_1deg);
00046             }
00047             break;
00048         case 'c':
00049         case 'C':
00050             if (RHS) {
00051                 pwm = c_ref + (angle * c_1deg);
00052             } else {
00053                 pwm = c_ref - (angle * c_1deg);
00054             }
00055             break;
00056         default:
00057             pwm = 1500;
00058             break;
00059     }
00060 
00061     return pwm;
00062 }
00063 
00064 
00065 void Hexapod_Leg::set_joint_angles(float angle_a, float angle_b, float angle_c)
00066 {
00067     driver.set_pwm_pw(a_pin, angle_to_pwm(angle_a, 'a'));
00068     driver.set_pwm_pw(b_pin, angle_to_pwm(angle_b, 'b'));
00069     driver.set_pwm_pw(c_pin, angle_to_pwm(angle_c, 'c'));
00070 }
00071 
00072 void Hexapod_Leg::set_leg_position(leg_x_angles angles)
00073 {
00074     driver.set_pwm_pw(a_pin, angle_to_pwm(angles.a, 'a'));
00075     driver.set_pwm_pw(b_pin, angle_to_pwm(angles.b, 'b'));
00076     driver.set_pwm_pw(c_pin, angle_to_pwm(angles.c, 'c'));
00077 }