diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index fa75ab966c875191bdb8143979bc45c63d83d77b..2336f96cc02a28258cc024536d7e83c2b3d5e42c 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -60,7 +60,7 @@ const led_info_t leds[GPIO_NUM_LEDS]={{DARWIN_TX_LED_CNTRL,DARWIN_TX_LED_PERIOD_
CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id)
{
- int num_buses,i;
+ int num_buses;
this->robot_device=NULL;
if(name!="")
@@ -77,7 +77,7 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
do{
/* get the current manager status */
this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&this->manager_status);
- usleep(100000);
+ usleep(10000);
}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);
@@ -869,7 +869,7 @@ void CDarwinRobot::mm_set_servo_offset(unsigned char servo_id,double offset)
else
{
this->robot_device->write_byte_register(DARWIN_MM_SERVO0_OFFSET+servo_id,(unsigned char)((int)(offset*16.0)));
- usleep(10000);
+ usleep(1000);
}
}
}
@@ -1003,6 +1003,7 @@ void CDarwinRobot::walk_set_x_offset(double offset_m)
{
offset=(unsigned char)(offset_m*1000.0);
this->robot_device->write_byte_register(DARWIN_WALK_X_OFFSET,offset);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1029,6 +1030,7 @@ void CDarwinRobot::walk_set_y_offset(double offset_m)
{
offset=(unsigned char)(offset_m*1000.0);
this->robot_device->write_byte_register(DARWIN_WALK_Y_OFFSET,offset);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1055,6 +1057,7 @@ void CDarwinRobot::walk_set_z_offset(double offset_m)
{
offset=(unsigned char)(offset_m*1000.0);
this->robot_device->write_byte_register(DARWIN_WALK_Z_OFFSET,offset);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1081,6 +1084,7 @@ void CDarwinRobot::walk_set_roll_offset(double offset_rad)
{
offset=(unsigned char)((offset_rad*180.0/PI)*8.0);
this->robot_device->write_byte_register(DARWIN_WALK_ROLL_OFFSET,offset);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1107,6 +1111,7 @@ void CDarwinRobot::walk_set_pitch_offset(double offset_rad)
{
offset=(unsigned char)((offset_rad*180.0/PI)*8.0);
this->robot_device->write_byte_register(DARWIN_WALK_PITCH_OFFSET,offset);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1133,6 +1138,7 @@ void CDarwinRobot::walk_set_yaw_offset(double offset_rad)
{
offset=(unsigned char)((offset_rad*180.0/PI)*8.0);
this->robot_device->write_byte_register(DARWIN_WALK_YAW_OFFSET,offset);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1159,6 +1165,7 @@ void CDarwinRobot::walk_set_hip_pitch_offset(double offset_rad)
{
offset=(unsigned short int)((offset_rad*180.0/PI)*1024.0);
this->robot_device->write_word_register(DARWIN_WALK_HIP_PITCH_OFF_L,offset);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1185,6 +1192,7 @@ void CDarwinRobot::walk_set_period(double period_s)
{
period=(unsigned short int)(period_s*1000.0);
this->robot_device->write_word_register(DARWIN_WALK_PERIOD_TIME_L,period);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1211,6 +1219,7 @@ void CDarwinRobot::walk_set_ssp_dsp_ratio(double ratio)
{
ratio_int=(unsigned char)(ratio*256.0);
this->robot_device->write_byte_register(DARWIN_WALK_DSP_RATIO,ratio_int);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1237,6 +1246,7 @@ void CDarwinRobot::walk_set_fwd_bwd_ratio(double ratio)
{
ratio_int=(unsigned char)(ratio*256.0);
this->robot_device->write_byte_register(DARWIN_WALK_STEP_FW_BW_RATIO,ratio_int);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1263,6 +1273,7 @@ void CDarwinRobot::walk_set_foot_height(double height_m)
{
height=(unsigned char)(height_m*1000.0);
this->robot_device->write_byte_register(DARWIN_WALK_FOOT_HEIGHT,height);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1289,6 +1300,7 @@ void CDarwinRobot::walk_set_left_right_swing(double swing_m)
{
swing=(unsigned char)(swing_m*1000.0);
this->robot_device->write_byte_register(DARWIN_WALK_SWING_RIGHT_LEFT,swing);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1315,6 +1327,7 @@ void CDarwinRobot::walk_set_top_down_swing(double swing_m)
{
swing=(unsigned char)(swing_m*1000.0);
this->robot_device->write_byte_register(DARWIN_WALK_SWING_TOP_DOWN,swing);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1341,6 +1354,7 @@ void CDarwinRobot::walk_set_pelvis_offset(double offset_rad)
{
offset=(unsigned char)((offset_rad*180.0/PI)*8.0);
this->robot_device->write_byte_register(DARWIN_WALK_PELVIS_OFFSET,offset);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1367,6 +1381,7 @@ void CDarwinRobot::walk_set_arm_swing_gain(double gain)
{
gain_int=(unsigned char)(gain*32.0);
this->robot_device->write_byte_register(DARWIN_WALK_ARM_SWING_GAIN,gain_int);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1393,6 +1408,7 @@ void CDarwinRobot::walk_set_trans_speed(double speed_m_s)
{
speed=(unsigned char)(speed_m_s*1000.0);
this->robot_device->write_byte_register(DARWIN_WALK_MAX_VEL,speed);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1419,6 +1435,7 @@ void CDarwinRobot::walk_set_rot_speed(double speed_rad_s)
{
speed=(unsigned char)((speed_rad_s*180.0/PI)*8.0);
this->robot_device->write_byte_register(DARWIN_WALK_MAX_ROT_VEL,speed);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1547,6 +1564,561 @@ double CDarwinRobot::walk_get_turn_step(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
+// stairs interface
+void CDarwinRobot::stairs_go_up(void)
+{
+ if(this->robot_device!=NULL)
+ this->robot_device->write_byte_register(DARWIN_STAIRS_CNTRL,STAIRS_START_UP);
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_go_down(void)
+{
+ if(this->robot_device!=NULL)
+ this->robot_device->write_byte_register(DARWIN_STAIRS_CNTRL,STAIRS_START_DOWN);
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_stop(void)
+{
+ if(this->robot_device!=NULL)
+ this->robot_device->write_byte_register(DARWIN_STAIRS_CNTRL,STAIRS_STOP);
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+bool CDarwinRobot::stairs_is_active(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_CNTRL,&value);
+ if(value&STAIRS_STATUS)
+ return true;
+ else
+ return false;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+stairs_phase_t CDarwinRobot::stairs_get_phase(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_CNTRL,&value);
+ return (stairs_phase_t)(value&STAIRS_PHASE);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_phase_time(stairs_phase_t phase_id,double time)
+{
+ unsigned short int address,value;
+
+ if(this->robot_device!=NULL)
+ {
+ switch(phase_id)
+ {
+ case SHIFT_WEIGHT_LEFT: address=DARWIN_STAIRS_PHASE1_TIME_L;
+ break;
+ case RISE_RIGHT_FOOT: address=DARWIN_STAIRS_PHASE2_TIME_L;
+ break;
+ case ADVANCE_RIGHT_FOOT: address=DARWIN_STAIRS_PHASE3_TIME_L;
+ break;
+ case CONTACT_RIGHT_FOOT: address=DARWIN_STAIRS_PHASE4_TIME_L;
+ break;
+ case SHIFT_WEIGHT_RIGHT: address=DARWIN_STAIRS_PHASE5_TIME_L;
+ break;
+ case RISE_LEFT_FOOT: address=DARWIN_STAIRS_PHASE6_TIME_L;
+ break;
+ case ADVANCE_LEFT_FOOT: address=DARWIN_STAIRS_PHASE7_TIME_L;
+ break;
+ case CONTACT_LEFT_FOOT: address=DARWIN_STAIRS_PHASE8_TIME_L;
+ break;
+ case CENTER_WEIGHT: address=DARWIN_STAIRS_PHASE9_TIME_L;
+ break;
+ }
+ value=time*1000.0;
+ this->robot_device->write_word_register(address,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_phase_time(stairs_phase_t phase_id)
+{
+ unsigned short int address,value;
+
+ if(this->robot_device!=NULL)
+ {
+ switch(phase_id)
+ {
+ case SHIFT_WEIGHT_LEFT: address=DARWIN_STAIRS_PHASE1_TIME_L;
+ break;
+ case RISE_RIGHT_FOOT: address=DARWIN_STAIRS_PHASE2_TIME_L;
+ break;
+ case ADVANCE_RIGHT_FOOT: address=DARWIN_STAIRS_PHASE3_TIME_L;
+ break;
+ case CONTACT_RIGHT_FOOT: address=DARWIN_STAIRS_PHASE4_TIME_L;
+ break;
+ case SHIFT_WEIGHT_RIGHT: address=DARWIN_STAIRS_PHASE5_TIME_L;
+ break;
+ case RISE_LEFT_FOOT: address=DARWIN_STAIRS_PHASE6_TIME_L;
+ break;
+ case ADVANCE_LEFT_FOOT: address=DARWIN_STAIRS_PHASE7_TIME_L;
+ break;
+ case CONTACT_LEFT_FOOT: address=DARWIN_STAIRS_PHASE8_TIME_L;
+ break;
+ case CENTER_WEIGHT: address=DARWIN_STAIRS_PHASE9_TIME_L;
+ break;
+ }
+ this->robot_device->read_word_register(address,&value);
+ return ((double)value)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_x_offset(double offset_m)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=offset_m*1000.0;
+ this->robot_device->write_byte_register(DARWIN_STAIRS_X_OFFSET,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_x_offset(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_X_OFFSET,&value);
+ return ((double)value)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_y_offset(double offset_m)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=offset_m*1000.0;
+ this->robot_device->write_byte_register(DARWIN_STAIRS_Y_OFFSET,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_y_offset(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_Y_OFFSET,&value);
+ return ((double)value)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_z_offset(double offset_m)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=offset_m*1000.0;
+ this->robot_device->write_byte_register(DARWIN_STAIRS_Z_OFFSET,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_z_offset(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_Z_OFFSET,&value);
+ return ((double)value)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_roll_offset(double offset_rad)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=(unsigned char)((offset_rad*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_STAIRS_R_OFFSET,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_roll_offset(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_R_OFFSET,&value);
+ return (((double)value)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_pitch_offset(double offset_rad)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=(unsigned char)((offset_rad*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_STAIRS_P_OFFSET,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_pitch_offset(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_P_OFFSET,&value);
+ return (((double)value)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_yaw_offset(double offset_rad)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=(unsigned char)((offset_rad*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_STAIRS_A_OFFSET,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_yaw_offset(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_A_OFFSET,&value);
+ return (((double)value)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_y_shift(double distance)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=distance*1000.0;
+ this->robot_device->write_byte_register(DARWIN_STAIRS_Y_SHIFT,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_y_shift(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_Y_SHIFT,&value);
+ return ((double)value)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_x_shift(double distance)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=distance*1000.0;
+ this->robot_device->write_byte_register(DARWIN_STAIRS_X_SHIFT,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_x_shift(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_X_SHIFT,&value);
+ return ((double)value)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_z_overshoot(double distance)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=distance*1000.0;
+ this->robot_device->write_byte_register(DARWIN_STAIRS_Z_OVERSHOOT,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_z_overshoot(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_Z_OVERSHOOT,&value);
+ return ((double)value)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_z_height(double distance)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=distance*1000.0;
+ this->robot_device->write_byte_register(DARWIN_STAIRS_Z_HEIGHT,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_z_height(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_Z_HEIGHT,&value);
+ return ((double)value)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_hip_pitch_offset(double offset_rad)
+{
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=(unsigned short int)((offset_rad*180.0/PI)*1024.0);
+ this->robot_device->write_word_register(DARWIN_STAIRS_HIP_PITCH_OFF_L,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_hip_pitch_offset(void)
+{
+ unsigned short int value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_word_register(DARWIN_STAIRS_HIP_PITCH_OFF_L,&value);
+ return (((double)value)*PI/180.0)/1024.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_roll_shift(double angle)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=(unsigned char)((angle*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_STAIRS_R_SHIFT,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_roll_shift(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_R_SHIFT,&value);
+ return (((double)value)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_pitch_shift(double angle)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=(unsigned char)((angle*180.0/PI)*8.0);
+ this->robot_device->write_byte_register(DARWIN_STAIRS_P_SHIFT,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_pitch_shift(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_P_SHIFT,&value);
+ return (((double)value)*PI/180.0)/8.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_yaw_shift(double angle)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=(unsigned char)((angle*180.0/PI)*4.0);
+ this->robot_device->write_byte_register(DARWIN_STAIRS_A_SHIFT,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_yaw_shift(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_A_SHIFT,&value);
+ return (((double)value)*PI/180.0)/4.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_y_spread(double distance)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=distance*1000.0;
+ this->robot_device->write_byte_register(DARWIN_STAIRS_Y_SPREAD,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_y_spread(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_Y_SPREAD,&value);
+ return ((double)value)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::stairs_set_x_shift_body(double distance)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ value=distance*1000.0;
+ this->robot_device->write_byte_register(DARWIN_STAIRS_X_SHIFT_BODY,value);
+ usleep(10000);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+double CDarwinRobot::stairs_get_x_shift_body(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_STAIRS_X_SHIFT_BODY,&value);
+ return ((double)value)/1000.0;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
// joint motion interface
void CDarwinRobot::joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
{
@@ -1636,16 +2208,16 @@ void CDarwinRobot::head_set_pan_pid(double p,double i,double d,double i_clamp)
{
value=p*65536.0;
this->robot_device->write_word_register(DARWIN_HEAD_PAN_P_L,value);
- usleep(100000);
+ usleep(10000);
value=i*65536.0;
this->robot_device->write_word_register(DARWIN_HEAD_PAN_I_L,value);
- usleep(100000);
+ usleep(10000);
value=d*65536.0;
this->robot_device->write_word_register(DARWIN_HEAD_PAN_D_L,value);
- usleep(100000);
+ usleep(10000);
value=i_clamp*128.0;
this->robot_device->write_word_register(DARWIN_HEAD_PAN_I_CLAMP_L,value);
- usleep(100000);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1678,16 +2250,16 @@ void CDarwinRobot::head_set_tilt_pid(double p,double i,double d,double i_clamp)
{
value=p*65536.0;
this->robot_device->write_word_register(DARWIN_HEAD_TILT_P_L,value);
- usleep(100000);
+ usleep(10000);
value=i*65536.0;
this->robot_device->write_word_register(DARWIN_HEAD_TILT_I_L,value);
- usleep(100000);
+ usleep(10000);
value=d*65536.0;
this->robot_device->write_word_register(DARWIN_HEAD_TILT_D_L,value);
- usleep(100000);
+ usleep(10000);
value=i_clamp*128.0;
this->robot_device->write_word_register(DARWIN_HEAD_TILT_I_CLAMP_L,value);
- usleep(100000);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -1998,7 +2570,7 @@ void CDarwinRobot::gripper_set_max_angle(grippers_t gripper_id,double max_angle)
this->robot_device->write_word_register(DARWIN_GRIPPER_LEFT_MAX_ANGLE_L,value);
else
this->robot_device->write_word_register(DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L,value);
- usleep(100000);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -2031,7 +2603,7 @@ void CDarwinRobot::gripper_set_min_angle(grippers_t gripper_id,double min_angle)
this->robot_device->write_word_register(DARWIN_GRIPPER_LEFT_MIN_ANGLE_L,value);
else
this->robot_device->write_word_register(DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L,value);
- usleep(100000);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -2063,7 +2635,7 @@ void CDarwinRobot::gripper_set_max_force(grippers_t gripper_id,unsigned short in
this->robot_device->write_word_register(DARWIN_GRIPPER_LEFT_MAX_FORCE_L,max_force);
else
this->robot_device->write_word_register(DARWIN_GRIPPER_RIGHT_MAX_FORCE_L,max_force);
- usleep(100000);
+ usleep(10000);
}
else
throw CDarwinRobotException(_HERE_,"Invalid robot device");
@@ -2092,16 +2664,16 @@ void CDarwinRobot::gripper_set_servo_ids(grippers_t gripper_id, unsigned char to
if(gripper_id==LEFT_GRIPPER)
{
this->robot_device->write_byte_register(DARWIN_GRIPPER_LEFT_TOP_ID,top_id);
- usleep(100000);
+ usleep(10000);
this->robot_device->write_byte_register(DARWIN_GRIPPER_LEFT_BOT_ID,bot_id);
- usleep(100000);
+ usleep(10000);
}
else
{
this->robot_device->write_byte_register(DARWIN_GRIPPER_RIGHT_TOP_ID,top_id);
- usleep(100000);
+ usleep(10000);
this->robot_device->write_byte_register(DARWIN_GRIPPER_RIGHT_BOT_ID,bot_id);
- usleep(100000);
+ usleep(10000);
}
}
else
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index b3f613082e7cb92f9a77a1f6b5432415a5313dbc..8c3f31a142013abdceaf9c8b6772275962cb4504 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -40,6 +40,8 @@ typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH
typedef enum {JOINTS_GRP0=0,JOINTS_GRP1=1,JOINTS_GRP2=2,JOINTS_GRP3=2} joints_grp_t;
+typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t;
+
//smart charger read data
#pragma pack (push, 1)
typedef struct
@@ -180,6 +182,46 @@ class CDarwinRobot
double walk_get_y_step(void);
void walk_set_turn_step(double step_rad);
double walk_get_turn_step(void);
+ // stairs interface
+ void stairs_go_up(void);
+ void stairs_go_down(void);
+ void stairs_stop(void);
+ bool stairs_is_active(void);
+ stairs_phase_t stairs_get_phase(void);
+ void stairs_set_phase_time(stairs_phase_t phase_id,double time);
+ double stairs_get_phase_time(stairs_phase_t phase_id);
+ void stairs_set_x_offset(double offset_m);
+ double stairs_get_x_offset(void);
+ void stairs_set_y_offset(double offset_m);
+ double stairs_get_y_offset(void);
+ void stairs_set_z_offset(double offset_m);
+ double stairs_get_z_offset(void);
+ void stairs_set_roll_offset(double offset_rad);
+ double stairs_get_roll_offset(void);
+ void stairs_set_pitch_offset(double offset_rad);
+ double stairs_get_pitch_offset(void);
+ void stairs_set_yaw_offset(double offset_rad);
+ double stairs_get_yaw_offset(void);
+ void stairs_set_y_shift(double distance);
+ double stairs_get_y_shift(void);
+ void stairs_set_x_shift(double distance);
+ double stairs_get_x_shift(void);
+ void stairs_set_z_overshoot(double distance);
+ double stairs_get_z_overshoot(void);
+ void stairs_set_z_height(double distance);
+ double stairs_get_z_height(void);
+ void stairs_set_hip_pitch_offset(double offset_rad);
+ double stairs_get_hip_pitch_offset(void);
+ void stairs_set_roll_shift(double angle);
+ double stairs_get_roll_shift(void);
+ void stairs_set_pitch_shift(double angle);
+ double stairs_get_pitch_shift(void);
+ void stairs_set_yaw_shift(double angle);
+ double stairs_get_yaw_shift(void);
+ void stairs_set_y_spread(double distance);
+ double stairs_get_y_spread(void);
+ void stairs_set_x_shift_body(double distance);
+ double stairs_get_x_shift_body(void);
// joint motion interface
void joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
std::vector<unsigned char> joints_get_group_servos(joints_grp_t group);
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 795bccb1ed62003cfbc8d51f2c63445b4a754c7b..ff70e9bc45b314d70850cd3f631a14fa076eaa93 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -28,6 +28,11 @@ 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_stairs_test darwin_stairs_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(darwin_stairs_test darwin_robot)
+
# create an example application
ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
# link necessary libraries
diff --git a/src/examples/darwin_stairs_test.cpp b/src/examples/darwin_stairs_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b31f59be7caed8b534665d6cd2797007d6b87ead
--- /dev/null
+++ b/src/examples/darwin_stairs_test.cpp
@@ -0,0 +1,69 @@
+#include "darwin_robot.h"
+#include "darwin_robot_exceptions.h"
+#include "action_id.h"
+
+#include <iostream>
+
+std::string robot_device="A603LOBS";
+//std::string robot_device="A4008atn";
+
+int main(int argc, char *argv[])
+{
+ int i=0,num_servos;
+ std::vector<double> angles;
+ unsigned int present_servos;
+
+ try{
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
+ num_servos=darwin.mm_get_num_servos();
+ present_servos=darwin.mm_get_present_servos();
+ std::cout << "Found " << num_servos << " servos" << std::endl;
+ std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl;
+ // enable all servos and assign them to the action module
+ darwin.mm_enable_power();
+ sleep(1);
+ // printf parameters
+ std::cout << darwin.stairs_get_phase(SHIFT_WEIGHT_LEFT) << std::endl;
+ std::cout << darwin.stairs_get_phase(RISE_RIGHT_FOOT) << std::endl;
+ std::cout << darwin.stairs_get_phase(ADVANCE_RIGHT_FOOT) << std::endl;
+ std::cout << darwin.stairs_get_phase(CONTACT_RIGHT_FOOT) << std::endl;
+ std::cout << darwin.stairs_get_phase(SHIFT_WEIGHT_RIGHT) << std::endl;
+ std::cout << darwin.stairs_get_phase(RISE_LEFT_FOOT) << std::endl;
+ std::cout << darwin.stairs_get_phase(ADVANCE_LEFT_FOOT) << std::endl;
+ std::cout << darwin.stairs_get_phase(CONTACT_LEFT_FOOT) << std::endl;
+ std::cout << darwin.stairs_get_phase(CENTER_WEIGHT) << std::endl;
+ for(i=0;i<MAX_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(WALK_READY);
+ darwin.action_start();
+ while(darwin.action_is_page_running())
+ usleep(100000);
+ std::cout << "starting calibration" << std::endl;
+ darwin.imu_start_gyro_cal();
+ while(darwin.imu_is_gyro_cal_done())
+ usleep(100000);
+ std::cout << "calibration done" << std::endl;
+ darwin.mm_enable_balance();
+ for(i=0;i<MAX_NUM_SERVOS;i++)
+ {
+ darwin.mm_enable_servo(i);
+ darwin.mm_assign_module(i,DARWIN_MM_WALKING);
+ }
+ std::cout << "Go upstairs ..." << std::endl;
+ darwin.stairs_go_up();
+ while(darwin.stairs_is_active())
+ {
+ std::cout << "Going up stairs ..." << std::endl;
+ usleep(100000);
+ }
+ darwin.mm_stop();
+ darwin.mm_disable_power();
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+}