Calum Johnston / Hexapod_Leg

Library to control a 3DOF leg with a PCA9685.

HEXAPOD_LEG.cpp@5:50039ebd3a8d, 2016-05-03 (annotated)

Committer:
el13cj
Date:
Tue May 03 10:57:31 2016 +0000
Revision:
5:50039ebd3a8d
Parent:
4:4d4cea424b7f 
All good baby

Who changed what in which revision?

UserRevisionLine numberNew contents of line
el13cj 4:4d4cea424b7f 1 //#include "mbed.h"
el13cj 0:ead365546ca5 2 #include "HEXAPOD_LEG.h"
el13cj 4:4d4cea424b7f 3 //#include "structs.h"
el13cj 0:ead365546ca5 4
el13cj 0:ead365546ca5 5
el13cj 0:ead365546ca5 6 Hexapod_Leg::Hexapod_Leg(PCA9685 Driver, calibrated_leg_centre leg_cent, calibrated_leg_1_degree leg_step, leg_outputs leg_out, bool ON_RHS) :
el13cj 0:ead365546ca5 7
el13cj 3:9cc530686346 8 a_ref(leg_cent.a),
el13cj 3:9cc530686346 9 b_ref(leg_cent.b),
el13cj 3:9cc530686346 10 c_ref(leg_cent.c),
el13cj 0:ead365546ca5 11
el13cj 3:9cc530686346 12 a_1deg(leg_step.a),
el13cj 3:9cc530686346 13 b_1deg(leg_step.b),
el13cj 3:9cc530686346 14 c_1deg(leg_step.c),
el13cj 0:ead365546ca5 15
el13cj 3:9cc530686346 16 a_pin(leg_out.a),
el13cj 3:9cc530686346 17 b_pin(leg_out.b),
el13cj 3:9cc530686346 18 c_pin(leg_out.c),
el13cj 0:ead365546ca5 19
el13cj 3:9cc530686346 20 RHS(ON_RHS),
el13cj 0:ead365546ca5 21
el13cj 3:9cc530686346 22 driver(Driver)
el13cj 0:ead365546ca5 23
el13cj 0:ead365546ca5 24 {
el13cj 3:9cc530686346 25
el13cj 0:ead365546ca5 26 }
el13cj 0:ead365546ca5 27
el13cj 0:ead365546ca5 28
el13cj 0:ead365546ca5 29
el13cj 3:9cc530686346 30 float Hexapod_Leg::angle_to_pwm(float angle, char joint)
el13cj 3:9cc530686346 31 {
el13cj 3:9cc530686346 32
el13cj 0:ead365546ca5 33 float pwm = 0;
el13cj 3:9cc530686346 34
el13cj 0:ead365546ca5 35 switch (joint) {
el13cj 0:ead365546ca5 36 case 'a':
el13cj 0:ead365546ca5 37 case 'A':
el13cj 0:ead365546ca5 38 pwm = a_ref + (angle * a_1deg);
el13cj 0:ead365546ca5 39 break;
el13cj 0:ead365546ca5 40 case 'b':
el13cj 0:ead365546ca5 41 case 'B':
el13cj 0:ead365546ca5 42 if (RHS) {
el13cj 0:ead365546ca5 43 pwm = b_ref - (angle * b_1deg);
el13cj 0:ead365546ca5 44 } else {
el13cj 0:ead365546ca5 45 pwm = b_ref + (angle * b_1deg);
el13cj 0:ead365546ca5 46 }
el13cj 0:ead365546ca5 47 break;
el13cj 0:ead365546ca5 48 case 'c':
el13cj 0:ead365546ca5 49 case 'C':
el13cj 0:ead365546ca5 50 if (RHS) {
el13cj 0:ead365546ca5 51 pwm = c_ref + (angle * c_1deg);
el13cj 0:ead365546ca5 52 } else {
el13cj 0:ead365546ca5 53 pwm = c_ref - (angle * c_1deg);
el13cj 0:ead365546ca5 54 }
el13cj 0:ead365546ca5 55 break;
el13cj 0:ead365546ca5 56 default:
el13cj 0:ead365546ca5 57 pwm = 1500;
el13cj 0:ead365546ca5 58 break;
el13cj 0:ead365546ca5 59 }
el13cj 3:9cc530686346 60
el13cj 0:ead365546ca5 61 return pwm;
el13cj 0:ead365546ca5 62 }
el13cj 0:ead365546ca5 63
el13cj 0:ead365546ca5 64
el13cj 3:9cc530686346 65 void Hexapod_Leg::set_joint_angles(float angle_a, float angle_b, float angle_c)
el13cj 3:9cc530686346 66 {
el13cj 3:9cc530686346 67 driver.set_pwm_pw(a_pin, angle_to_pwm(angle_a, 'a'));
el13cj 3:9cc530686346 68 driver.set_pwm_pw(b_pin, angle_to_pwm(angle_b, 'b'));
el13cj 3:9cc530686346 69 driver.set_pwm_pw(c_pin, angle_to_pwm(angle_c, 'c'));
el13cj 3:9cc530686346 70 }
el13cj 3:9cc530686346 71
el13cj 3:9cc530686346 72 void Hexapod_Leg::set_leg_position(leg_x_angles angles)
el13cj 3:9cc530686346 73 {
el13cj 3:9cc530686346 74 driver.set_pwm_pw(a_pin, angle_to_pwm(angles.a, 'a'));
el13cj 3:9cc530686346 75 driver.set_pwm_pw(b_pin, angle_to_pwm(angles.b, 'b'));
el13cj 3:9cc530686346 76 driver.set_pwm_pw(c_pin, angle_to_pwm(angles.c, 'c'));
el13cj 3:9cc530686346 77 }