Calum Johnston / Tripod

Group of 3 hexapod legs

Dependents:   Hexapod_Library

Tripod.cpp@4:28821e248538, 2016-05-03 (annotated)

Committer:
el13cj
Date:
Tue May 03 15:54:16 2016 +0000
Revision:
4:28821e248538
Parent:
3:97561b972647
Child:
5:b37ae852304f
Working without globals

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 3:97561b972647 15
el13cj 3:97561b972647 16 void Tripod::set_gait_start(int start_point)
el13cj 3:97561b972647 17 {
el13cj 3:97561b972647 18
el13cj 3:97561b972647 19 if (start_point > 5) {
el13cj 3:97561b972647 20 start_point = 5;
el13cj 3:97561b972647 21 }
el13cj 3:97561b972647 22 if (start_point < 0) {
el13cj 3:97561b972647 23 start_point = 0;
el13cj 3:97561b972647 24 }
el13cj 4:28821e248538 25
el13cj 4:28821e248538 26 i = 0;
el13cj 3:97561b972647 27
el13cj 3:97561b972647 28 current_state = start_point;
el13cj 3:97561b972647 29 next_state = start_point++;
el13cj 3:97561b972647 30
el13cj 3:97561b972647 31 }
el13cj 3:97561b972647 32
el13cj 3:97561b972647 33
el13cj 3:97561b972647 34
el13cj 3:97561b972647 35
el13cj 0:22d279b8c2f1 36 void Tripod::gait_step(void)
el13cj 0:22d279b8c2f1 37 {
el13cj 0:22d279b8c2f1 38
el13cj 0:22d279b8c2f1 39 switch(group_number) {
el13cj 0:22d279b8c2f1 40 case 1:
el13cj 3:97561b972647 41 leg_1.set_leg_position(leg_1_angles[next_state]);
el13cj 3:97561b972647 42 leg_2.set_leg_position(leg_5_angles[next_state]);
el13cj 3:97561b972647 43 leg_3.set_leg_position(leg_3_angles[next_state]);
el13cj 0:22d279b8c2f1 44
el13cj 3:97561b972647 45 next_state++;
el13cj 3:97561b972647 46 if (next_state > 5) {
el13cj 3:97561b972647 47 next_state = 0;
el13cj 0:22d279b8c2f1 48 }
el13cj 0:22d279b8c2f1 49 break;
el13cj 0:22d279b8c2f1 50 case 2:
el13cj 3:97561b972647 51 leg_1.set_leg_position(leg_4_angles[next_state]);
el13cj 3:97561b972647 52 leg_2.set_leg_position(leg_2_angles[next_state]);
el13cj 3:97561b972647 53 leg_3.set_leg_position(leg_6_angles[next_state]);
el13cj 0:22d279b8c2f1 54
el13cj 3:97561b972647 55 next_state++;
el13cj 3:97561b972647 56 if (next_state > 5) {
el13cj 3:97561b972647 57 next_state = 0;
el13cj 0:22d279b8c2f1 58 }
el13cj 0:22d279b8c2f1 59 break;
el13cj 0:22d279b8c2f1 60 default:
el13cj 0:22d279b8c2f1 61
el13cj 0:22d279b8c2f1 62 break;
el13cj 0:22d279b8c2f1 63 }
el13cj 0:22d279b8c2f1 64
el13cj 0:22d279b8c2f1 65 //i2c.stop();
el13cj 0:22d279b8c2f1 66 board.update();
el13cj 0:22d279b8c2f1 67
el13cj 0:22d279b8c2f1 68 }
el13cj 0:22d279b8c2f1 69
el13cj 0:22d279b8c2f1 70
el13cj 0:22d279b8c2f1 71 void Tripod::sweep_step_group(void)
el13cj 0:22d279b8c2f1 72 {
el13cj 0:22d279b8c2f1 73
el13cj 0:22d279b8c2f1 74 switch (group_number) {
el13cj 0:22d279b8c2f1 75
el13cj 0:22d279b8c2f1 76 case 1: {
el13cj 3:97561b972647 77 float an_a_1 = (leg_1_angles[current_state].a + ((leg_1_angles[next_state].a - leg_1_angles[current_state].a) / NUM_STEPS) * i);
el13cj 3:97561b972647 78 float an_b_1 = (leg_1_angles[current_state].b + ((leg_1_angles[next_state].b - leg_1_angles[current_state].b) / NUM_STEPS) * i);
el13cj 3:97561b972647 79 float an_c_1 = (leg_1_angles[current_state].c + ((leg_1_angles[next_state].c - leg_1_angles[current_state].c) / NUM_STEPS) * i);
el13cj 0:22d279b8c2f1 80
el13cj 3:97561b972647 81 float an_a_2 = (leg_5_angles[current_state].a + ((leg_5_angles[next_state].a - leg_5_angles[current_state].a) / NUM_STEPS) * i);
el13cj 3:97561b972647 82 float an_b_2 = (leg_5_angles[current_state].b + ((leg_5_angles[next_state].b - leg_5_angles[current_state].b) / NUM_STEPS) * i);
el13cj 3:97561b972647 83 float an_c_2 = (leg_5_angles[current_state].c + ((leg_5_angles[next_state].c - leg_5_angles[current_state].c) / NUM_STEPS) * i);
el13cj 0:22d279b8c2f1 84
el13cj 3:97561b972647 85 float an_a_3 = (leg_3_angles[current_state].a + ((leg_3_angles[next_state].a - leg_3_angles[current_state].a) / NUM_STEPS) * i);
el13cj 3:97561b972647 86 float an_b_3 = (leg_3_angles[current_state].b + ((leg_3_angles[next_state].b - leg_3_angles[current_state].b) / NUM_STEPS) * i);
el13cj 3:97561b972647 87 float an_c_3 = (leg_3_angles[current_state].c + ((leg_3_angles[next_state].c - leg_3_angles[current_state].c) / NUM_STEPS) * i);
el13cj 0:22d279b8c2f1 88
el13cj 0:22d279b8c2f1 89 leg_1.set_joint_angles(an_a_1, an_b_1, an_c_1);
el13cj 0:22d279b8c2f1 90 leg_2.set_joint_angles(an_a_2, an_b_2, an_c_2);
el13cj 0:22d279b8c2f1 91 leg_3.set_joint_angles(an_a_3, an_b_3, an_c_3);
el13cj 3:97561b972647 92
el13cj 2:81b3104caec1 93 board.update();
el13cj 0:22d279b8c2f1 94 i++;
el13cj 0:22d279b8c2f1 95
el13cj 0:22d279b8c2f1 96 if (i > NUM_STEPS) {
el13cj 0:22d279b8c2f1 97 group_ticker.detach();
el13cj 0:22d279b8c2f1 98 i = 0;
el13cj 0:22d279b8c2f1 99 }
el13cj 1:c6ba788a29a1 100
el13cj 1:c6ba788a29a1 101
el13cj 0:22d279b8c2f1 102
el13cj 0:22d279b8c2f1 103
el13cj 0:22d279b8c2f1 104 }
el13cj 0:22d279b8c2f1 105 break;
el13cj 0:22d279b8c2f1 106 case 2: {
el13cj 4:28821e248538 107 float an_a_1 = (leg_4_angles[current_state].a + ((leg_4_angles[next_state].a - leg_4_angles[current_state].a) / NUM_STEPS) * i);
el13cj 4:28821e248538 108 float an_b_1 = (leg_4_angles[current_state].b + ((leg_4_angles[next_state].b - leg_4_angles[current_state].b) / NUM_STEPS) * i);
el13cj 4:28821e248538 109 float an_c_1 = (leg_4_angles[current_state].c + ((leg_4_angles[next_state].c - leg_4_angles[current_state].c) / NUM_STEPS) * i);
el13cj 0:22d279b8c2f1 110
el13cj 4:28821e248538 111 float an_a_2 = (leg_2_angles[current_state].a + ((leg_2_angles[next_state].a - leg_2_angles[current_state].a) / NUM_STEPS) * i);
el13cj 4:28821e248538 112 float an_b_2 = (leg_2_angles[current_state].b + ((leg_2_angles[next_state].b - leg_2_angles[current_state].b) / NUM_STEPS) * i);
el13cj 4:28821e248538 113 float an_c_2 = (leg_2_angles[current_state].c + ((leg_2_angles[next_state].c - leg_2_angles[current_state].c) / NUM_STEPS) * i);
el13cj 0:22d279b8c2f1 114
el13cj 4:28821e248538 115 float an_a_3 = (leg_6_angles[current_state].a + ((leg_6_angles[next_state].a - leg_6_angles[current_state].a) / NUM_STEPS) * i);
el13cj 4:28821e248538 116 float an_b_3 = (leg_6_angles[current_state].b + ((leg_6_angles[next_state].b - leg_6_angles[current_state].b) / NUM_STEPS) * i);
el13cj 4:28821e248538 117 float an_c_3 = (leg_6_angles[current_state].c + ((leg_6_angles[next_state].c - leg_6_angles[current_state].c) / NUM_STEPS) * i);
el13cj 0:22d279b8c2f1 118
el13cj 0:22d279b8c2f1 119 leg_1.set_joint_angles(an_a_1, an_b_1, an_c_1);
el13cj 0:22d279b8c2f1 120 leg_2.set_joint_angles(an_a_2, an_b_2, an_c_2);
el13cj 0:22d279b8c2f1 121 leg_3.set_joint_angles(an_a_3, an_b_3, an_c_3);
el13cj 3:97561b972647 122
el13cj 2:81b3104caec1 123 board.update();
el13cj 3:97561b972647 124
el13cj 4:28821e248538 125 i++;
el13cj 0:22d279b8c2f1 126
el13cj 4:28821e248538 127 if (i > NUM_STEPS) {
el13cj 0:22d279b8c2f1 128 group_ticker.detach();
el13cj 4:28821e248538 129 i = 0;
el13cj 0:22d279b8c2f1 130 }
el13cj 0:22d279b8c2f1 131
el13cj 0:22d279b8c2f1 132
el13cj 0:22d279b8c2f1 133
el13cj 0:22d279b8c2f1 134
el13cj 0:22d279b8c2f1 135 }
el13cj 0:22d279b8c2f1 136 break;
el13cj 0:22d279b8c2f1 137 default:
el13cj 0:22d279b8c2f1 138
el13cj 0:22d279b8c2f1 139 break;
el13cj 0:22d279b8c2f1 140
el13cj 0:22d279b8c2f1 141 }
el13cj 0:22d279b8c2f1 142
el13cj 3:97561b972647 143
el13cj 0:22d279b8c2f1 144 //i2c.stop();
el13cj 0:22d279b8c2f1 145
el13cj 0:22d279b8c2f1 146
el13cj 0:22d279b8c2f1 147
el13cj 0:22d279b8c2f1 148
el13cj 0:22d279b8c2f1 149
el13cj 0:22d279b8c2f1 150
el13cj 0:22d279b8c2f1 151
el13cj 0:22d279b8c2f1 152 }
el13cj 0:22d279b8c2f1 153
el13cj 0:22d279b8c2f1 154
el13cj 0:22d279b8c2f1 155
el13cj 0:22d279b8c2f1 156 void Tripod::gait_smooth(void)
el13cj 0:22d279b8c2f1 157 {
el13cj 1:c6ba788a29a1 158
el13cj 1:c6ba788a29a1 159
el13cj 2:81b3104caec1 160 group_ticker.attach(this, &Tripod::sweep_step_group, STEP_DELAY);
el13cj 1:c6ba788a29a1 161
el13cj 3:97561b972647 162
el13cj 3:97561b972647 163 current_state = next_state;
el13cj 3:97561b972647 164 next_state++;
el13cj 3:97561b972647 165
el13cj 3:97561b972647 166 if (next_state > 5) {
el13cj 3:97561b972647 167 next_state = 0;
el13cj 1:c6ba788a29a1 168 }
el13cj 1:c6ba788a29a1 169
el13cj 3:97561b972647 170
el13cj 0:22d279b8c2f1 171 }