Hexapod.cpp

00001 #include "Hexapod.h"
00002 
00003 #define I2C_ADDR_1 0x82
00004 #define I2C_ADDR_2 0x84
00005 
00006 Hexapod::Hexapod(I2C i2c) :
00007     
00008     board_1(I2C_ADDR_1, i2c, 50),
00009     board_2(I2C_ADDR_2, i2c, 50),    
00010     
00011     //*GROUP 1*
00012     leg_1(board_1, leg_cent[1], leg_step[1], leg_out[0], true),
00013     leg_5(board_2, leg_cent[5], leg_step[5], leg_out[1], false),    
00014     leg_3(board_1, leg_cent[3], leg_step[3], leg_out[2], true),
00015     
00016     group_1(board_1, leg_1, leg_5, leg_3, 1, 0),
00017     //**
00018     
00019     //*GROUP 2*
00020     leg_4(board_2, leg_cent[4], leg_step[4], leg_out[0], false),
00021     leg_2(board_1, leg_cent[2], leg_step[2], leg_out[1], true),
00022     leg_6(board_2, leg_cent[6], leg_step[6], leg_out[2], false),
00023     
00024     group_2(board_2, leg_4, leg_2, leg_6, 2, 3),
00025     //**
00026     
00027     gait(group_1, group_2)
00028 
00029 {
00030     
00031 }
00032 
00033 void Hexapod::init(void)
00034 {
00035     
00036     board_1.init();
00037     board_2.init();
00038     
00039     /*
00040     group_1.set_gait_start(0);
00041     group_2.set_gait_start(5); 
00042     */
00043     
00044     stand();
00045     
00046 }
00047 
00048 void Hexapod::stand(void)
00049 {
00050     leg_1.set_joint_angles(0, 0, -90);
00051     leg_2.set_joint_angles(0, 0, -90);
00052     leg_3.set_joint_angles(0, 0, -90);
00053     board_1.update();
00054 
00055     leg_4.set_joint_angles(0, 0, -90);
00056     leg_5.set_joint_angles(0, 0, -90);
00057     leg_6.set_joint_angles(0, 0, -90);
00058     board_2.update();
00059 }
00060 
00061 void Hexapod::walk(int Dir) 
00062 {
00063     gait.stop();
00064     
00065     direction = Dir;
00066     
00067     gait.start(1);
00068        
00069 }
00070 
00071 void Hexapod::halt(void)
00072 {
00073     
00074     gait.stop();
00075        
00076 }