Calum Johnston / Tripod

Group of 3 hexapod legs

Dependents:   Hexapod_Library

Tripod.cpp@1:c6ba788a29a1, 2016-05-02 (annotated)

Committer:
el13cj
Date:
Mon May 02 16:33:37 2016 +0000
Revision:
1:c6ba788a29a1
Parent:
0:22d279b8c2f1
Child:
2:81b3104caec1
Group 1 working, group 2 mysteriously not

Who changed what in which revision?

UserRevisionLine numberNew contents of line
el13cj 0:22d279b8c2f1 1 #include "Tripod.h"
el13cj 0:22d279b8c2f1 2
el13cj 0:22d279b8c2f1 3 Tripod::Tripod(PCA9685 Board, Hexapod_Leg Leg_1, Hexapod_Leg Leg_2, Hexapod_Leg Leg_3, int group) :
el13cj 0:22d279b8c2f1 4
el13cj 0:22d279b8c2f1 5 leg_1(Leg_1),
el13cj 0:22d279b8c2f1 6 leg_2(Leg_2),
el13cj 0:22d279b8c2f1 7 leg_3(Leg_3),
el13cj 0:22d279b8c2f1 8 group_number(group),
el13cj 0:22d279b8c2f1 9 board(Board)
el13cj 0:22d279b8c2f1 10
el13cj 0:22d279b8c2f1 11 {
el13cj 0:22d279b8c2f1 12
el13cj 0:22d279b8c2f1 13 }
el13cj 0:22d279b8c2f1 14
el13cj 0:22d279b8c2f1 15 void Tripod::gait_step(void)
el13cj 0:22d279b8c2f1 16 {
el13cj 0:22d279b8c2f1 17
el13cj 0:22d279b8c2f1 18 switch(group_number) {
el13cj 0:22d279b8c2f1 19 case 1:
el13cj 0:22d279b8c2f1 20 leg_1.set_leg_position(leg_1_angles[next_state_1]);
el13cj 0:22d279b8c2f1 21 leg_2.set_leg_position(leg_5_angles[next_state_1]);
el13cj 0:22d279b8c2f1 22 leg_3.set_leg_position(leg_3_angles[next_state_1]);
el13cj 0:22d279b8c2f1 23
el13cj 0:22d279b8c2f1 24 next_state_1++;
el13cj 0:22d279b8c2f1 25 if (next_state_1 > 5) {
el13cj 0:22d279b8c2f1 26 next_state_1 = 0;
el13cj 0:22d279b8c2f1 27 }
el13cj 0:22d279b8c2f1 28 break;
el13cj 0:22d279b8c2f1 29 case 2:
el13cj 0:22d279b8c2f1 30 leg_1.set_leg_position(leg_4_angles[next_state_2]);
el13cj 0:22d279b8c2f1 31 leg_2.set_leg_position(leg_2_angles[next_state_2]);
el13cj 0:22d279b8c2f1 32 leg_3.set_leg_position(leg_6_angles[next_state_2]);
el13cj 0:22d279b8c2f1 33
el13cj 0:22d279b8c2f1 34 next_state_2++;
el13cj 0:22d279b8c2f1 35 if (next_state_2 > 5) {
el13cj 0:22d279b8c2f1 36 next_state_2 = 0;
el13cj 0:22d279b8c2f1 37 }
el13cj 0:22d279b8c2f1 38 break;
el13cj 0:22d279b8c2f1 39 default:
el13cj 0:22d279b8c2f1 40
el13cj 0:22d279b8c2f1 41 break;
el13cj 0:22d279b8c2f1 42 }
el13cj 0:22d279b8c2f1 43
el13cj 0:22d279b8c2f1 44 //i2c.stop();
el13cj 0:22d279b8c2f1 45 board.update();
el13cj 0:22d279b8c2f1 46
el13cj 0:22d279b8c2f1 47 }
el13cj 0:22d279b8c2f1 48
el13cj 0:22d279b8c2f1 49
el13cj 0:22d279b8c2f1 50 void Tripod::sweep_step_group(void)
el13cj 0:22d279b8c2f1 51 {
el13cj 0:22d279b8c2f1 52
el13cj 0:22d279b8c2f1 53 switch (group_number) {
el13cj 0:22d279b8c2f1 54
el13cj 0:22d279b8c2f1 55 case 1: {
el13cj 0:22d279b8c2f1 56 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);
el13cj 0:22d279b8c2f1 57 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);
el13cj 0:22d279b8c2f1 58 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);
el13cj 0:22d279b8c2f1 59
el13cj 0:22d279b8c2f1 60 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);
el13cj 0:22d279b8c2f1 61 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);
el13cj 0:22d279b8c2f1 62 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);
el13cj 0:22d279b8c2f1 63
el13cj 0:22d279b8c2f1 64 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);
el13cj 0:22d279b8c2f1 65 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);
el13cj 0:22d279b8c2f1 66 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);
el13cj 0:22d279b8c2f1 67
el13cj 0:22d279b8c2f1 68 leg_1.set_joint_angles(an_a_1, an_b_1, an_c_1);
el13cj 0:22d279b8c2f1 69 leg_2.set_joint_angles(an_a_2, an_b_2, an_c_2);
el13cj 0:22d279b8c2f1 70 leg_3.set_joint_angles(an_a_3, an_b_3, an_c_3);
el13cj 0:22d279b8c2f1 71
el13cj 0:22d279b8c2f1 72 i++;
el13cj 0:22d279b8c2f1 73
el13cj 0:22d279b8c2f1 74 if (i > NUM_STEPS) {
el13cj 0:22d279b8c2f1 75 group_ticker.detach();
el13cj 0:22d279b8c2f1 76 i = 0;
el13cj 0:22d279b8c2f1 77 }
el13cj 1:c6ba788a29a1 78
el13cj 1:c6ba788a29a1 79
el13cj 0:22d279b8c2f1 80
el13cj 0:22d279b8c2f1 81
el13cj 0:22d279b8c2f1 82 }
el13cj 0:22d279b8c2f1 83 break;
el13cj 0:22d279b8c2f1 84 case 2: {
el13cj 0:22d279b8c2f1 85 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);
el13cj 0:22d279b8c2f1 86 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);
el13cj 0:22d279b8c2f1 87 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);
el13cj 0:22d279b8c2f1 88
el13cj 0:22d279b8c2f1 89 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);
el13cj 0:22d279b8c2f1 90 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);
el13cj 0:22d279b8c2f1 91 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);
el13cj 0:22d279b8c2f1 92
el13cj 0:22d279b8c2f1 93 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);
el13cj 0:22d279b8c2f1 94 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);
el13cj 0:22d279b8c2f1 95 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);
el13cj 0:22d279b8c2f1 96
el13cj 0:22d279b8c2f1 97 leg_1.set_joint_angles(an_a_1, an_b_1, an_c_1);
el13cj 0:22d279b8c2f1 98 leg_2.set_joint_angles(an_a_2, an_b_2, an_c_2);
el13cj 0:22d279b8c2f1 99 leg_3.set_joint_angles(an_a_3, an_b_3, an_c_3);
el13cj 1:c6ba788a29a1 100
el13cj 0:22d279b8c2f1 101 j++;
el13cj 0:22d279b8c2f1 102
el13cj 0:22d279b8c2f1 103 if (j > NUM_STEPS) {
el13cj 0:22d279b8c2f1 104 group_ticker.detach();
el13cj 0:22d279b8c2f1 105 j = 0;
el13cj 0:22d279b8c2f1 106 }
el13cj 0:22d279b8c2f1 107
el13cj 0:22d279b8c2f1 108
el13cj 0:22d279b8c2f1 109
el13cj 0:22d279b8c2f1 110
el13cj 0:22d279b8c2f1 111 }
el13cj 0:22d279b8c2f1 112 break;
el13cj 0:22d279b8c2f1 113 default:
el13cj 0:22d279b8c2f1 114
el13cj 0:22d279b8c2f1 115 break;
el13cj 0:22d279b8c2f1 116
el13cj 0:22d279b8c2f1 117 }
el13cj 0:22d279b8c2f1 118
el13cj 0:22d279b8c2f1 119 board.update();
el13cj 0:22d279b8c2f1 120 //i2c.stop();
el13cj 0:22d279b8c2f1 121
el13cj 0:22d279b8c2f1 122
el13cj 0:22d279b8c2f1 123
el13cj 0:22d279b8c2f1 124
el13cj 0:22d279b8c2f1 125
el13cj 0:22d279b8c2f1 126
el13cj 0:22d279b8c2f1 127
el13cj 0:22d279b8c2f1 128 }
el13cj 0:22d279b8c2f1 129
el13cj 0:22d279b8c2f1 130
el13cj 0:22d279b8c2f1 131
el13cj 0:22d279b8c2f1 132 void Tripod::gait_smooth(void)
el13cj 0:22d279b8c2f1 133 {
el13cj 1:c6ba788a29a1 134
el13cj 1:c6ba788a29a1 135
el13cj 1:c6ba788a29a1 136 group_ticker.attach(this, &Tripod::sweep_step_group, 0.5);
el13cj 1:c6ba788a29a1 137
el13cj 1:c6ba788a29a1 138 if (group_number == 1) {
el13cj 1:c6ba788a29a1 139 current_state_1 = next_state_1;
el13cj 1:c6ba788a29a1 140 next_state_1++;
el13cj 1:c6ba788a29a1 141 if (next_state_1 > 5) {
el13cj 1:c6ba788a29a1 142 next_state_1 = 0;
el13cj 1:c6ba788a29a1 143 }
el13cj 1:c6ba788a29a1 144 } else if (group_number == 2) {
el13cj 1:c6ba788a29a1 145 current_state_2 = next_state_2;
el13cj 1:c6ba788a29a1 146 next_state_2++;
el13cj 1:c6ba788a29a1 147 if (next_state_2 > 5) {
el13cj 1:c6ba788a29a1 148 next_state_2 = 0;
el13cj 1:c6ba788a29a1 149 }
el13cj 1:c6ba788a29a1 150 }
el13cj 1:c6ba788a29a1 151
el13cj 0:22d279b8c2f1 152 }