Diff: Tripod.cpp

Revision:

0:22d279b8c2f1

Child:

1:c6ba788a29a1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Tripod.cpp	Mon May 02 16:20:36 2016 +0000
@@ -0,0 +1,144 @@
+#include "Tripod.h"
+
+Tripod::Tripod(PCA9685 Board, Hexapod_Leg Leg_1, Hexapod_Leg Leg_2, Hexapod_Leg Leg_3, int group) :
+
+    leg_1(Leg_1),
+    leg_2(Leg_2),
+    leg_3(Leg_3),
+    group_number(group),
+    board(Board)
+
+{
+
+}
+
+void Tripod::gait_step(void)
+{
+
+    switch(group_number) {
+        case 1:
+            leg_1.set_leg_position(leg_1_angles[next_state_1]);
+            leg_2.set_leg_position(leg_5_angles[next_state_1]);
+            leg_3.set_leg_position(leg_3_angles[next_state_1]);
+
+            next_state_1++;
+            if (next_state_1 > 5) {
+                next_state_1 = 0;
+            }
+            break;
+        case 2:
+            leg_1.set_leg_position(leg_4_angles[next_state_2]);
+            leg_2.set_leg_position(leg_2_angles[next_state_2]);
+            leg_3.set_leg_position(leg_6_angles[next_state_2]);
+
+            next_state_2++;
+            if (next_state_2 > 5) {
+                next_state_2 = 0;
+            }
+            break;
+        default:
+
+            break;
+    }
+
+    //i2c.stop();
+    board.update();
+
+}
+
+
+void Tripod::sweep_step_group(void)
+{
+
+    switch (group_number) {
+
+        case 1: {
+            float an_a_1 = (leg_1_angles[current_state_1].a + ((leg_1_angles[next_state_1].a - leg_1_angles[current_state_1].a) / NUM_STEPS) * i);
+            float an_b_1 = (leg_1_angles[current_state_1].b + ((leg_1_angles[next_state_1].b - leg_1_angles[current_state_1].b) / NUM_STEPS) * i);
+            float an_c_1 = (leg_1_angles[current_state_1].c + ((leg_1_angles[next_state_1].c - leg_1_angles[current_state_1].c) / NUM_STEPS) * i);
+
+            float an_a_2 = (leg_5_angles[current_state_1].a + ((leg_5_angles[next_state_1].a - leg_5_angles[current_state_1].a) / NUM_STEPS) * i);
+            float an_b_2 = (leg_5_angles[current_state_1].b + ((leg_5_angles[next_state_1].b - leg_5_angles[current_state_1].b) / NUM_STEPS) * i);
+            float an_c_2 = (leg_5_angles[current_state_1].c + ((leg_5_angles[next_state_1].c - leg_5_angles[current_state_1].c) / NUM_STEPS) * i);
+
+            float an_a_3 = (leg_3_angles[current_state_1].a + ((leg_3_angles[next_state_1].a - leg_3_angles[current_state_1].a) / NUM_STEPS) * i);
+            float an_b_3 = (leg_3_angles[current_state_1].b + ((leg_3_angles[next_state_1].b - leg_3_angles[current_state_1].b) / NUM_STEPS) * i);
+            float an_c_3 = (leg_3_angles[current_state_1].c + ((leg_3_angles[next_state_1].c - leg_3_angles[current_state_1].c) / NUM_STEPS) * i);
+
+            leg_1.set_joint_angles(an_a_1, an_b_1, an_c_1);
+            leg_2.set_joint_angles(an_a_2, an_b_2, an_c_2);
+            leg_3.set_joint_angles(an_a_3, an_b_3, an_c_3);
+
+            i++;
+
+            if (i > NUM_STEPS) {
+                group_ticker.detach();
+                i = 0;
+            }
+            
+            current_state_1 = next_state_1;
+            next_state_1++;
+            if (next_state_1 > 5) {
+                next_state_1 = 0;
+            }
+
+
+        }
+        break;
+        case 2: {
+            float an_a_1 = (leg_4_angles[current_state_2].a + ((leg_4_angles[next_state_2].a - leg_4_angles[current_state_2].a) / NUM_STEPS) * i);
+            float an_b_1 = (leg_4_angles[current_state_2].b + ((leg_4_angles[next_state_2].b - leg_4_angles[current_state_2].b) / NUM_STEPS) * i);
+            float an_c_1 = (leg_4_angles[current_state_2].c + ((leg_4_angles[next_state_2].c - leg_4_angles[current_state_2].c) / NUM_STEPS) * i);
+
+            float an_a_2 = (leg_2_angles[current_state_2].a + ((leg_2_angles[next_state_2].a - leg_2_angles[current_state_2].a) / NUM_STEPS) * i);
+            float an_b_2 = (leg_2_angles[current_state_2].b + ((leg_2_angles[next_state_2].b - leg_2_angles[current_state_2].b) / NUM_STEPS) * i);
+            float an_c_2 = (leg_2_angles[current_state_2].c + ((leg_2_angles[next_state_2].c - leg_2_angles[current_state_2].c) / NUM_STEPS) * i);
+
+            float an_a_3 = (leg_6_angles[current_state_2].a + ((leg_6_angles[next_state_2].a - leg_6_angles[current_state_2].a) / NUM_STEPS) * i);
+            float an_b_3 = (leg_6_angles[current_state_2].b + ((leg_6_angles[next_state_2].b - leg_6_angles[current_state_2].b) / NUM_STEPS) * i);
+            float an_c_3 = (leg_6_angles[current_state_2].c + ((leg_6_angles[next_state_2].c - leg_6_angles[current_state_2].c) / NUM_STEPS) * i);
+
+            leg_1.set_joint_angles(an_a_1, an_b_1, an_c_1);
+            leg_2.set_joint_angles(an_a_2, an_b_2, an_c_2);
+            leg_3.set_joint_angles(an_a_3, an_b_3, an_c_3);
+            
+            j++;
+
+            if (j > NUM_STEPS) {
+                group_ticker.detach();
+                j = 0;
+            }
+
+            current_state_2 = next_state_2;
+            next_state_2++;
+            if (next_state_2 > 5) {
+                next_state_2 = 0;
+            }
+
+
+
+        }
+        break;
+        default:
+
+            break;
+
+    }
+
+    board.update();
+    //i2c.stop();
+
+
+
+
+
+
+
+}
+
+
+
+void Tripod::gait_smooth(void)
+{
+    group_ticker.attach(this, &Tripod::sweep_step_group, 0.1);
+}
\ No newline at end of file