diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a6fbf6e1d0815b9285f6691a3d0e0c056343d09a..30fbad80f9702110f5c43aee0431e6d2e2334c28 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -13,7 +13,7 @@ INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR}) INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR}) INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR}) INCLUDE_DIRECTORIES(${bbb_gpio_INCLUDE_DIR}) -INCLUDE_DIRECTORIES(~/bioloid_robot/stm32_code/include/) +INCLUDE_DIRECTORIES(~/humanoids/bioloid_stm32_fw/include/) # create the shared library ADD_LIBRARY(bioloid_robot SHARED ${sources}) # link necessary libraries diff --git a/src/bioloid_robot.cpp b/src/bioloid_robot.cpp index e6fff49bf5c9cf8afff3961e47515e1b06d47a81..e459005be7ab43f7d26c17e9f8a2c123f1034ae2 100644 --- a/src/bioloid_robot.cpp +++ b/src/bioloid_robot.cpp @@ -875,7 +875,7 @@ bool CBioloidRobot::action_is_page_running(void) } /* walking interface */ -void walk_set_x_offset(double offset_m) +void CBioloidRobot::walk_set_x_offset(double offset_m) { unsigned char offset; @@ -888,7 +888,7 @@ void walk_set_x_offset(double offset_m) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_x_offset(void) +double CBioloidRobot::walk_get_x_offset(void) { unsigned char offset; @@ -901,7 +901,7 @@ double walk_get_x_offset(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_y_offset(double offset_m) +void CBioloidRobot::walk_set_y_offset(double offset_m) { unsigned char offset; @@ -914,7 +914,7 @@ void walk_set_y_offset(double offset_m) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_y_offset(void) +double CBioloidRobot::walk_get_y_offset(void) { unsigned char offset; @@ -927,7 +927,7 @@ double walk_get_y_offset(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_z_offset(double offset_m) +void CBioloidRobot::walk_set_z_offset(double offset_m) { unsigned char offset; @@ -940,7 +940,7 @@ void walk_set_z_offset(double offset_m) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -unsigned int walk_get_z_offset(void) +double CBioloidRobot::walk_get_z_offset(void) { unsigned char offset; @@ -953,7 +953,7 @@ unsigned int walk_get_z_offset(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_roll_offset(double offset_rad) +void CBioloidRobot::walk_set_roll_offset(double offset_rad) { unsigned char offset; @@ -966,7 +966,7 @@ void walk_set_roll_offset(double offset_rad) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_roll_offset(void) +double CBioloidRobot::walk_get_roll_offset(void) { unsigned char offset; @@ -979,7 +979,7 @@ double walk_get_roll_offset(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_pitch_offset(double offset_rad) +void CBioloidRobot::walk_set_pitch_offset(double offset_rad) { unsigned char offset; @@ -992,7 +992,7 @@ void walk_set_pitch_offset(double offset_rad) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_pitch_offset(void) +double CBioloidRobot::walk_get_pitch_offset(void) { unsigned char offset; @@ -1005,7 +1005,7 @@ double walk_get_pitch_offset(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_yaw_offset(double offset_rad) +void CBioloidRobot::walk_set_yaw_offset(double offset_rad) { unsigned char offset; @@ -1018,7 +1018,7 @@ void walk_set_yaw_offset(double offset_rad) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_yaw_offset(void) +double CBioloidRobot::walk_get_yaw_offset(void) { unsigned char offset; @@ -1031,7 +1031,7 @@ double walk_get_yaw_offset(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_hip_picth_offset(double offset_rad) +void CBioloidRobot::walk_set_hip_picth_offset(double offset_rad) { unsigned short int offset; @@ -1044,7 +1044,7 @@ void walk_set_hip_picth_offset(double offset_rad) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_hip_pitch_offset(void) +double CBioloidRobot::walk_get_hip_pitch_offset(void) { unsigned short int offset; @@ -1057,7 +1057,7 @@ double walk_get_hip_pitch_offset(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_period(unsigned int period_s) +void CBioloidRobot::walk_set_period(double period_s) { unsigned short int period; @@ -1070,7 +1070,7 @@ void walk_set_period(unsigned int period_s) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -unsigned int walk_get_period(void) +double CBioloidRobot::walk_get_period(void) { unsigned short int period; @@ -1083,7 +1083,7 @@ unsigned int walk_get_period(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_ssp_dsp_ratio(double ratio) +void CBioloidRobot::walk_set_ssp_dsp_ratio(double ratio) { unsigned char ratio_int; @@ -1096,7 +1096,7 @@ void walk_set_ssp_dsp_ratio(double ratio) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_ssp_dsp_ratio(void) +double CBioloidRobot::walk_get_ssp_dsp_ratio(void) { unsigned char ratio_int; @@ -1109,7 +1109,7 @@ double walk_get_ssp_dsp_ratio(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_fwd_bwd_ratio(double ratio) +void CBioloidRobot::walk_set_fwd_bwd_ratio(double ratio) { unsigned char ratio_int; @@ -1122,7 +1122,7 @@ void walk_set_fwd_bwd_ratio(double ratio) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_fwd_bwd_ratio(void) +double CBioloidRobot::walk_get_fwd_bwd_ratio(void) { unsigned char ratio_int; @@ -1135,7 +1135,7 @@ double walk_get_fwd_bwd_ratio(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_foot_height(double height_m) +void CBioloidRobot::walk_set_foot_height(double height_m) { unsigned char height; @@ -1148,7 +1148,7 @@ void walk_set_foot_height(double height_m) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_foot_height(void) +double CBioloidRobot::walk_get_foot_height(void) { unsigned char height; @@ -1161,7 +1161,7 @@ double walk_get_foot_height(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_left_right_swing(double swing_m) +void CBioloidRobot::walk_set_left_right_swing(double swing_m) { unsigned char swing; @@ -1174,20 +1174,20 @@ void walk_set_left_right_swing(double swing_m) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_left_right_swing(void) +double CBioloidRobot::walk_get_left_right_swing(void) { unsigned char swing; if(this->robot_device!=NULL) { this->robot_device->read_byte_register(BIOLOID_WALK_SWING_RIGHT_LEFT,&swing); - return ((double)wsing)/1000.0; + return ((double)swing)/1000.0; } else throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_top_down_swing(double swing_m) +void CBioloidRobot::walk_set_top_down_swing(double swing_m) { unsigned char swing; @@ -1200,20 +1200,20 @@ void walk_set_top_down_swing(double swing_m) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_top_down_swing(void) +double CBioloidRobot::walk_get_top_down_swing(void) { unsigned char swing; if(this->robot_device!=NULL) { this->robot_device->read_byte_register(BIOLOID_WALK_SWING_TOP_DOWN,&swing); - return ((double)wsing)/1000.0; + return ((double)swing)/1000.0; } else throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_pelvis_offset(double offset_rad) +void CBioloidRobot::walk_set_pelvis_offset(double offset_rad) { unsigned char offset; @@ -1226,7 +1226,7 @@ void walk_set_pelvis_offset(double offset_rad) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_pelvis_offset(void) +double CBioloidRobot::walk_get_pelvis_offset(void) { unsigned char offset; @@ -1239,7 +1239,7 @@ double walk_get_pelvis_offset(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_arm_swing_gain(double gain) +void CBioloidRobot::walk_set_arm_swing_gain(double gain) { unsigned char gain_int; @@ -1252,7 +1252,7 @@ void walk_set_arm_swing_gain(double gain) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_arm_swing_gain(void) +double CBioloidRobot::walk_get_arm_swing_gain(void) { unsigned char gain_int; @@ -1265,7 +1265,7 @@ double walk_get_arm_swing_gain(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_trans_speed(double speed_m_s) +void CBioloidRobot::walk_set_trans_speed(double speed_m_s) { unsigned char speed; @@ -1278,7 +1278,7 @@ void walk_set_trans_speed(double speed_m_s) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_trans_speed(void) +double CBioloidRobot::walk_get_trans_speed(void) { unsigned char speed; @@ -1291,7 +1291,7 @@ double walk_get_trans_speed(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_rot_speed(double speed_rad_s) +void CBioloidRobot::walk_set_rot_speed(double speed_rad_s) { unsigned char speed; @@ -1304,7 +1304,7 @@ void walk_set_rot_speed(double speed_rad_s) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_rot_speed(void) +double CBioloidRobot::walk_get_rot_speed(void) { unsigned char speed; @@ -1317,7 +1317,7 @@ double walk_get_rot_speed(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_start(void) +void CBioloidRobot::walk_start(void) { if(this->robot_device!=NULL) { @@ -1328,7 +1328,7 @@ void walk_start(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_stop(void) +void CBioloidRobot::walk_stop(void) { if(this->robot_device!=NULL) { @@ -1339,7 +1339,7 @@ void walk_stop(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -bool is_walking(void) +bool CBioloidRobot::is_walking(void) { unsigned char value; @@ -1355,7 +1355,7 @@ bool is_walking(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_x_step(double step_m) +void CBioloidRobot::walk_set_x_step(double step_m) { unsigned char step; @@ -1368,7 +1368,7 @@ void walk_set_x_step(double step_m) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_x_step(void) +double CBioloidRobot::walk_get_x_step(void) { unsigned char step; @@ -1381,7 +1381,7 @@ double walk_get_x_step(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_y_step(double step_m) +void CBioloidRobot::walk_set_y_step(double step_m) { unsigned char step; @@ -1394,7 +1394,7 @@ void walk_set_y_step(double step_m) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_y_step(void) +double CBioloidRobot::walk_get_y_step(void) { unsigned char step; @@ -1407,7 +1407,7 @@ double walk_get_y_step(void) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void walk_set_turn_step(double step_rad) +void CBioloidRobot::walk_set_turn_step(double step_rad) { unsigned char step; @@ -1420,7 +1420,7 @@ void walk_set_turn_step(double step_rad) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -double walk_get_turn_step(void) +double CBioloidRobot::walk_get_turn_step(void) { unsigned char step; diff --git a/src/bioloid_robot.h b/src/bioloid_robot.h index bf61301aa34e508292e5983cc61e478ac017bbb3..d7633f0bb0cf3789034256e4b7eace55d481a9b2 100644 --- a/src/bioloid_robot.h +++ b/src/bioloid_robot.h @@ -11,6 +11,7 @@ #include "eventserver.h" #define MAX_NUM_SERVOS 31 +#define PI 3.14159 /* available motion modules */ typedef enum {BIOLOID_MM_NONE=0x00,BIOLOID_MM_ACTION=0x01,BIOLOID_MM_WALKING=0x02,BIOLOID_MM_JOINTS=0x03} mm_mode_t; @@ -173,48 +174,48 @@ class CBioloidRobot bool action_is_int_enabled(void); bool action_is_page_running(void); /* walking interface */ - void walk_set_x_offset(unsigned int offset_mm); - unsigned int walk_get_x_offset(void); - void walk_set_y_offset(unsigned int offset_mm); - unsigned int walk_get_y_offset(void); - void walk_set_z_offset(unsigned int offset_mm); - unsigned int walk_get_z_offset(void); - void walk_set_roll_offset(double offset_deg); + void walk_set_x_offset(double offset_m); + double walk_get_x_offset(void); + void walk_set_y_offset(double offset_m); + double walk_get_y_offset(void); + void walk_set_z_offset(double offset_m); + double walk_get_z_offset(void); + void walk_set_roll_offset(double offset_rad); double walk_get_roll_offset(void); - void walk_set_pitch_offset(double offset_deg); + void walk_set_pitch_offset(double offset_rad); double walk_get_pitch_offset(void); - void walk_set_yaw_offset(double offset_deg); + void walk_set_yaw_offset(double offset_rad); double walk_get_yaw_offset(void); - void walk_set_hip_picth_offset(double offset_deg); + void walk_set_hip_picth_offset(double offset_rad); double walk_get_hip_pitch_offset(void); - void walk_set_period(unsigned int period_ms); - unsigned int walk_get_period(void); + void walk_set_period(double period_s); + double walk_get_period(void); void walk_set_ssp_dsp_ratio(double ratio); double walk_get_ssp_dsp_ratio(void); void walk_set_fwd_bwd_ratio(double ratio); double walk_get_fwd_bwd_ratio(void); - void walk_set_foot_height(unsigned int height_mm); - unsigned int walk_get_foot_height(void); - void walk_set_left_right_swing(unsigned int swing_mm); - unsigned int walk_get_left_right_swing(void); - void walk_set_top_down_swing(unsigned int swing_mm); - unsigned int walk_get_top_down_swing(void); - void walk_set_pelvis_offset(double offset_deg); + void walk_set_foot_height(double height_m); + double walk_get_foot_height(void); + void walk_set_left_right_swing(double swing_m); + double walk_get_left_right_swing(void); + void walk_set_top_down_swing(double swing_m); + double walk_get_top_down_swing(void); + void walk_set_pelvis_offset(double offset_rad); double walk_get_pelvis_offset(void); void walk_set_arm_swing_gain(double gain); double walk_get_arm_swing_gain(void); - void walk_set_trans_speed(double speed_mm_s); + void walk_set_trans_speed(double speed_m_s); double walk_get_trans_speed(void); void walk_set_rot_speed(double speed_rad_s); double walk_get_rot_speed(void); void walk_start(void); void walk_stop(void); bool is_walking(void); - void walk_set_x_step(unsigned int step_mm); - unsigned int walk_get_x_step(void); - void walk_set_y_step(unsigned int step_mm); - unsigned int walk_get_y_step(void); - void walk_set_turn_step(double step_deg); + void walk_set_x_step(double step_m); + double walk_get_x_step(void); + void walk_set_y_step(double step_m); + double walk_get_y_step(void); + void walk_set_turn_step(double step_rad); double walk_get_turn_step(void); ~CBioloidRobot(); };