Calum Johnston / Tripod

Group of 3 hexapod legs

Dependents:   Hexapod_Library

Tripod.cpp@2:81b3104caec1, 2016-05-02 (annotated)

Committer:
el13cj
Date:
Mon May 02 18:23:40 2016 +0000
Revision:
2:81b3104caec1
Parent:
1:c6ba788a29a1
Child:
3:97561b972647
IT'S ALIVE;

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 2:81b3104caec1 71
el13cj 2:81b3104caec1 72 board.update();
el13cj 0:22d279b8c2f1 73 i++;
el13cj 0:22d279b8c2f1 74
el13cj 0:22d279b8c2f1 75 if (i > NUM_STEPS) {
el13cj 0:22d279b8c2f1 76 group_ticker.detach();
el13cj 0:22d279b8c2f1 77 i = 0;
el13cj 0:22d279b8c2f1 78 }
el13cj 1:c6ba788a29a1 79
el13cj 1:c6ba788a29a1 80
el13cj 0:22d279b8c2f1 81
el13cj 0:22d279b8c2f1 82
el13cj 0:22d279b8c2f1 83 }
el13cj 0:22d279b8c2f1 84 break;
el13cj 0:22d279b8c2f1 85 case 2: {
el13cj 2:81b3104caec1 86 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) * j);
el13cj 2:81b3104caec1 87 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) * j);
el13cj 2:81b3104caec1 88 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) * j);
el13cj 0:22d279b8c2f1 89
el13cj 2:81b3104caec1 90 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) * j);
el13cj 2:81b3104caec1 91 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) * j);
el13cj 2:81b3104caec1 92 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) * j);
el13cj 0:22d279b8c2f1 93
el13cj 2:81b3104caec1 94 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) * j);
el13cj 2:81b3104caec1 95 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) * j);
el13cj 2:81b3104caec1 96 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) * j);
el13cj 0:22d279b8c2f1 97
el13cj 0:22d279b8c2f1 98 leg_1.set_joint_angles(an_a_1, an_b_1, an_c_1);
el13cj 0:22d279b8c2f1 99 leg_2.set_joint_angles(an_a_2, an_b_2, an_c_2);
el13cj 0:22d279b8c2f1 100 leg_3.set_joint_angles(an_a_3, an_b_3, an_c_3);
el13cj 2:81b3104caec1 101
el13cj 2:81b3104caec1 102 board.update();
el13cj 2:81b3104caec1 103
el13cj 0:22d279b8c2f1 104 j++;
el13cj 0:22d279b8c2f1 105
el13cj 0:22d279b8c2f1 106 if (j > NUM_STEPS) {
el13cj 0:22d279b8c2f1 107 group_ticker.detach();
el13cj 0:22d279b8c2f1 108 j = 0;
el13cj 0:22d279b8c2f1 109 }
el13cj 0:22d279b8c2f1 110
el13cj 0:22d279b8c2f1 111
el13cj 0:22d279b8c2f1 112
el13cj 0:22d279b8c2f1 113
el13cj 0:22d279b8c2f1 114 }
el13cj 0:22d279b8c2f1 115 break;
el13cj 0:22d279b8c2f1 116 default:
el13cj 0:22d279b8c2f1 117
el13cj 0:22d279b8c2f1 118 break;
el13cj 0:22d279b8c2f1 119
el13cj 0:22d279b8c2f1 120 }
el13cj 0:22d279b8c2f1 121
el13cj 2:81b3104caec1 122
el13cj 0:22d279b8c2f1 123 //i2c.stop();
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
el13cj 0:22d279b8c2f1 133
el13cj 0:22d279b8c2f1 134
el13cj 0:22d279b8c2f1 135 void Tripod::gait_smooth(void)
el13cj 0:22d279b8c2f1 136 {
el13cj 1:c6ba788a29a1 137
el13cj 1:c6ba788a29a1 138
el13cj 2:81b3104caec1 139 group_ticker.attach(this, &Tripod::sweep_step_group, STEP_DELAY);
el13cj 1:c6ba788a29a1 140
el13cj 1:c6ba788a29a1 141 if (group_number == 1) {
el13cj 1:c6ba788a29a1 142 current_state_1 = next_state_1;
el13cj 1:c6ba788a29a1 143 next_state_1++;
el13cj 1:c6ba788a29a1 144 if (next_state_1 > 5) {
el13cj 1:c6ba788a29a1 145 next_state_1 = 0;
el13cj 1:c6ba788a29a1 146 }
el13cj 1:c6ba788a29a1 147 } else if (group_number == 2) {
el13cj 1:c6ba788a29a1 148 current_state_2 = next_state_2;
el13cj 1:c6ba788a29a1 149 next_state_2++;
el13cj 1:c6ba788a29a1 150 if (next_state_2 > 5) {
el13cj 1:c6ba788a29a1 151 next_state_2 = 0;
el13cj 1:c6ba788a29a1 152 }
el13cj 1:c6ba788a29a1 153 }
el13cj 1:c6ba788a29a1 154
el13cj 0:22d279b8c2f1 155 }