File content as of revision 3:97561b972647:

#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::set_gait_start(int start_point)
{

    if (start_point > 5) {
        start_point = 5;
    }
    if (start_point < 0) {
        start_point = 0;
    }

    current_state = start_point;
    next_state = start_point++;

}




void Tripod::gait_step(void)
{

    switch(group_number) {
        case 1:
            leg_1.set_leg_position(leg_1_angles[next_state]);
            leg_2.set_leg_position(leg_5_angles[next_state]);
            leg_3.set_leg_position(leg_3_angles[next_state]);

            next_state++;
            if (next_state > 5) {
                next_state = 0;
            }
            break;
        case 2:
            leg_1.set_leg_position(leg_4_angles[next_state]);
            leg_2.set_leg_position(leg_2_angles[next_state]);
            leg_3.set_leg_position(leg_6_angles[next_state]);

            next_state++;
            if (next_state > 5) {
                next_state = 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].a + ((leg_1_angles[next_state].a - leg_1_angles[current_state].a) / NUM_STEPS) * i);
            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);
            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);

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

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

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

            board.update();
            i++;

            if (i > NUM_STEPS) {
                group_ticker.detach();
                i = 0;
            }




        }
        break;
        case 2: {
            float an_a_1 = (leg_4_angles[current_state].a + ((leg_4_angles[next_state].a - leg_4_angles[current_state].a) / NUM_STEPS) * j);
            float an_b_1 = (leg_4_angles[current_state].b + ((leg_4_angles[next_state].b - leg_4_angles[current_state].b) / NUM_STEPS) * j);
            float an_c_1 = (leg_4_angles[current_state].c + ((leg_4_angles[next_state].c - leg_4_angles[current_state].c) / NUM_STEPS) * j);

            float an_a_2 = (leg_2_angles[current_state].a + ((leg_2_angles[next_state].a - leg_2_angles[current_state].a) / NUM_STEPS) * j);
            float an_b_2 = (leg_2_angles[current_state].b + ((leg_2_angles[next_state].b - leg_2_angles[current_state].b) / NUM_STEPS) * j);
            float an_c_2 = (leg_2_angles[current_state].c + ((leg_2_angles[next_state].c - leg_2_angles[current_state].c) / NUM_STEPS) * j);

            float an_a_3 = (leg_6_angles[current_state].a + ((leg_6_angles[next_state].a - leg_6_angles[current_state].a) / NUM_STEPS) * j);
            float an_b_3 = (leg_6_angles[current_state].b + ((leg_6_angles[next_state].b - leg_6_angles[current_state].b) / NUM_STEPS) * j);
            float an_c_3 = (leg_6_angles[current_state].c + ((leg_6_angles[next_state].c - leg_6_angles[current_state].c) / NUM_STEPS) * j);

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

            board.update();

            j++;

            if (j > NUM_STEPS) {
                group_ticker.detach();
                j = 0;
            }




        }
        break;
        default:

            break;

    }


    //i2c.stop();







}



void Tripod::gait_smooth(void)
{


    group_ticker.attach(this, &Tripod::sweep_step_group, STEP_DELAY);


    current_state = next_state;
    next_state++;
    
    if (next_state > 5) {
        next_state = 0;
    }


}