diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index cd4b704523e56b739992dcbee6b8470571df5d7d..7f0c58706705330dc18b19bd6a45c1f94782026c 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -8,25 +8,27 @@
#include "xml/darwin_config.hxx"
#endif
+#define PI 3.14159
+
const std::string servo_names[MAX_NUM_SERVOS]={std::string("Servo0"),
- std::string("j_shoulder_r"),
- std::string("j_shoulder_l"),
- std::string("j_high_arm_r"),
- std::string("j_high_arm_l"),
- std::string("j_low_arm_r"),
- std::string("j_low_arm_l"),
- std::string("j_pelvis_r"),
- std::string("j_pelvis_l"),
- std::string("j_thigh1_r"),
- std::string("j_thigh1_l"),
- std::string("j_thigh2_r"),
- std::string("j_thigh2_l"),
- std::string("j_tibia_r"),
- std::string("j_tibia_l"),
- std::string("j_ankle1_r"),
- std::string("j_ankle1_l"),
- std::string("j_ankle2_r"),
- std::string("j_ankle2_l"),
+ std::string("j_shoulder_pitch_r"),
+ std::string("j_shoulder_pitch_l"),
+ std::string("j_shoulder_roll_r"),
+ std::string("j_shoulder_roll_l"),
+ std::string("j_elbow_r"),
+ std::string("j_elbow_l"),
+ std::string("j_hip_yaw_r"),
+ std::string("j_hip_yaw_l"),
+ std::string("j_hip_roll_r"),
+ std::string("j_hip_roll_l"),
+ std::string("j_hip_pitch_r"),
+ std::string("j_hip_pitch_l"),
+ std::string("j_knee_r"),
+ std::string("j_knee_l"),
+ std::string("j_ankle_pitch_r"),
+ std::string("j_ankle_pitch_l"),
+ std::string("j_ankle_roll_r"),
+ std::string("j_ankle_roll_l"),
std::string("j_pan"),
std::string("j_tilt"),
std::string("Servo21"),
@@ -74,6 +76,12 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&this->manager_status);
usleep(100000);
}while(this->manager_status&MANAGER_SCANNING);
+ /* get the number of servos detected */
+ this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->manager_num_servos);
+ /* get the current action status */
+ this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&this->action_status);
+ /* get the current walking status */
+ this->robot_device->read_byte_register(DARWIN_WALK_CNTRL,&this->walk_status);
}
else
{
@@ -442,8 +450,8 @@ unsigned char CDarwinRobot::mm_get_num_servos(void)
{
if(this->robot_device!=NULL)
{
- this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->num_servos);
- return this->num_servos;
+ this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->manager_num_servos);
+ return this->manager_num_servos;
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -824,271 +832,620 @@ std::vector<double> CDarwinRobot::mm_get_servo_offsets(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-/*void CDarwinRobot::mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
-{
- this->robot_device->write_byte_register(DARWIN_MM_KNEE_GAIN,(unsigned char)(knee*64.0));
- this->robot_device->write_byte_register(DARWIN_MM_ANKLE_PITCH_GAIN,(unsigned char)(ankle_pitch*64.0));
- this->robot_device->write_byte_register(DARWIN_MM_HIP_ROLL_GAIN,(unsigned char)(hip_roll*64.0));
- this->robot_device->write_byte_register(DARWIN_MM_ANKLE_ROLL_GAIN,(unsigned char)(ankle_roll*64.0));
-}
-
-void CDarwinRobot::mm_get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll)
-{
- unsigned char value;
-
- this->robot_device->read_byte_register(DARWIN_MM_KNEE_GAIN,&value);
- *knee=value/64.0;
- this->robot_device->read_byte_register(DARWIN_MM_ANKLE_PITCH_GAIN,&value);
- *ankle_pitch=value/64.0;
- this->robot_device->read_byte_register(DARWIN_MM_HIP_ROLL_GAIN,&value);
- *hip_roll=value/64.0;
- this->robot_device->read_byte_register(DARWIN_MM_ANKLE_ROLL_GAIN,&value);
- *ankle_roll=value/64.0;
-}
-
// motion action interface
void CDarwinRobot::action_load_page(unsigned char page_id)
{
- this->robot_device->write_byte_register(DARWIN_ACTION_PAGE,page_id);
+ if(this->robot_device!=NULL)
+ this->robot_device->write_byte_register(DARWIN_ACTION_PAGE,page_id);
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
unsigned char CDarwinRobot::action_get_current_page(void)
{
unsigned char page_id;
- this->robot_device->read_byte_register(DARWIN_ACTION_PAGE,&page_id);
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_ACTION_PAGE,&page_id);
- return page_id;
+ return page_id;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::action_start(void)
{
- this->robot_device->write_byte_register(DARWIN_ACTION_CONTROL,0x01);
+ if(this->robot_device!=NULL)
+ {
+ this->action_status|=ACTION_START;
+ this->robot_device->write_byte_register(DARWIN_ACTION_CNTRL,this->action_status);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
void CDarwinRobot::action_stop(void)
{
- this->robot_device->write_byte_register(DARWIN_ACTION_CONTROL,0x02);
+ if(this->robot_device!=NULL)
+ {
+ this->action_status|=ACTION_STOP;
+ this->robot_device->write_byte_register(DARWIN_ACTION_CNTRL,this->action_status);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
bool CDarwinRobot::action_is_page_running(void)
{
unsigned char status;
- this->robot_device->read_byte_register(DARWIN_ACTION_STATUS,&status);
- if(status&0x01)
- return true;
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&status);
+ if(status&ACTION_STATUS)
+ return true;
+ else
+ return false;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+/* walking interface */
+void CDarwinRobot::walk_set_x_offset(double offset_m)
+{
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ offset=(unsigned char)(offset_m*1000.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_X_OFFSET,offset);
+ }
else
- return false;
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-// walking interface
-void CDarwinRobot::walking_set_speeds(double fw_step,double side_step,double turn)
+double CDarwinRobot::walk_get_x_offset(void)
{
- this->robot_device->write_byte_register(DARWIN_STEP_FW_BW,(unsigned char)fw_step);
- this->robot_device->write_byte_register(DARWIN_STEP_LEFT_RIGHT,(unsigned char)side_step);
- this->robot_device->write_byte_register(DARWIN_STEP_DIRECTION,(unsigned char)(turn*8.0));
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_X_OFFSET,&offset);
+ return ((double)offset)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::walking_start(void)
+void CDarwinRobot::walk_set_y_offset(double offset_m)
{
- this->robot_device->write_byte_register(DARWIN_WALK_CONTROL,0x01);
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ offset=(unsigned char)(offset_m*1000.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_Y_OFFSET,offset);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::walking_stop(void)
+double CDarwinRobot::walk_get_y_offset(void)
{
- this->robot_device->write_byte_register(DARWIN_WALK_CONTROL,0x02);
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_Y_OFFSET,&offset);
+ return ((double)offset)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-bool CDarwinRobot::is_walking(void)
+double CDarwinRobot::walk_get_z_offset(void)
{
- unsigned char status;
-
- this->robot_device->read_byte_register(DARWIN_WALK_STATUS,&status);
- if(status==0x01)
- return true;
- else
- return false;
-}
-
-void CDarwinRobot::walking_set_param(int param_id,double value)
-{
- switch(param_id)
- {
- case DARWIN_X_OFFSET:
- case DARWIN_Y_OFFSET:
- case DARWIN_Z_OFFSET:
- case DARWIN_STEP_FW_BW:
- case DARWIN_FOOT_HEIGHT:
- case DARWIN_SWING_RIGHT_LEFT:
- case DARWIN_SWING_TOP_DOWN:
- case DARWIN_P_GAIN:
- case DARWIN_I_GAIN:
- case DARWIN_D_GAIN:
- case DARWIN_STEP_LEFT_RIGHT: this->robot_device->write_byte_register(param_id,(char)value);
- break;
- case DARWIN_ROLL:
- case DARWIN_PITCH:
- case DARWIN_YAW:
- case DARWIN_PELVIS_OFFSET:
- case DARWIN_STEP_DIRECTION: this->robot_device->write_byte_register(param_id,(char)(value*8.0));
- break;
- case DARWIN_HIP_PITCH_OFF_L: this->robot_device->write_word_register(param_id,(short int)(value*1024.0));
- break;
- case DARWIN_PERIOD_TIME_L: this->robot_device->write_word_register(param_id,(short int)(value));
- break;
- case DARWIN_DSP_RATIO:
- case DARWIN_STEP_FW_BW_RATIO: this->robot_device->write_byte_register(param_id,(char)(value*128.0));
- break;
- case DARWIN_ARM_SWING_GAIN: this->robot_device->write_byte_register(param_id,(char)(value*32.0));
- break;
- case DARWIN_MAX_ACC:
- case DARWIN_MAX_ROT_ACC: this->robot_device->write_byte_register(param_id,(char)(value*1024));
- break;
- }
-}
-
-double CDarwinRobot::walking_get_param(int param_id)
-{
- unsigned char byte_value;
- unsigned short int word_value;
- double value;
-
- switch(param_id)
- {
- case DARWIN_X_OFFSET:
- case DARWIN_Y_OFFSET:
- case DARWIN_Z_OFFSET:
- case DARWIN_STEP_FW_BW:
- case DARWIN_FOOT_HEIGHT:
- case DARWIN_SWING_RIGHT_LEFT:
- case DARWIN_SWING_TOP_DOWN:
- case DARWIN_P_GAIN:
- case DARWIN_I_GAIN:
- case DARWIN_D_GAIN:
- case DARWIN_STEP_LEFT_RIGHT: this->robot_device->read_byte_register(param_id,&byte_value);
- value=(double)((char)byte_value);
- break;
- case DARWIN_ROLL:
- case DARWIN_PITCH:
- case DARWIN_YAW:
- case DARWIN_PELVIS_OFFSET:
- case DARWIN_STEP_DIRECTION: this->robot_device->read_byte_register(param_id,&byte_value);
- value=((double)((char)byte_value))/8.0;
- break;
- case DARWIN_HIP_PITCH_OFF_L: this->robot_device->read_word_register(param_id,&word_value);
- value=((double)((short int)word_value))/1024.0;
- break;
- case DARWIN_PERIOD_TIME_L: this->robot_device->read_word_register(param_id,&word_value);
- value=((double)((short int)word_value));
- break;
- case DARWIN_DSP_RATIO:
- case DARWIN_STEP_FW_BW_RATIO: this->robot_device->read_byte_register(param_id,&byte_value);
- value=((double)((char)byte_value))/128.0;
- break;
- case DARWIN_ARM_SWING_GAIN: this->robot_device->read_byte_register(param_id,&byte_value);
- value=((double)((char)byte_value))/32.0;
- break;
- case DARWIN_MAX_ACC:
- case DARWIN_MAX_ROT_ACC: this->robot_device->read_byte_register(param_id,&byte_value);
- value=((double)((char)byte_value))/1024.0;
- break;
- }
-
- return value;
-}
-
-// joints interface
-void CDarwinRobot::joints_load(std::vector<int> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_Y_OFFSET,&offset);
+ return ((double)offset)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_roll_offset(double offset_rad)
{
- unsigned int i=0;
-
- for(i=0;i<servos.size();i++)
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ offset=(unsigned char)((offset_rad*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_ROLL_OFFSET,offset);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::walk_get_roll_offset(void)
+{
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_ROLL_OFFSET,&offset);
+ return (((double)offset)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_pitch_offset(double offset_rad)
+{
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
{
- this->robot_device->write_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servos[i]*2,((unsigned short int)(angles[i]*128.0)));
- this->robot_device->write_byte_register(DARWIN_SERVO_0_SPEED+servos[i],(unsigned char)speeds[i]);
- this->robot_device->write_byte_register(DARWIN_SERVO_0_ACCEL+servos[i],(unsigned char)accels[i]);
+ offset=(unsigned char)((offset_rad*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_PITCH_OFFSET,offset);
}
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::joints_start(void)
+double CDarwinRobot::walk_get_pitch_offset(void)
{
- this->robot_device->write_byte_register(DARWIN_JOINTS_CONTROL,0x01);
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_PITCH_OFFSET,&offset);
+ return (((double)offset)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::joints_stop(void)
+void CDarwinRobot::walk_set_yaw_offset(double offset_rad)
{
- this->robot_device->write_byte_register(DARWIN_JOINTS_CONTROL,0x02);
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ offset=(unsigned char)((offset_rad*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_YAW_OFFSET,offset);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-bool CDarwinRobot::are_joints_moving(void)
+double CDarwinRobot::walk_get_yaw_offset(void)
{
- unsigned char status;
+ unsigned char offset;
- this->robot_device->read_byte_register(DARWIN_JOINTS_STATUS,&status);
- if(status==0x01)
- return true;
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_YAW_OFFSET,&offset);
+ return (((double)offset)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_hip_picth_offset(double offset_rad)
+{
+ unsigned short int offset;
+
+ if(this->robot_device!=NULL)
+ {
+ offset=(unsigned char)((offset_rad*180.0/PI)*1024.0);
+ this->robot_device->write_word_register(DARWIN_WALK_HIP_PITCH_OFF_L,offset);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_period(double period_s)
+{
+ unsigned short int period;
+
+ if(this->robot_device!=NULL)
+ {
+ period=(unsigned char)(period_s*1000.0);
+ this->robot_device->write_word_register(DARWIN_WALK_PERIOD_TIME_L,period);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::walk_get_period(void)
+{
+ unsigned short int period;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(DARWIN_WALK_PERIOD_TIME_L,&period);
+ return ((double)period)/1000.0;
+ }
else
- return false;
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-// head tracking interface
-void CDarwinRobot::head_tracking_set_target(double pan,double tilt)
+void CDarwinRobot::walk_set_ssp_dsp_ratio(double ratio)
{
- this->robot_device->write_word_register(DARWIN_HEAD_PAN_L,(unsigned short int)(pan*128.0));
- this->robot_device->write_word_register(DARWIN_HEAD_TILT_L,(unsigned short int)(tilt*128.0));
+ unsigned char ratio_int;
+
+ if(this->robot_device!=NULL)
+ {
+ ratio_int=(unsigned char)(ratio*256.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_DSP_RATIO,ratio_int);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::head_tracking_set_pan_pid(double p,double i,double d)
+double CDarwinRobot::walk_get_ssp_dsp_ratio(void)
{
- this->robot_device->write_byte_register(DARWIN_HEAD_PAN_P,(unsigned char)(p*1024.0));
- this->robot_device->write_byte_register(DARWIN_HEAD_PAN_I,(unsigned char)(i*1024.0));
- this->robot_device->write_byte_register(DARWIN_HEAD_PAN_D,(unsigned char)(d*1024.0));
+ unsigned char ratio_int;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_DSP_RATIO,&ratio_int);
+ return ((double)ratio_int)/256.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::head_tracking_set_tilt_pid(double p,double i,double d)
+void CDarwinRobot::walk_set_fwd_bwd_ratio(double ratio)
{
- this->robot_device->write_byte_register(DARWIN_HEAD_TILT_P,(unsigned char)(p*1024.0));
- this->robot_device->write_byte_register(DARWIN_HEAD_TILT_I,(unsigned char)(i*1024.0));
- this->robot_device->write_byte_register(DARWIN_HEAD_TILT_D,(unsigned char)(d*1024.0));
+ unsigned char ratio_int;
+
+ if(this->robot_device!=NULL)
+ {
+ ratio_int=(unsigned char)(ratio*256.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_STEP_FW_BW_RATIO,ratio_int);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::head_tracking_start(void)
+double CDarwinRobot::walk_get_fwd_bwd_ratio(void)
{
- this->robot_device->write_byte_register(DARWIN_HEAD_CONTROL,0x01);
+ unsigned char ratio_int;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_STEP_FW_BW_RATIO,&ratio_int);
+ return ((double)ratio_int)/256.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::head_tracking_stop(void)
+void CDarwinRobot::walk_set_foot_height(double height_m)
{
- this->robot_device->write_byte_register(DARWIN_HEAD_CONTROL,0x00);
+ unsigned char height;
+
+ if(this->robot_device!=NULL)
+ {
+ height=(unsigned char)(height_m*1000.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_FOOT_HEIGHT,height);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-bool CDarwinRobot::is_head_tracking(void)
+double CDarwinRobot::walk_get_foot_height(void)
{
- unsigned char data;
+ unsigned char height;
- this->robot_device->read_byte_register(DARWIN_HEAD_STATUS,&data);
- if(data==0x00)
- return false;
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_FOOT_HEIGHT,&height);
+ return ((double)height)/1000.0;
+ }
else
- return true;
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-/ gripper interface
-void CDarwinRobot::gripper_open(grippers_t gripper)
+void CDarwinRobot::walk_set_left_right_swing(double swing_m)
{
- if(gripper==LEFT_GRIPPER)
- this->robot_device->write_byte_register(DARWIN_GRIPPER_CONTROL,0x01);
+ unsigned char swing;
+
+ if(this->robot_device!=NULL)
+ {
+ swing=(unsigned char)(swing_m*1000.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_SWING_RIGHT_LEFT,swing);
+ }
else
- this->robot_device->write_byte_register(DARWIN_GRIPPER_CONTROL,0x04);
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwinRobot::gripper_close(grippers_t gripper)
+double CDarwinRobot::walk_get_left_right_swing(void)
{
- if(gripper==LEFT_GRIPPER)
- this->robot_device->write_byte_register(DARWIN_GRIPPER_CONTROL,0x02);
+ unsigned char swing;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_SWING_RIGHT_LEFT,&swing);
+ return ((double)swing)/1000.0;
+ }
else
- this->robot_device->write_byte_register(DARWIN_GRIPPER_CONTROL,0x08);
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_top_down_swing(double swing_m)
+{
+ unsigned char swing;
+
+ if(this->robot_device!=NULL)
+ {
+ swing=(unsigned char)(swing_m*1000.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_SWING_TOP_DOWN,swing);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::walk_get_top_down_swing(void)
+{
+ unsigned char swing;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_SWING_TOP_DOWN,&swing);
+ return ((double)swing)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_pelvis_offset(double offset_rad)
+{
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ offset=(unsigned char)((offset_rad*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_PELVIS_OFFSET,offset);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::walk_get_pelvis_offset(void)
+{
+ unsigned char offset;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_PELVIS_OFFSET,&offset);
+ return (((double)offset)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_arm_swing_gain(double gain)
+{
+ unsigned char gain_int;
+
+ if(this->robot_device!=NULL)
+ {
+ gain_int=(unsigned char)(gain*32.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_ARM_SWING_GAIN,gain_int);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::walk_get_arm_swing_gain(void)
+{
+ unsigned char gain_int;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_ARM_SWING_GAIN,&gain_int);
+ return ((double)gain_int)*32.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_trans_speed(double speed_m_s)
+{
+ unsigned char speed;
+
+ if(this->robot_device!=NULL)
+ {
+ speed=(unsigned char)(speed_m_s*1000.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_MAX_VEL,speed);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::walk_get_trans_speed(void)
+{
+ unsigned char speed;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_MAX_VEL,&speed);
+ return ((double)speed)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_rot_speed(double speed_rad_s)
+{
+ unsigned char speed;
+
+ if(this->robot_device!=NULL)
+ {
+ speed=(unsigned char)((speed_rad_s*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_MAX_ROT_VEL,speed);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::walk_get_rot_speed(void)
+{
+ unsigned char speed;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_MAX_ROT_VEL,&speed);
+ return (((double)speed)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_start(void)
+{
+ if(this->robot_device!=NULL)
+ {
+ this->walk_status|=WALK_START;
+ this->robot_device->write_byte_register(DARWIN_WALK_CNTRL,this->walk_status);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_stop(void)
+{
+ if(this->robot_device!=NULL)
+ {
+ this->walk_status|=WALK_STOP;
+ this->robot_device->write_byte_register(DARWIN_WALK_CNTRL,this->walk_status);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+bool CDarwinRobot::is_walking(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_CNTRL,&value);
+ if(value&WALK_STATUS)
+ return true;
+ else
+ return false;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_x_step(double step_m)
+{
+ unsigned char step;
+
+ if(this->robot_device!=NULL)
+ {
+ step=(unsigned char)(step_m*1000.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_STEP_FW_BW,step);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::walk_get_x_step(void)
+{
+ unsigned char step;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_STEP_FW_BW,&step);
+ return ((double)step)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_y_step(double step_m)
+{
+ unsigned char step;
+
+ if(this->robot_device!=NULL)
+ {
+ step=(unsigned char)(step_m*1000.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_STEP_LEFT_RIGHT,step);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::walk_get_y_step(void)
+{
+ unsigned char step;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_STEP_LEFT_RIGHT,&step);
+ return ((double)step)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::walk_set_turn_step(double step_rad)
+{
+ unsigned char step;
+
+ if(this->robot_device!=NULL)
+ {
+ step=(unsigned char)((step_rad*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_WALK_STEP_DIRECTION,step);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::walk_get_turn_step(void)
+{
+ unsigned char step;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_WALK_STEP_DIRECTION,&step);
+ return (((double)step)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+/*void CDarwinRobot::mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
+{
+ this->robot_device->write_byte_register(DARWIN_MM_KNEE_GAIN,(unsigned char)(knee*64.0));
+ this->robot_device->write_byte_register(DARWIN_MM_ANKLE_PITCH_GAIN,(unsigned char)(ankle_pitch*64.0));
+ this->robot_device->write_byte_register(DARWIN_MM_HIP_ROLL_GAIN,(unsigned char)(hip_roll*64.0));
+ this->robot_device->write_byte_register(DARWIN_MM_ANKLE_ROLL_GAIN,(unsigned char)(ankle_roll*64.0));
+}
+
+void CDarwinRobot::mm_get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll)
+{
+ unsigned char value;
+
+ this->robot_device->read_byte_register(DARWIN_MM_KNEE_GAIN,&value);
+ *knee=value/64.0;
+ this->robot_device->read_byte_register(DARWIN_MM_ANKLE_PITCH_GAIN,&value);
+ *ankle_pitch=value/64.0;
+ this->robot_device->read_byte_register(DARWIN_MM_HIP_ROLL_GAIN,&value);
+ *hip_roll=value/64.0;
+ this->robot_device->read_byte_register(DARWIN_MM_ANKLE_ROLL_GAIN,&value);
+ *ankle_roll=value/64.0;
}*/
CDarwinRobot::~CDarwinRobot()
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index 27bab9794aa7edc05f66513d476d9a1774b54e62..116487ec93198c4e469b80d52d35a7bd35cafec0 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -44,8 +44,12 @@ class CDarwinRobot
std::string robot_name;
unsigned char robot_id;
// motion manager variables
- unsigned char num_servos;
+ unsigned char manager_num_servos;
unsigned char manager_status;
+ /* action status */
+ unsigned char action_status;
+ /* walk status */
+ unsigned char walk_status;
public:
CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
// GPIO interface
@@ -108,28 +112,50 @@ class CDarwinRobot
void action_start(void);
void action_stop(void);
bool action_is_page_running(void);
- // walking interface
- void walking_set_speeds(double fw_step,double side_step,double turn);
- void walking_start(void);
- void walking_stop(void);
+ /* walking interface */
+ 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_rad);
+ double walk_get_pitch_offset(void);
+ void walk_set_yaw_offset(double offset_rad);
+ double walk_get_yaw_offset(void);
+ void walk_set_hip_picth_offset(double offset_rad);
+ double walk_get_hip_pitch_offset(void);
+ void walk_set_period(double period_ms);
+ 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(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_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 walking_set_param(int param_id,double value);
- double walking_get_param(int param_id);
- // joint motion interface
- void joints_load(std::vector<int> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
- void joints_start(void);
- void joints_stop(void);
- bool are_joints_moving(void);
- // head tracking interface
- void head_tracking_set_target(double pan,double tilt);
- void head_tracking_set_pan_pid(double p,double i,double d);
- void head_tracking_set_tilt_pid(double p,double i,double d);
- void head_tracking_start(void);
- void head_tracking_stop(void);
- bool is_head_tracking(void);
- // gripper interface
- void gripper_open(grippers_t gripper);
- void gripper_close(grippers_t gripper);
+ 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);
~CDarwinRobot();
};
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 71aa7af0b2993f0f7fe2f1c222d3b53b7f573659..a60b77f414ecc089d7ce6844d570410ad2693b7b 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -19,36 +19,17 @@ ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
# create an example application
-#ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
+ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
+TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
# create an example application
-#ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
+ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
-
-# create an example application
-#ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
-# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
-
-# create an example application
-#ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
-# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
-
-# create an example application
-#ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
-# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
+TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
# create an example application
#ADD_EXECUTABLE(darwin_kin darwin_kin.cpp)
# link necessary libraries
#TARGET_LINK_LIBRARIES(darwin_kin darwin_arm_kinematics)
-# create an example application
-#ADD_EXECUTABLE(darwin_gripper_test darwin_gripper_test.cpp)
-# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_gripper_test darwin_robot)
diff --git a/src/examples/darwin_action_test.cpp b/src/examples/darwin_action_test.cpp
index 465e55da0bf9dc6d204c4b6f62de5b9f49e3feca..f9a6e31d07816395b043161099b29c447f93d465 100644
--- a/src/examples/darwin_action_test.cpp
+++ b/src/examples/darwin_action_test.cpp
@@ -1,29 +1,31 @@
#include "darwin_robot.h"
#include "darwin_robot_exceptions.h"
+#include "action_id.h"
#include <iostream>
-std::string robot_device="A603LOBS";
+//std::string robot_device="A603LOBS";
+std::string robot_device="A4008atn";
int main(int argc, char *argv[])
{
- int i=0,num_v1_servos,num_v2_servos;
+ int i=0,num_servos;
try{
- CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
+ CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
- darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
- std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
+ num_servos=darwin.mm_get_num_servos();
+ std::cout << "Found " << num_servos << " servos " << std::endl;
// enable all servos and assign them to the action module
darwin.mm_enable_power();
- for(i=1;i<=num_v1_servos;i++)
+ for(i=1;i<=num_servos;i++)
{
darwin.mm_enable_servo(i);
darwin.mm_assign_module(i,DARWIN_MM_ACTION);
}
darwin.mm_start();
// execute an action
- darwin.action_load_page(9);
+ darwin.action_load_page(WALK_READY);
darwin.action_start();
while(darwin.action_is_page_running())
usleep(100000);
diff --git a/src/examples/darwin_walking_test.cpp b/src/examples/darwin_walking_test.cpp
index 23a844fb08658365e85f1c9190a0cd9170ad7402..23faa1869decd3dfe4541ed532ee2b1bcc90465d 100644
--- a/src/examples/darwin_walking_test.cpp
+++ b/src/examples/darwin_walking_test.cpp
@@ -1,40 +1,44 @@
#include "darwin_robot.h"
#include "darwin_robot_exceptions.h"
+#include "action_id.h"
#include <iostream>
-std::string robot_device="A603LOBS";
+//std::string robot_device="A603LOBS";
+std::string robot_device="A4008atn";
int main(int argc, char *argv[])
{
- int i=0,num_v1_servos,num_v2_servos;
+ int i=0,num_servos;
std::vector<double> angles;
try{
- CDarwin_Robot darwin("Darwin",robot_device,1000000,0xC0);
- darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
- std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
+ CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+ num_servos=darwin.mm_get_num_servos();
+ std::cout << "Found " << num_servos << " servos" << std::endl;
// enable all servos and assign them to the action module
darwin.mm_enable_power();
- for(i=1;i<=num_v1_servos;i++)
+ for(i=1;i<=num_servos;i++)
{
darwin.mm_enable_servo(i);
darwin.mm_assign_module(i,DARWIN_MM_ACTION);
}
darwin.mm_start();
// execute an action
- darwin.action_load_page(9);
+ darwin.action_load_page(WALK_READY);
darwin.action_start();
while(darwin.action_is_page_running())
usleep(100000);
- for(i=1;i<=num_v1_servos;i++)
+ for(i=1;i<=num_servos;i++)
darwin.mm_assign_module(i,DARWIN_MM_WALKING);
std::cout << "Start walking ..." << std::endl;
- darwin.walking_set_speeds(20,0,0);
- darwin.walking_start();
+ darwin.walk_set_x_step(0.02);
+ darwin.walk_set_y_step(0.0);
+ darwin.walk_set_turn_step(0.0);
+ darwin.walk_start();
sleep(2);
std::cout << "Stop walking ..." << std::endl;
- darwin.walking_stop();
+ darwin.walk_stop();
while(darwin.is_walking())
{
std::cout << "Walking ..." << std::endl;