diff --git a/include/darwin_walk.h b/include/darwin_walk.h new file mode 100644 index 0000000000000000000000000000000000000000..d40d1d100c4726e91ffeb6548decca41d2b9e0f6 --- /dev/null +++ b/include/darwin_walk.h @@ -0,0 +1,57 @@ +#ifndef _DARWIN_WALK_H +#define _DARWIN_WALK_H + +#include "darwin_robot_base.h" +#include "darwin_registers.h" + +class CDarwinWalk : public CDarwinRobotBase +{ + public: + CDarwinWalk(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim=false); + void set_x_offset(double offset_m); + double get_x_offset(void); + void set_y_offset(double offset_m); + double get_y_offset(void); + void set_z_offset(double offset_m); + double get_z_offset(void); + void set_roll_offset(double offset_rad); + double get_roll_offset(void); + void set_pitch_offset(double offset_rad); + double get_pitch_offset(void); + void set_yaw_offset(double offset_rad); + double get_yaw_offset(void); + void set_hip_pitch_offset(double offset_rad); + double get_hip_pitch_offset(void); + void set_period(double period_s); + double get_period(void); + void set_ssp_dsp_ratio(double ratio); + double get_ssp_dsp_ratio(void); + void set_fwd_bwd_ratio(double ratio); + double get_fwd_bwd_ratio(void); + void set_foot_height(double height_m); + double get_foot_height(void); + void set_left_right_swing(double swing_m); + double get_left_right_swing(void); + void set_top_down_swing(double swing_m); + double get_top_down_swing(void); + void set_pelvis_offset(double offset_rad); + double get_pelvis_offset(void); + void set_arm_swing_gain(double gain); + double get_arm_swing_gain(void); + void set_trans_speed(double speed_m_s); + double get_trans_speed(void); + void set_rot_speed(double speed_rad_s); + double get_rot_speed(void); + void start(void); + void stop(void); + bool is_walking(void); + void set_x_step(double step_m); + double get_x_step(void); + void set_y_step(double step_m); + double get_y_step(void); + void set_turn_step(double step_rad); + double get_turn_step(void); + ~CDarwinWalk(); +}; + +#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index d0520e18707b5aa18fd63efb4f508f4eeecf6ece..47fa514beeab58b0e4be2ad2791b8efd2ead8ea5 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -11,9 +11,9 @@ SET(KDL_INSTALL /opt/ros/indigo) INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake) # driver source files -SET(robot_sources darwin_robot_base.cpp darwin_robot_exceptions.cpp darwin_imu.cpp darwin_dyn_manager.cpp darwin_mmanager.cpp darwin_action.cpp darwin_balance.cpp darwin_joint_motion.cpp) +SET(robot_sources darwin_robot_base.cpp darwin_robot_exceptions.cpp darwin_imu.cpp darwin_dyn_manager.cpp darwin_mmanager.cpp darwin_action.cpp darwin_balance.cpp darwin_joint_motion.cpp darwin_walk.cpp) # application header files -SET(robot_headers ../include/darwin_robot.h ../include/darwin_robot_exceptions.h ../include/darwin_imu.h ../include/darwin_dyn_manager.h ../include/darwin_mmanager.h ../include/darwin_action.h ../include/darwin_balance.h ../include/darwin_joint_motion.h) +SET(robot_headers ../include/darwin_robot.h ../include/darwin_robot_exceptions.h ../include/darwin_imu.h ../include/darwin_dyn_manager.h ../include/darwin_mmanager.h ../include/darwin_action.h ../include/darwin_balance.h ../include/darwin_joint_motion.h ../include/darwin_walk.h) # locate the necessary dependencies FIND_PACKAGE(iriutils REQUIRED) diff --git a/src/darwin_walk.cpp b/src/darwin_walk.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2c84fc84fd19b33997253e2d25502355b40814bb --- /dev/null +++ b/src/darwin_walk.cpp @@ -0,0 +1,414 @@ +#include "darwin_walk.h" +#include "darwin_robot_exceptions.h" + +#define PI 3.14159 + +CDarwinWalk::CDarwinWalk(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id,bool sim) : CDarwinRobotBase(name,bus_id,bus_speed,id,sim) +{ +} + +void CDarwinWalk::set_x_offset(double offset_m) +{ + unsigned char offset; + + this->is_valid(); + offset=(unsigned char)(offset_m*1000.0); + this->robot_device->write_byte_register(WALK_X_OFFSET,offset); + usleep(10000); +} + +double CDarwinWalk::get_x_offset(void) +{ + unsigned char offset; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_X_OFFSET,&offset); + return ((double)((signed char)offset))/1000.0; +} + +void CDarwinWalk::set_y_offset(double offset_m) +{ + unsigned char offset; + + this->is_valid(); + offset=(unsigned char)(offset_m*1000.0); + this->robot_device->write_byte_register(WALK_Y_OFFSET,offset); + usleep(10000); +} + +double CDarwinWalk::get_y_offset(void) +{ + unsigned char offset; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_Y_OFFSET,&offset); + return ((double)((signed char)offset))/1000.0; +} + +void CDarwinWalk::set_z_offset(double offset_m) +{ + unsigned char offset; + + this->is_valid(); + offset=(unsigned char)(offset_m*1000.0); + this->robot_device->write_byte_register(WALK_Z_OFFSET,offset); + usleep(10000); +} + +double CDarwinWalk::get_z_offset(void) +{ + unsigned char offset; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_Z_OFFSET,&offset); + return ((double)((signed char)offset))/1000.0; +} + +void CDarwinWalk::set_roll_offset(double offset_rad) +{ + unsigned char offset; + + this->is_valid(); + offset=(unsigned char)((offset_rad*180.0/PI)*8.0); + this->robot_device->write_byte_register(WALK_ROLL_OFFSET,offset); + usleep(10000); +} + +double CDarwinWalk::get_roll_offset(void) +{ + unsigned char offset; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_ROLL_OFFSET,&offset); + return (((double)offset)*PI/180.0)/8.0; +} + +void CDarwinWalk::set_pitch_offset(double offset_rad) +{ + unsigned char offset; + + this->is_valid(); + offset=(unsigned char)((offset_rad*180.0/PI)*8.0); + this->robot_device->write_byte_register(WALK_PITCH_OFFSET,offset); + usleep(10000); +} + +double CDarwinWalk::get_pitch_offset(void) +{ + unsigned char offset; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_PITCH_OFFSET,&offset); + return (((double)offset)*PI/180.0)/8.0; +} + +void CDarwinWalk::set_yaw_offset(double offset_rad) +{ + unsigned char offset; + + this->is_valid(); + offset=(unsigned char)((offset_rad*180.0/PI)*8.0); + this->robot_device->write_byte_register(WALK_YAW_OFFSET,offset); + usleep(10000); +} + +double CDarwinWalk::get_yaw_offset(void) +{ + unsigned char offset; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_YAW_OFFSET,&offset); + return (((double)offset)*PI/180.0)/8.0; +} + +void CDarwinWalk::set_hip_pitch_offset(double offset_rad) +{ + unsigned short int offset; + + this->is_valid(); + offset=(unsigned short int)((offset_rad*180.0/PI)*1024.0); + this->robot_device->write_word_register(WALK_HIP_PITCH_OFFSET,offset); + usleep(10000); +} + +double CDarwinWalk::get_hip_pitch_offset(void) +{ + unsigned short int offset; + + this->is_valid(); + this->robot_device->read_word_register(WALK_HIP_PITCH_OFFSET,&offset); + return (((double)offset)*PI/180.0)/1024.0; +} + +void CDarwinWalk::set_period(double period_s) +{ + unsigned short int period; + + this->is_valid(); + period=(unsigned short int)(period_s*1000.0); + this->robot_device->write_word_register(WALK_PERIOD_TIME,period); + usleep(10000); +} + +double CDarwinWalk::get_period(void) +{ + unsigned short int period; + + this->is_valid(); + this->robot_device->read_word_register(WALK_PERIOD_TIME,&period); + return ((double)period)/1000.0; +} + +void CDarwinWalk::set_ssp_dsp_ratio(double ratio) +{ + unsigned char ratio_int; + + this->is_valid(); + ratio_int=(unsigned char)(ratio*256.0); + this->robot_device->write_byte_register(WALK_DSP_RATIO,ratio_int); + usleep(10000); +} + +double CDarwinWalk::get_ssp_dsp_ratio(void) +{ + unsigned char ratio_int; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_DSP_RATIO,&ratio_int); + return ((double)ratio_int)/256.0; +} + +void CDarwinWalk::set_fwd_bwd_ratio(double ratio) +{ + unsigned char ratio_int; + + this->is_valid(); + ratio_int=(unsigned char)(ratio*256.0); + this->robot_device->write_byte_register(WALK_STEP_FW_BW_RATIO,ratio_int); + usleep(10000); +} + +double CDarwinWalk::get_fwd_bwd_ratio(void) +{ + unsigned char ratio_int; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_STEP_FW_BW_RATIO,&ratio_int); + return ((double)ratio_int)/256.0; +} + +void CDarwinWalk::set_foot_height(double height_m) +{ + unsigned char height; + + this->is_valid(); + height=(unsigned char)(height_m*1000.0); + this->robot_device->write_byte_register(WALK_FOOT_HEIGHT,height); + usleep(10000); +} + +double CDarwinWalk::get_foot_height(void) +{ + unsigned char height; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_FOOT_HEIGHT,&height); + return ((double)height)/1000.0; +} + +void CDarwinWalk::set_left_right_swing(double swing_m) +{ + unsigned char swing; + + this->is_valid(); + swing=(unsigned char)(swing_m*1000.0); + this->robot_device->write_byte_register(WALK_SWING_RIGHT_LEFT,swing); + usleep(10000); +} + +double CDarwinWalk::get_left_right_swing(void) +{ + unsigned char swing; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_SWING_RIGHT_LEFT,&swing); + return ((double)swing)/1000.0; +} + +void CDarwinWalk::set_top_down_swing(double swing_m) +{ + unsigned char swing; + + this->is_valid(); + swing=(unsigned char)(swing_m*1000.0); + this->robot_device->write_byte_register(WALK_SWING_TOP_DOWN,swing); + usleep(10000); +} + +double CDarwinWalk::get_top_down_swing(void) +{ + unsigned char swing; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_SWING_TOP_DOWN,&swing); + return ((double)swing)/1000.0; +} + +void CDarwinWalk::set_pelvis_offset(double offset_rad) +{ + unsigned char offset; + + this->is_valid(); + offset=(unsigned char)((offset_rad*180.0/PI)*8.0); + this->robot_device->write_byte_register(WALK_PELVIS_OFFSET,offset); + usleep(10000); +} + +double CDarwinWalk::get_pelvis_offset(void) +{ + unsigned char offset; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_PELVIS_OFFSET,&offset); + return (((double)offset)*PI/180.0)/8.0; +} + +void CDarwinWalk::set_arm_swing_gain(double gain) +{ + unsigned char gain_int; + + this->is_valid(); + gain_int=(unsigned char)(gain*32.0); + this->robot_device->write_byte_register(WALK_ARM_SWING_GAIN,gain_int); + usleep(10000); +} + +double CDarwinWalk::get_arm_swing_gain(void) +{ + unsigned char gain_int; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_ARM_SWING_GAIN,&gain_int); + return ((double)gain_int)/32.0; +} + +void CDarwinWalk::set_trans_speed(double speed_m_s) +{ + unsigned char speed; + + this->is_valid(); + speed=(unsigned char)(speed_m_s*1000.0); + this->robot_device->write_byte_register(WALK_MAX_VEL,speed); + usleep(10000); +} + +double CDarwinWalk::get_trans_speed(void) +{ + unsigned char speed; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_MAX_VEL,&speed); + return ((double)speed)/1000.0; +} + +void CDarwinWalk::set_rot_speed(double speed_rad_s) +{ + unsigned char speed; + + this->is_valid(); + speed=(unsigned char)((speed_rad_s*180.0/PI)*8.0); + this->robot_device->write_byte_register(WALK_MAX_ROT_VEL,speed); + usleep(10000); +} + +double CDarwinWalk::get_rot_speed(void) +{ + unsigned char speed; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_MAX_ROT_VEL,&speed); + return (((double)speed)*PI/180.0)/8.0; +} + +void CDarwinWalk::start(void) +{ + this->is_valid(); + this->robot_device->write_byte_register(WALK_CONTROL,WALK_START); +} + +void CDarwinWalk::stop(void) +{ + this->is_valid(); + this->robot_device->write_byte_register(WALK_CONTROL,WALK_STOP); +} + +bool CDarwinWalk::is_walking(void) +{ + unsigned char value; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_CONTROL,&value); + if(value&WALK_RUNNING) + return true; + else + return false; +} + +void CDarwinWalk::set_x_step(double step_m) +{ + unsigned char step; + + this->is_valid(); + step=(unsigned char)(step_m*1000.0); + this->robot_device->write_byte_register(WALK_STEP_FW_BW,step); +} + +double CDarwinWalk::get_x_step(void) +{ + unsigned char step; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_STEP_FW_BW,&step); + return ((double)step)/1000.0; +} + +void CDarwinWalk::set_y_step(double step_m) +{ + unsigned char step; + + this->is_valid(); + step=(unsigned char)(step_m*1000.0); + this->robot_device->write_byte_register(WALK_STEP_LEFT_RIGHT,step); +} + +double CDarwinWalk::get_y_step(void) +{ + unsigned char step; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_STEP_LEFT_RIGHT,&step); + return ((double)step)/1000.0; +} + +void CDarwinWalk::set_turn_step(double step_rad) +{ + unsigned char step; + + this->is_valid(); + step=(unsigned char)((step_rad*180.0/PI)*8.0); + this->robot_device->write_byte_register(WALK_STEP_DIRECTION,step); +} + +double CDarwinWalk::get_turn_step(void) +{ + unsigned char step; + + this->is_valid(); + this->robot_device->read_byte_register(WALK_STEP_DIRECTION,&step); + return (((double)step)*PI/180.0)/8.0; +} + +CDarwinWalk::~CDarwinWalk() +{ +} +