Calum Johnston / Hexapod_Leg

Library to control a 3DOF leg with a PCA9685.

HEXAPOD_LEG.cpp@0:ead365546ca5, 2016-04-09 (annotated)

Committer:
el13cj
Date:
Sat Apr 09 18:57:46 2016 +0000
Revision:
0:ead365546ca5
Child:
3:9cc530686346
FINISHED AND WORKING MAY NEED CHANGES TO STRUCTS

Who changed what in which revision?

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