Calum Johnston
Library to control a 3DOF leg with a PCA9685.
Revision 0:ead365546ca5, committed 2016-04-09
- Comitter:
- el13cj
- Date:
- Sat Apr 09 18:57:46 2016 +0000
- Child:
- 1:4c98fa9a06a4
- Commit message:
- FINISHED AND WORKING MAY NEED CHANGES TO STRUCTS
Changed in this revision
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HEXAPOD_LEG.h Sat Apr 09 18:57:46 2016 +0000
@@ -0,0 +1,40 @@
+#ifndef HEXAPOD_LEG_LIBRARY_H
+#define HEXAPOD_LEG_LIBRARY_H
+
+#include "PCA9685.h"
+#include "mbed.h"
+#include "structs.h"
+
+class Hexapod_Leg {
+
+ public:
+ Hexapod_Leg(PCA9685 Driver, calibrated_leg_centre leg_cent, calibrated_leg_1_degree leg_step, leg_outputs leg_out, bool ON_RHS);
+ void set_joint_angles(float angle_a, float angle_b, float angle_c);
+
+
+ private:
+ float angle_to_pwm(float angle, char joint);
+
+ private:
+ float a_ref;
+ float b_ref;
+ float c_ref;
+
+ float a_1deg;
+ float b_1deg;
+ float c_1deg;
+
+ int a_pin;
+ int b_pin;
+ int c_pin;
+
+ bool RHS;
+
+ PCA9685 driver;
+
+};
+
+
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PCA9685.lib Sat Apr 09 18:57:46 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/el13cj/code/PCA9685/#8017239aa374
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/structs.h Sat Apr 09 18:57:46 2016 +0000
@@ -0,0 +1,66 @@
+#ifndef HEXAPOD_LEG_STRUCTS_H
+#define HEXAPOD_LEG_STRUCTS_H
+
+struct leg_angles
+
+{
+
+ float a; //Body Joint
+ float b; //Mid Joint
+ float c; //End Joint
+
+};
+
+//Calibrated centre positions for all six legs
+
+typedef const struct leg_angles calibrated_leg_centre;
+
+calibrated_leg_centre leg_cent[6] =
+{
+// A B C
+ {1500, 1545, 2480}, //LEG 1
+ {1475, 1460, 2445}, //LEG 2
+ {1520, 1450, 2490}, //LEG 3
+ {1490, 1535, 490 }, //LEG 4
+ {1480, 1500, 580 }, //LEG 5
+ {1470, 1495, 550 }, //LEG 6
+};
+
+
+typedef const struct leg_angles calibrated_leg_1_degree;
+
+calibrated_leg_1_degree leg_step[6] =
+{
+// A B C
+ {10.889, 10.167, 10.778}, //LEG 1
+ {10.444, 10.500, 10.444}, //LEG 2
+ {10.667, 10.778, 10.389}, //LEG 3
+ {10.556, 10.333, 10.111}, //LEG 4
+ {10.361, 10.111, 10.611}, //LEG 5
+ {10.806, 10.778, 10.222}, //LEG 6
+};
+
+
+struct pwm_outputs
+
+{
+ int a;
+ int b;
+ int c;
+
+};
+
+
+typedef const struct pwm_outputs leg_outputs;
+
+leg_outputs leg_out[3] =
+{
+// A B C
+ {0, 1, 2}, //FRONT LEG
+ {3, 4, 5}, //MID LEG
+ {6, 7, 8}, //REAR LEG
+};
+
+
+
+#endif
\ No newline at end of file