Tripod.cpp

00001 #include "Tripod.h"
00002 
00003 Tripod::Tripod(PCA9685 Board, Hexapod_Leg Leg_1, Hexapod_Leg Leg_2, Hexapod_Leg Leg_3, int group, int start_point) :
00004 
00005     leg_1(Leg_1),
00006     leg_2(Leg_2),
00007     leg_3(Leg_3),
00008     group_number(group),
00009     board(Board),
00010 
00011     current_state(0),
00012     next_state(start_point++),
00013     i(0)
00014 
00015 {
00016     //direction = 0;
00017 }
00018 
00019 /*
00020 void Tripod::set_direction(int dir)
00021 {
00022 
00023     direction = dir;
00024 
00025 }
00026 */
00027 /*
00028 void Tripod::set_gait_start(int start_point)
00029 {
00030 
00031     if (start_point > 5) {
00032         start_point = 5;
00033     }
00034     if (start_point < 0) {
00035         start_point = 0;
00036     }
00037 
00038     i = 0;
00039 
00040     current_state = start_point;
00041     next_state = start_point++;
00042 
00043 }
00044 */
00045 
00046 
00047 
00048 void Tripod::gait_step(void)
00049 {
00050     int d1 = (direction + 0)%6;
00051     int d5 = (direction + 4)%6;
00052     int d3 = (direction + 2)%6;
00053     int d4 = (direction + 3)%6;
00054     int d2 = (direction + 1)%6;
00055     int d6 = (direction + 5)%6;
00056 
00057     switch(group_number) {
00058         case 1:
00059 
00060 
00061 
00062             leg_1.set_leg_position(all_leg_angles[d1][next_state]);
00063             leg_2.set_leg_position(all_leg_angles[d5][next_state]);
00064             leg_3.set_leg_position(all_leg_angles[d3][next_state]);
00065 
00066             next_state++;
00067             if (next_state > 5) {
00068                 next_state = 1;
00069             }
00070             break;
00071         case 2:
00072 
00073 
00074 
00075             leg_1.set_leg_position(all_leg_angles[d4][next_state]);
00076             leg_2.set_leg_position(all_leg_angles[d2][next_state]);
00077             leg_3.set_leg_position(all_leg_angles[d6][next_state]);
00078 
00079             next_state++;
00080             if (next_state > 5) {
00081                 next_state = 1;
00082             }
00083             break;
00084         default:
00085 
00086             break;
00087     }
00088 
00089     //i2c.stop();
00090     board.update();
00091 
00092 }
00093 
00094 void Tripod::sweep_step_group(void)
00095 {
00096 
00097     switch (group_number) {
00098 
00099         case 1: {
00100 
00101             int d1 = (direction + 0)%6;
00102             int d5 = (direction + 4)%6;
00103             int d3 = (direction + 2)%6;
00104 
00105             float an_a_1 = (all_leg_angles[d1][current_state].a + ((all_leg_angles[d1][next_state].a - all_leg_angles[d1][current_state].a) / NUM_STEPS) * i);
00106             float an_b_1 = (all_leg_angles[d1][current_state].b + ((all_leg_angles[d1][next_state].b - all_leg_angles[d1][current_state].b) / NUM_STEPS) * i);
00107             float an_c_1 = (all_leg_angles[d1][current_state].c + ((all_leg_angles[d1][next_state].c - all_leg_angles[d1][current_state].c) / NUM_STEPS) * i);
00108 
00109             float an_a_2 = (all_leg_angles[d5][current_state].a + ((all_leg_angles[d5][next_state].a - all_leg_angles[d5][current_state].a) / NUM_STEPS) * i);
00110             float an_b_2 = (all_leg_angles[d5][current_state].b + ((all_leg_angles[d5][next_state].b - all_leg_angles[d5][current_state].b) / NUM_STEPS) * i);
00111             float an_c_2 = (all_leg_angles[d5][current_state].c + ((all_leg_angles[d5][next_state].c - all_leg_angles[d5][current_state].c) / NUM_STEPS) * i);
00112 
00113             float an_a_3 = (all_leg_angles[d3][current_state].a + ((all_leg_angles[d3][next_state].a - all_leg_angles[d3][current_state].a) / NUM_STEPS) * i);
00114             float an_b_3 = (all_leg_angles[d3][current_state].b + ((all_leg_angles[d3][next_state].b - all_leg_angles[d3][current_state].b) / NUM_STEPS) * i);
00115             float an_c_3 = (all_leg_angles[d3][current_state].c + ((all_leg_angles[d3][next_state].c - all_leg_angles[d3][current_state].c) / NUM_STEPS) * i);
00116 
00117             leg_1.set_joint_angles(an_a_1, an_b_1, an_c_1);
00118             leg_2.set_joint_angles(an_a_2, an_b_2, an_c_2);
00119             leg_3.set_joint_angles(an_a_3, an_b_3, an_c_3);
00120 
00121             board.update();
00122             i++;
00123 
00124             if (i > NUM_STEPS) {
00125                 group_ticker.detach();
00126                 i = 0;
00127             }
00128 
00129         }
00130         break;
00131         case 2: {
00132 
00133             int d4 = (direction + 3)%6;
00134             int d2 = (direction + 1)%6;
00135             int d6 = (direction + 5)%6;
00136 
00137             float an_a_1 = (all_leg_angles[d4][current_state].a + ((all_leg_angles[d4][next_state].a - all_leg_angles[d4][current_state].a) / NUM_STEPS) * i);
00138             float an_b_1 = (all_leg_angles[d4][current_state].b + ((all_leg_angles[d4][next_state].b - all_leg_angles[d4][current_state].b) / NUM_STEPS) * i);
00139             float an_c_1 = (all_leg_angles[d4][current_state].c + ((all_leg_angles[d4][next_state].c - all_leg_angles[d4][current_state].c) / NUM_STEPS) * i);
00140 
00141             float an_a_2 = (all_leg_angles[d2][current_state].a + ((all_leg_angles[d2][next_state].a - all_leg_angles[d2][current_state].a) / NUM_STEPS) * i);
00142             float an_b_2 = (all_leg_angles[d2][current_state].b + ((all_leg_angles[d2][next_state].b - all_leg_angles[d2][current_state].b) / NUM_STEPS) * i);
00143             float an_c_2 = (all_leg_angles[d2][current_state].c + ((all_leg_angles[d2][next_state].c - all_leg_angles[d2][current_state].c) / NUM_STEPS) * i);
00144 
00145             float an_a_3 = (all_leg_angles[d6][current_state].a + ((all_leg_angles[d6][next_state].a - all_leg_angles[d6][current_state].a) / NUM_STEPS) * i);
00146             float an_b_3 = (all_leg_angles[d6][current_state].b + ((all_leg_angles[d6][next_state].b - all_leg_angles[d6][current_state].b) / NUM_STEPS) * i);
00147             float an_c_3 = (all_leg_angles[d6][current_state].c + ((all_leg_angles[d6][next_state].c - all_leg_angles[d6][current_state].c) / NUM_STEPS) * i);
00148 
00149             leg_1.set_joint_angles(an_a_1, an_b_1, an_c_1);
00150             leg_2.set_joint_angles(an_a_2, an_b_2, an_c_2);
00151             leg_3.set_joint_angles(an_a_3, an_b_3, an_c_3);
00152 
00153             board.update();
00154 
00155             i++;
00156 
00157             if (i > NUM_STEPS) {
00158                 group_ticker.detach();
00159                 i = 0;
00160             }
00161 
00162         }
00163         break;
00164         default:
00165 
00166             break;
00167 
00168     }
00169 
00170 
00171 }
00172 
00173 
00174 
00175 void Tripod::gait_smooth(void)
00176 {
00177 
00178 
00179     group_ticker.attach(this, &Tripod::sweep_step_group, STEP_DELAY);
00180 
00181 
00182     current_state = next_state;
00183     next_state++;
00184 
00185     if (next_state > 5) {
00186         next_state = 1;
00187     }
00188 
00189 
00190 }