File content as of revision 0:22d279b8c2f1:

#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);
}