Commit cb90ed51 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Updated the arm control module.

parent ef585d6d
......@@ -18,7 +18,50 @@
#define IMU_MODE 23
#define IMU_CAL 21
#define IMU_NUM_SAMPLES 5
#define IMU_NUM_SAMPLES 1
#ifdef PS3_WIRELESS
#define IMU_TOGGLE 9
#define INC_SPEED 13
#define DEC_SPEED 14
#define TORQUE_EN 6
#define DEAD_MAN_SW 4
#define SAFE_POS 5
#define POS_TRIANGLE 0
#define POS_CIRCLE 1
#define POS_CROSS 2
#define POS_SQUARE 3
#define EXIT 8
#else
#define IMU_TOGGLE 3
#define INC_SPEED 4
#define DEC_SPEED 6
#define TORQUE_EN 10
#define DEAD_MAN_SW 8
#define SAFE_POS 9
#define POS_TRIANGLE 12
#define POS_CIRCLE 13
#define POS_CROSS 14
#define POS_SQUARE 15
#define EXIT 0
#endif
#define MAX_PAN_ANGLE 80.0
#define MIN_PAN ANGLE -80.0
#define MAX_PAN_TORQUE 50.0
#define PAN_DIR 1.0
#define MAX_TILT_ANGLE 80.0
#define MIN_TILT_ANGLE -80.0
#define MAX_TILT_TORQUE 50.0
#define TILT_DIR 1.0
#define MAX_ROLL_ANGLE 80.0
#define MIN_ROLL_ANGLE -80.0
#define MAX_ROLL_TORQUE 50.0
#define ROLL_DIR 1.0
#define MAX_SPEED 200.0
void button_callback(void *param,unsigned int button_id,bool level)
{
......@@ -27,58 +70,58 @@ void button_callback(void *param,unsigned int button_id,bool level)
try{
switch(button_id)
{
case 9: if(level)
arm->toggle_imu();
break;
case 13: if(level)
arm->increase_max_speed();
break;
case 14: if(level)
arm->decrease_max_speed();
break;
case 6: if(level)
arm->enable_torque(false);
else
arm->enable_torque(true);
break;
case 4: if(!level)
{
// button rear left 1 released -> keep position
arm->stop();
}
break;
case 2: if(level)
{
if(arm->get_button_state(5))
arm->update_pose(0);// update circle pose
else
arm->execute_pose(0);// execute circle pose
}
break;
case 1: if(level)
{
if(arm->get_button_state(5))
arm->update_pose(1);// update circle pose
else
arm->execute_pose(1);// execute circle pose
}
break;
case 0: if(level)
{
if(arm->get_button_state(5))
arm->update_pose(2);// update circle pose
else
arm->execute_pose(2);// execute circle pose
}
break;
case 3: if(level)
{
if(arm->get_button_state(5))
arm->update_pose(3);// update circle pose
else
arm->execute_pose(3);// execute circle pose
}
break;
case IMU_TOGGLE: if(level)
arm->toggle_imu();
break;
case INC_SPEED: if(level)
arm->increase_max_speed();
break;
case DEC_SPEED: if(level)
arm->decrease_max_speed();
break;
case TORQUE_EN: if(level)
arm->enable_torque(false);
else
arm->enable_torque(true);
break;
case DEAD_MAN_SW: if(!level)
{
// button rear left 1 released -> keep position
arm->stop();
}
break;
case POS_TRIANGLE: if(level)
{
if(arm->get_button_state(SAFE_POS))
arm->update_pose(0);// update circle pose
else
arm->execute_pose(0);// execute circle pose
}
break;
case POS_CIRCLE: if(level)
{
if(arm->get_button_state(SAFE_POS))
arm->update_pose(1);// update circle pose
else
arm->execute_pose(1);// execute circle pose
}
break;
case POS_CROSS: if(level)
{
if(arm->get_button_state(SAFE_POS))
arm->update_pose(2);// update circle pose
else
arm->execute_pose(2);// execute circle pose
}
break;
case POS_SQUARE: if(level)
{
if(arm->get_button_state(SAFE_POS))
arm->update_pose(3);// update circle pose
else
arm->execute_pose(3);// execute circle pose
}
break;
}
}catch(CException &e){
std::cout << e.what() << std::endl;
......@@ -96,10 +139,10 @@ void axis_callback(void *param,unsigned int axis_id, float value)
arm->move(PAN,value);
break;
case 1: if(arm->get_button_state(4))
arm->move(TILT,value);
arm->move(ROLL,value);
break;
case 3: if(arm->get_button_state(4))
arm->move(ROLL,value);
arm->move(TILT,value);
break;
}
}catch(CException &e){
......@@ -126,36 +169,36 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de
name="arm_control_pan";
this->motors[PAN] = new CDynamixelMotor(name,this->dyn_server,pan_id);
this->motors[PAN]->set_position_range(-150.0,150.0);
this->set_max_angles(PAN,90.0,-90.0);
this->set_max_torque(PAN,50.0);
this->set_max_angles(PAN,MAX_PAN_ANGLE,MIN_PAN_ANGLE);
this->set_max_torque(PAN,MAX_PAN_TORQUE);
this->pan_id=pan_id;
name="arm_control_tilt";
this->motors[TILT] = new CDynamixelMotor(name,this->dyn_server,tilt_id);
this->motors[PAN]->set_position_range(-150.0,150.0);
this->set_max_angles(TILT,90.0,-90.0);
this->set_max_torque(TILT,50.0);
this->set_max_angles(TILT,MAX_TILT_ANGLE,MIN_TILT_ANGLE);
this->set_max_torque(TILT,MAX_TILT_TORQUE);
this->tilt_id=tilt_id;
name="arm_control_roll";
this->motors[ROLL] = new CDynamixelMotor(name,this->dyn_server,roll_id);
this->motors[PAN]->set_position_range(-150.0,150.0);
this->set_max_angles(ROLL,90.0,-90.0);
this->set_max_torque(ROLL,50.0);
this->set_max_angles(ROLL,MAX_ROLL_ANGLE,MIN_ROLL_ANGLE);
this->set_max_torque(ROLL,MAX_ROLL_TORQUE);
this->roll_id=roll_id;
this->stored_poses.resize(4);
this->max_speed=50.0;
this->max_speed=MAX_SPEED;
this->max_accel=100.0;
this->target_angles.pan=this->motors[PAN]->get_current_angle();
this->target_speeds.pan=this->max_speed;
this->target_accels.pan=this->max_accel;
this->target_dir.pan=1.0;
this->target_dir.pan=PAN_DIR;
this->target_angles.tilt=this->motors[TILT]->get_current_angle();
this->target_speeds.tilt=this->max_speed;
this->target_accels.tilt=this->max_accel;
this->target_dir.tilt=1.0;
this->target_dir.tilt=TILT_DIR;
this->target_angles.roll=this->motors[ROLL]->get_current_angle();
this->target_speeds.roll=this->max_speed;
this->target_accels.roll=this->max_accel;
this->target_dir.roll=1.0;
this->target_dir.roll=ROLL_DIR;
this->target_run=false;
this->enable_torque(true);
// initialize IMU
......@@ -170,15 +213,15 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de
for(unsigned int i=0;i<this->joy.get_num_buttons();i++)
this->current_button_state[i]=this->joy.get_button_state(i);
// assign callbacks
this->joy.enable_button_callback(4,button_callback,this);
this->joy.enable_button_callback(13,button_callback,this);
this->joy.enable_button_callback(14,button_callback,this);
this->joy.enable_button_callback(6,button_callback,this);
this->joy.enable_button_callback(9,button_callback,this);
this->joy.enable_button_callback(0,button_callback,this);
this->joy.enable_button_callback(1,button_callback,this);
this->joy.enable_button_callback(2,button_callback,this);
this->joy.enable_button_callback(3,button_callback,this);
this->joy.enable_button_callback(DEAD_MAN_SW,button_callback,this);
this->joy.enable_button_callback(INC_SPEED,button_callback,this);
this->joy.enable_button_callback(DEC_SPEED,button_callback,this);
this->joy.enable_button_callback(TORQUE_EN,button_callback,this);
this->joy.enable_button_callback(IMU_TOGGLE,button_callback,this);
this->joy.enable_button_callback(POS_TRIANGLE,button_callback,this);
this->joy.enable_button_callback(POS_CIRCLE,button_callback,this);
this->joy.enable_button_callback(POS_CROSS,button_callback,this);
this->joy.enable_button_callback(POS_SQUARE,button_callback,this);
this->joy.enable_position_change_callback(0,axis_callback,this);
this->joy.enable_position_change_callback(1,axis_callback,this);
this->joy.enable_position_change_callback(3,axis_callback,this);
......@@ -243,16 +286,16 @@ void *CArmControl::position_monitor_thread(void *param)
else
{
try{
if(arm->get_button_state(4))
if(arm->get_button_state(DEAD_MAN_SW))
{
arm->access.enter();
current_pos=arm->motors[PAN]->get_current_angle();
if(current_pos>arm->max_angles.pan && arm->target_torques.pan<0.0)
if(current_pos<arm->min_angles.pan && arm->target_torques.pan<0.0)
arm->motors[PAN]->move_absolute_angle(current_pos,50.0);
else if(current_pos<arm->min_angles.pan && arm->target_torques.pan>0.0)
else if(current_pos>arm->max_angles.pan && arm->target_torques.pan>0.0)
arm->motors[PAN]->move_absolute_angle(current_pos,50.0);
else
arm->motors[PAN]->move_torque(-arm->target_torques.pan);
arm->motors[PAN]->move_torque(arm->target_torques.pan);
current_pos=arm->motors[TILT]->get_current_angle();
if(current_pos>arm->max_angles.tilt && arm->target_torques.tilt<0.0)
arm->motors[TILT]->move_absolute_angle(current_pos,50.0);
......@@ -283,31 +326,34 @@ void *CArmControl::position_monitor_thread(void *param)
current_sample++;
pan=0;
for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
pan+=arm->imu_zero.pan-(euler[i])[0];
pan+=(euler[i])[0]-arm->imu_zero.pan;
pan/=IMU_NUM_SAMPLES;
if(pan>arm->max_angles.pan)
pan=arm->max_angles.pan;
else if(pan<arm->min_angles.pan)
pan=arm->min_angles.pan;
arm->motors[PAN]->move_absolute_angle(pan,arm->max_speed);
current_pos=arm->motors[PAN]->get_current_angle();
arm->motors[PAN]->move_torque(pan-current_pos);
tilt=0;
for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
tilt+=-euler[i][2];
tilt+=-euler[i][1];
tilt/=IMU_NUM_SAMPLES;
if(tilt>arm->max_angles.tilt)
tilt=arm->max_angles.tilt;
else if(tilt<arm->min_angles.tilt)
tilt=arm->min_angles.tilt;
arm->motors[TILT]->move_absolute_angle(tilt,arm->max_speed);
current_pos=arm->motors[TILT]->get_current_angle();
arm->motors[TILT]->move_torque(tilt-current_pos);
roll=0;
for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
roll+=euler[i][1];
roll+=euler[i][2];
roll/=IMU_NUM_SAMPLES;
if(roll>arm->max_angles.roll)
roll=arm->max_angles.roll;
else if(roll<arm->min_angles.roll)
roll=arm->min_angles.roll;
arm->motors[ROLL]->move_absolute_angle(roll,arm->max_speed);
current_pos=arm->motors[ROLL]->get_current_angle();
arm->motors[ROLL]->move_torque(roll-current_pos);
arm->access.exit();
}
}
......@@ -317,7 +363,7 @@ void *CArmControl::position_monitor_thread(void *param)
std::cout << e.what() << std::endl;
arm->access.exit();
}
if(arm->get_button_state(8))
if(arm->get_button_state(EXIT))
arm->exit_flag=true;
}
usleep(10000);
......@@ -679,7 +725,7 @@ void CArmControl::stop(void)
void CArmControl::increase_max_speed(void)
{
this->max_speed+=10.0;
if(this->max_speed>300.0)
if(this->max_speed>MAX_SPEED)
this->max_speed=300.0;
}
......@@ -939,6 +985,7 @@ void CArmControl::toggle_imu(void)
{
this->imu_enabled=false;
digitalWrite(IMU_MODE,LOW);
this->stop();
}
else
{
......
......@@ -12,9 +12,12 @@ int main(int argc, char *argv[])
CArmControl arm(joy_device,imu_device,dyn_device,1000000,6,5,4);
arm.load_poses("/home/pi/code/arm_control/src/xml/poses.xml");
arm.set_max_angles(PAN,90.0,-90.0);
arm.set_max_angles(PAN,130.0,-130.0);
arm.set_max_torque(PAN,30);
arm.set_max_angles(TILT,90.0,-90.0);
arm.set_max_torque(TILT,30);
arm.set_max_angles(ROLL,90.0,-90.0);
arm.set_max_torque(ROLL,30);
while(!arm.exit());
......
......@@ -2,26 +2,26 @@
<arm_motions xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="arm_motions_cfg.xsd">
<arm_motion>
<button_name>triangle</button_name>
<pan>90.176</pan>
<tilt>2.78592</tilt>
<roll>-3.37243</roll>
<pan>95.7478</pan>
<tilt>-1.31965</tilt>
<roll>-10.9971</roll>
</arm_motion>
<arm_motion>
<button_name>circle</button_name>
<pan>-98.9736</pan>
<tilt>-8.06452</tilt>
<roll>-2.49267</roll>
<pan>28.8856</pan>
<tilt>-0.733138</tilt>
<roll>-8.65103</roll>
</arm_motion>
<arm_motion>
<button_name>cross</button_name>
<pan>-3.37243</pan>
<tilt>-4.2522</tilt>
<roll>-92.8152</roll>
<pan>-109.824</pan>
<tilt>3.66569</tilt>
<roll>-40.0293</roll>
</arm_motion>
<arm_motion>
<button_name>square</button_name>
<pan>-4.54545</pan>
<tilt>41.4956</tilt>
<roll>-90.7625</roll>
<pan>-99.2669</pan>
<tilt>-4.2522</tilt>
<roll>-4.54545</roll>
</arm_motion>
</arm_motions>
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment