Skip to content
GitLab
Commit 6e5e3876 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Implemented the IMU relative control. 

Added a set of functions to set/get the IMU control mode and its parameters.
parent 6d48198e
Hide whitespace changes
Inline Side-by-side
include/arm_control.h
View file @ 6e5e3876
......@@ -16,6 +16,8 @@ typedef struct
double roll;
}TJoints;
typedef enum {IMU_ABSOLUTE=1,IMU_RELATIVE=2} imu_control_mode_t;
class CArmControl
{
private:
......@@ -37,6 +39,7 @@ class CArmControl
TJoints max_angles;
TJoints min_angles;
TJoints max_torques;
TJoints servo_dir;
TJoints target_torques;
TJoints target_angles;
TJoints target_stop_angles;
......@@ -50,6 +53,10 @@ class CArmControl
bool imu_enabled;
TJoints imu_zero;
TJoints imu_angles;
TJoints imu_last_angles;
imu_control_mode_t imu_control_mode;
double imu_threshold;
double imu_gain;
bool torque_enabled;
protected:
static void *position_monitor_thread(void *param);
......@@ -64,6 +71,7 @@ class CArmControl
// servo functions
void set_max_angles(unsigned int axis_id,double max,double min);
void set_max_torque(unsigned int axis_id,double max);
void invert_direction(unsigned int axis_id,bool invert);
void enable_torque(bool enable);
bool is_torque_enabled(void);
void move(unsigned int axis_id,double torque);
......@@ -72,6 +80,7 @@ class CArmControl
// stored poses
void increase_max_speed(void);
void decrease_max_speed(void);
void set_max_speed(double speed);
void load_poses(const std::string &filename);
void save_poses(const std::string &filename);
void update_pose(unsigned int pose_id);
......@@ -81,6 +90,12 @@ class CArmControl
void disable_imu(void);
bool is_imu_enabled(void);
void update_imu(double pan,double tilt, double roll);
void set_imu_control_mode(imu_control_mode_t mode);
// IMU relative control functions
void set_imu_rel_control_threshold(double angle);
double get_imu_rel_control_threshold(void);
void set_imu_rel_control_gain(double gain);
double get_imu_rel_control_gan(void);
~CArmControl();
};
......
src/arm_control.cpp
View file @ 6e5e3876
......@@ -62,6 +62,7 @@ CArmControl::CArmControl(std::string &dyn_serial, unsigned int baudrate,unsigned
this->current_angles.pan=this->motors[PAN]->get_current_angle();
this->set_max_angles(PAN,MAX_PAN_ANGLE,MIN_PAN_ANGLE);
this->set_max_torque(PAN,MAX_PAN_TORQUE);
this->servo_dir.pan=1.0;
this->pan_id=pan_id;
name="arm_control_tilt";
this->motors[TILT] = new CDynamixelMotor(name,this->dyn_server,tilt_id);
......@@ -69,6 +70,7 @@ CArmControl::CArmControl(std::string &dyn_serial, unsigned int baudrate,unsigned
this->current_angles.tilt=this->motors[TILT]->get_current_angle();
this->set_max_angles(TILT,MAX_TILT_ANGLE,MIN_TILT_ANGLE);
this->set_max_torque(TILT,MAX_TILT_TORQUE);
this->servo_dir.tilt=1.0;
this->tilt_id=tilt_id;
name="arm_control_roll";
this->motors[ROLL] = new CDynamixelMotor(name,this->dyn_server,roll_id);
......@@ -76,6 +78,7 @@ CArmControl::CArmControl(std::string &dyn_serial, unsigned int baudrate,unsigned
this->current_angles.roll=this->motors[ROLL]->get_current_angle();
this->set_max_angles(ROLL,MAX_ROLL_ANGLE,MIN_ROLL_ANGLE);
this->set_max_torque(ROLL,MAX_ROLL_TORQUE);
this->servo_dir.roll=1.0;
this->roll_id=roll_id;
this->stored_poses.resize(4);
this->max_speed=MAX_SPEED;
......@@ -97,6 +100,9 @@ CArmControl::CArmControl(std::string &dyn_serial, unsigned int baudrate,unsigned
this->target_dir.roll=ROLL_DIR;
this->target_run=false;
this->imu_enabled=false;
this->imu_control_mode=IMU_ABSOLUTE;
this->imu_threshold=1.0;
this->imu_gain=1.0;
this->enable_torque(true);
// create threads and events
this->thread_server=CThreadServer::instance();
......@@ -148,7 +154,7 @@ void *CArmControl::position_monitor_thread(void *param)
arm->current_angles.tilt=current_pos;
current_pos=arm->motors[ROLL]->get_current_angle();
if(current_pos>arm->max_angles.roll && arm->target_torques.roll<0.0)
arm->motors[ROLL]->move_absolute_angle(current_pos,50.0);
arm->motors[ROLL]->move_absolute_angle(current_pos,50.0);
else if(current_pos<arm->min_angles.roll && arm->target_torques.roll>0.0)
arm->motors[ROLL]->move_absolute_angle(current_pos,50.0);
else
......@@ -163,35 +169,89 @@ void *CArmControl::position_monitor_thread(void *param)
if(count==10)
{
count=0;
if(arm->imu_angles.pan>arm->max_angles.pan)
pan=arm->max_angles.pan;
else if(arm->imu_angles.pan<arm->min_angles.pan)
pan=arm->min_angles.pan;
else
pan=arm->imu_angles.pan;
current_pos=arm->motors[PAN]->get_current_angle();
arm->motors[PAN]->move_pwm(pan-current_pos);
arm->current_angles.pan=current_pos;
tilt=0;
if(arm->imu_angles.tilt>arm->max_angles.tilt)
tilt=arm->max_angles.tilt;
else if(arm->imu_angles.tilt<arm->min_angles.tilt)
tilt=arm->min_angles.tilt;
else
tilt=arm->imu_angles.tilt;
current_pos=arm->motors[TILT]->get_current_angle();
arm->motors[TILT]->move_pwm(tilt-current_pos);
arm->current_angles.tilt=current_pos;
roll=0;
if(arm->imu_angles.roll>arm->max_angles.roll)
roll=arm->max_angles.roll;
else if(arm->imu_angles.roll<arm->min_angles.roll)
roll=arm->min_angles.roll;
else
roll=arm->imu_angles.roll;
current_pos=arm->motors[ROLL]->get_current_angle();
arm->motors[ROLL]->move_pwm(roll-current_pos);
arm->current_angles.roll=current_pos;
if(arm->imu_control_mode==IMU_ABSOLUTE)
{
if(arm->imu_angles.pan>arm->max_angles.pan)
pan=arm->max_angles.pan;
else if(arm->imu_angles.pan<arm->min_angles.pan)
pan=arm->min_angles.pan;
else
pan=arm->imu_angles.pan;
current_pos=arm->motors[PAN]->get_current_angle();
arm->motors[PAN]->move_pwm(pan-current_pos);
arm->current_angles.pan=current_pos;
tilt=0;
if(arm->imu_angles.tilt>arm->max_angles.tilt)
tilt=arm->max_angles.tilt;
else if(arm->imu_angles.tilt<arm->min_angles.tilt)
tilt=arm->min_angles.tilt;
else
tilt=arm->imu_angles.tilt;
current_pos=arm->motors[TILT]->get_current_angle();
arm->motors[TILT]->move_pwm(tilt-current_pos);
arm->current_angles.tilt=current_pos;
roll=0;
if(arm->imu_angles.roll>arm->max_angles.roll)
roll=arm->max_angles.roll;
else if(arm->imu_angles.roll<arm->min_angles.roll)
roll=arm->min_angles.roll;
else
roll=arm->imu_angles.roll;
current_pos=arm->motors[ROLL]->get_current_angle();
arm->motors[ROLL]->move_pwm(roll-current_pos);
arm->current_angles.roll=current_pos;
}
else if(arm->imu_control_mode==IMU_RELATIVE)
{
current_pos=arm->motors[PAN]->get_current_angle();
pan=(arm->imu_angles.pan-arm->imu_zero.pan);
if(fabs(pan)>arm->imu_threshold)
{
pan*=arm->imu_gain;
if(pan>100.0)
pan=100.0;
else if(pan<-100.0)
pan=-100.0;
arm->motors[PAN]->move_pwm(pan);
arm->imu_last_angles.pan=current_pos;
}
else
{
std::cout << "keep pos" << std::endl;
arm->motors[PAN]->move_absolute_angle(arm->imu_last_angles.pan,50.0);
}
arm->current_angles.pan=current_pos;
current_pos=arm->motors[TILT]->get_current_angle();
tilt=(arm->imu_angles.tilt-arm->imu_zero.tilt);
if(fabs(tilt)>arm->imu_threshold)
{
tilt*=arm->imu_gain;
if(tilt>100.0)
tilt=100.0;
else if(pan<-100.0)
tilt=-100.0;
arm->motors[TILT]->move_pwm(tilt);
arm->imu_last_angles.tilt=current_pos;
}
else
arm->motors[TILT]->move_absolute_angle(arm->imu_last_angles.tilt,50.0);
arm->current_angles.tilt=current_pos;
current_pos=arm->motors[ROLL]->get_current_angle();
roll=(arm->imu_angles.roll-arm->imu_zero.roll);
if(fabs(roll)>arm->imu_threshold)
{
roll*=arm->imu_gain;
if(roll>100.0)
roll=100.0;
else if(roll<-100.0)
roll=-100.0;
arm->motors[ROLL]->move_pwm(roll);
arm->imu_last_angles.roll=current_pos;
}
else
arm->motors[ROLL]->move_absolute_angle(arm->imu_last_angles.roll=current_pos,50.0);
arm->current_angles.roll=current_pos;
}
}
}
else
......@@ -420,18 +480,57 @@ void CArmControl::set_max_angles(unsigned int axis_id, double max, double min)
{
if(axis_id==PAN)
{
this->access.enter();
this->max_angles.pan=max;
this->min_angles.pan=min;
this->access.exit();
}
else if(axis_id==TILT)
{
this->access.enter();
this->max_angles.tilt=max;
this->min_angles.tilt=min;
this->access.exit();
}
else if(axis_id==ROLL)
{
this->access.enter();
this->max_angles.roll=max;
this->min_angles.roll=min;
this->access.exit();
}
else
throw CException(_HERE_,"Invalid axis ID");
}
void CArmControl::invert_direction(unsigned int axis_id,bool invert)
{
if(axis_id==PAN)
{
this->access.enter();
if(invert)
this->servo_dir.pan=-1.0;
else
this->servo_dir.pan=1.0;
this->access.exit();
}
else if(axis_id==TILT)
{
this->access.enter();
if(invert)
this->servo_dir.tilt=-1.0;
else
this->servo_dir.tilt=1.0;
this->access.exit();
}
else if(axis_id==ROLL)
{
this->access.enter();
if(invert)
this->servo_dir.roll=-1.0;
else
this->servo_dir.roll=1.0;
this->access.exit();
}
else
throw CException(_HERE_,"Invalid axis ID");
......@@ -550,11 +649,11 @@ bool CArmControl::is_torque_enabled(void)
void CArmControl::move(unsigned int axis_id,double torque)
{
if(axis_id==PAN)
this->target_torques.pan=(torque*this->max_torques.pan);
this->target_torques.pan=this->servo_dir.pan*(torque*this->max_torques.pan);
else if(axis_id==TILT)
this->target_torques.tilt=(torque*this->max_torques.tilt);
this->target_torques.tilt=this->servo_dir.tilt*(torque*this->max_torques.tilt);
else if(axis_id==ROLL)
this->target_torques.roll=(torque*this->max_torques.roll);
this->target_torques.roll=this->servo_dir.roll*(torque*this->max_torques.roll);
else
throw CException(_HERE_,"Invalid axis ID");
}
......@@ -581,16 +680,31 @@ void CArmControl::stop(void)
void CArmControl::increase_max_speed(void)
{
this->access.enter();
this->max_speed+=10.0;
if(this->max_speed>MAX_SPEED)
this->max_speed=300.0;
this->access.exit();
}
void CArmControl::decrease_max_speed(void)
{
this->access.enter();
this->max_speed-=10.0;
if(this->max_speed<10.0)
this->max_speed=10.0;
this->access.exit();
}
void CArmControl::set_max_speed(double speed)
{
this->access.enter();
this->max_speed=speed;
if(this->max_speed>MAX_SPEED)
this->max_speed=300.0;
else if(this->max_speed<10.0)
this->max_speed=10.0;
this->access.exit();
}
void CArmControl::load_poses(const std::string &filename)
......@@ -843,8 +957,11 @@ void CArmControl::enable_imu(double pan,double tilt, double roll)
digitalWrite(IMU_MODE,HIGH);
#endif
this->imu_zero.pan=pan;
this->imu_last_angles.pan=this->current_angles.pan;
this->imu_zero.tilt=tilt;
this->imu_last_angles.tilt=this->current_angles.tilt;
this->imu_zero.roll=roll;
this->imu_last_angles.roll=this->current_angles.roll;
this->access.exit();
}
......@@ -873,6 +990,44 @@ void CArmControl::update_imu(double pan,double tilt, double roll)
this->access.exit();
}
void CArmControl::set_imu_control_mode(imu_control_mode_t mode)
{
this->imu_control_mode=mode;
}
// IMU relative control functions
void CArmControl::set_imu_rel_control_threshold(double angle)
{
this->access.enter();
if(angle>0.0)
this->imu_threshold=angle;
else
this->imu_threshold=-angle;
this->access.exit();
}
double CArmControl::get_imu_rel_control_threshold(void)
{
return this->imu_threshold;
}
void CArmControl::set_imu_rel_control_gain(double gain)
{
this->access.enter();
if(gain<0.01)
this->imu_gain=0.01;
else if(gain>10.0)
this->imu_gain=10.0;
else
this->imu_gain=gain;
this->access.exit();
}
double CArmControl::get_imu_rel_control_gan(void)
{
return this->imu_gain;
}
CArmControl::~CArmControl()
{
// destroy thread and events
......
Supports Markdown
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