Diff: HEXAPOD_LEG.cpp

Revision:

0:ead365546ca5

Child:

3:9cc530686346

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HEXAPOD_LEG.cpp	Sat Apr 09 18:57:46 2016 +0000
@@ -0,0 +1,70 @@
+#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'));
+       
+} 
\ No newline at end of file