Commit 3b9437e8 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Enabled the Raspberry PI GPIO.

Tuned the pre-defined motions parameters.
Added functions to enable/disable the servos power.
parent b9e41922
......@@ -63,6 +63,8 @@ class CArmControl
imu_control_mode_t imu_control_mode;
double imu_threshold;
double imu_gain;
// servo power
bool power_enabled;
protected:
static void *position_monitor_thread(void *param);
void move_joints(void);
......@@ -83,6 +85,9 @@ class CArmControl
void move(unsigned int axis_id,double torque);
void stop(unsigned int axis_id=ALL);
void get_current_angles(double &pan, double &tilt,double &roll);
void enable_servo_power(void);
void disable_servo_power(void);
bool is_servo_power_enabled(void);
// stored poses
void increase_max_speed(void);
void decrease_max_speed(void);
......
......@@ -26,10 +26,12 @@ TARGET_LINK_LIBRARIES(arm_control ${comm_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${dynamixel_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${dynamixel_motor_cont_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${XSD_LIBRARY})
#TARGET_LINK_LIBRARIES(arm_control libwiringPi.so)
TARGET_LINK_LIBRARIES(arm_control libwiringPi.so)
ADD_DEPENDENCIES(arm_control xsd_files_gen)
ADD_DEFINITIONS(-DPI)
# link necessary libraries
INSTALL(TARGETS arm_control
RUNTIME DESTINATION bin
......
......@@ -17,8 +17,9 @@
#include "xml/arm_motions_cfg_file.hxx"
#define OK_LED 25
#define IMU_MODE 23
#define IMU_CAL 21
#define POWERED 23
#define MOTION_PERIOD 0.01
#define MAX_PAN_ANGLE 80.0
#define MIN_PAN_ANGLE -80.0
......@@ -43,13 +44,12 @@ CArmControl::CArmControl(std::string &dyn_serial, unsigned int baudrate,unsigned
#ifdef PI
wiringPiSetup();
pinMode(IMU_CAL,OUTPUT);
digitalWrite(IMU_CAL,LOW);
pinMode(IMU_MODE,OUTPUT);
digitalWrite(IMU_MODE,LOW);
pinMode(POWERED,OUTPUT);
digitalWrite(POWERED,LOW);
pinMode(OK_LED,OUTPUT);
digitalWrite(OK_LED,LOW);
#endif
this->power_enabled=true;
this->exit_flag=false;
// detect and initialize servos
this->dyn_server=CDynamixelServerSerial::instance();
......@@ -87,7 +87,7 @@ CArmControl::CArmControl(std::string &dyn_serial, unsigned int baudrate,unsigned
this->enabled.roll=false;
this->stored_poses.resize(4);
this->max_speed=MAX_SPEED;
this->max_accel=100.0;
this->max_accel=30.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;
......@@ -267,30 +267,33 @@ void *CArmControl::position_monitor_thread(void *param)
void CArmControl::move_joints(void)
{
static double period=0;
bool moving=false;
std::string name;
if(this->target_run)
period-=0.01;
if(this->target_run && period<=0.0)
{
period=MOTION_PERIOD;
if(abs(this->target_angles.pan-this->current_angles.pan)>=1.0)
{
moving=true;
if(this->current_speeds.pan!=(this->target_dir.pan*this->target_speeds.pan))// it is necessary to change the current speed
{
this->current_angles.pan+=this->current_speeds.pan*0.01/2.0;
this->current_angles.pan+=this->current_speeds.pan*MOTION_PERIOD/2.0;
if(this->current_speeds.pan>this->target_dir.pan*this->target_speeds.pan)
{
this->current_speeds.pan-=this->target_accels.pan*0.01;// reduce speed
this->current_speeds.pan-=this->target_accels.pan*MOTION_PERIOD;// reduce speed
if(this->current_speeds.pan<this->target_dir.pan*this->target_speeds.pan)
this->current_speeds.pan=this->target_dir.pan*this->target_speeds.pan;
}
else
{
this->current_speeds.pan+=this->target_accels.pan*0.01;// increase speed
this->current_speeds.pan+=this->target_accels.pan*MOTION_PERIOD;// increase speed
if(this->current_speeds.pan>this->target_dir.pan*this->target_speeds.pan)
this->current_speeds.pan=this->target_dir.pan*this->target_speeds.pan;
}
this->current_angles.pan+=this->current_speeds.pan*0.01/2.0;
this->current_angles.pan+=this->current_speeds.pan*MOTION_PERIOD/2.0;
if(this->target_dir.pan==1.0 && this->current_angles.pan>this->target_angles.pan)
this->current_angles.pan=this->target_angles.pan;
if(this->target_dir.pan==-1.0 && this->current_angles.pan<this->target_angles.pan)
......@@ -300,7 +303,7 @@ void CArmControl::move_joints(void)
{
if(abs(this->target_angles.pan-this->current_angles.pan)>this->target_stop_angles.pan)// constant speed phase
{
this->current_angles.pan+=this->current_speeds.pan*0.01;
this->current_angles.pan+=this->current_speeds.pan*MOTION_PERIOD;
if(this->target_dir.pan==1.0 && this->current_angles.pan>this->target_angles.pan)
this->current_angles.pan=this->target_angles.pan;
if(this->target_dir.pan==-1.0 && this->current_angles.pan<this->target_angles.pan)
......@@ -308,12 +311,12 @@ void CArmControl::move_joints(void)
}
else// deceleration phase
{
this->current_angles.pan=this->current_angles.pan+this->current_speeds.pan*0.01/2.0;
this->target_speeds.pan=this->target_speeds.pan-this->target_accels.pan*0.01;
this->current_angles.pan=this->current_angles.pan+this->current_speeds.pan*MOTION_PERIOD/2.0;
this->target_speeds.pan=this->target_speeds.pan-this->target_accels.pan*MOTION_PERIOD;
if(this->target_speeds.pan<0)
this->target_speeds.pan=0;
this->current_speeds.pan=this->target_dir.pan*this->target_speeds.pan;
this->current_angles.pan=this->current_angles.pan+this->current_speeds.pan*0.01/2.0;
this->current_angles.pan=this->current_angles.pan+this->current_speeds.pan*MOTION_PERIOD/2.0;
if(this->target_dir.pan==1.0 && this->current_angles.pan>this->target_angles.pan)
this->current_angles.pan=this->target_angles.pan;
if(this->target_dir.pan==-1.0 && this->current_angles.pan<this->target_angles.pan)
......@@ -328,20 +331,20 @@ void CArmControl::move_joints(void)
moving=true;
if(this->current_speeds.tilt!=(this->target_dir.tilt*this->target_speeds.tilt))// it is necessary to change the current speed
{
this->current_angles.tilt+=this->current_speeds.tilt*0.01/2.0;
this->current_angles.tilt+=this->current_speeds.tilt*MOTION_PERIOD/2.0;
if(this->current_speeds.tilt>this->target_dir.tilt*this->target_speeds.tilt)
{
this->current_speeds.tilt-=this->target_accels.tilt*0.01;// reduce speed
this->current_speeds.tilt-=this->target_accels.tilt*MOTION_PERIOD;// reduce speed
if(this->current_speeds.tilt<this->target_dir.tilt*this->target_speeds.tilt)
this->current_speeds.tilt=this->target_dir.tilt*this->target_speeds.tilt;
}
else
{
this->current_speeds.tilt+=this->target_accels.tilt*0.01;// increase speed
this->current_speeds.tilt+=this->target_accels.tilt*MOTION_PERIOD;// increase speed
if(this->current_speeds.tilt>this->target_dir.tilt*this->target_speeds.tilt)
this->current_speeds.tilt=this->target_dir.tilt*this->target_speeds.tilt;
}
this->current_angles.tilt+=this->current_speeds.tilt*0.01/2.0;
this->current_angles.tilt+=this->current_speeds.tilt*MOTION_PERIOD/2.0;
if(this->target_dir.tilt==1.0 && this->current_angles.tilt>this->target_angles.tilt)
this->current_angles.tilt=this->target_angles.tilt;
if(this->target_dir.tilt==-1.0 && this->current_angles.tilt<this->target_angles.tilt)
......@@ -351,7 +354,7 @@ void CArmControl::move_joints(void)
{
if(abs(this->target_angles.tilt-this->current_angles.tilt)>this->target_stop_angles.tilt)// constant speed phase
{
this->current_angles.tilt+=this->current_speeds.tilt*0.01;
this->current_angles.tilt+=this->current_speeds.tilt*MOTION_PERIOD;
if(this->target_dir.tilt==1.0 && this->current_angles.tilt>this->target_angles.tilt)
this->current_angles.tilt=this->target_angles.tilt;
if(this->target_dir.tilt==-1.0 && this->current_angles.tilt<this->target_angles.tilt)
......@@ -359,12 +362,12 @@ void CArmControl::move_joints(void)
}
else// deceleration phase
{
this->current_angles.tilt=this->current_angles.tilt+this->current_speeds.tilt*0.01/2.0;
this->target_speeds.tilt=this->target_speeds.tilt-this->target_accels.tilt*0.01;
this->current_angles.tilt=this->current_angles.tilt+this->current_speeds.tilt*MOTION_PERIOD/2.0;
this->target_speeds.tilt=this->target_speeds.tilt-this->target_accels.tilt*MOTION_PERIOD;
if(this->target_speeds.tilt<0)
this->target_speeds.tilt=0;
this->current_speeds.tilt=this->target_dir.tilt*this->target_speeds.tilt;
this->current_angles.tilt=this->current_angles.tilt+this->current_speeds.tilt*0.01/2.0;
this->current_angles.tilt=this->current_angles.tilt+this->current_speeds.tilt*MOTION_PERIOD/2.0;
if(this->target_dir.tilt==1.0 && this->current_angles.tilt>this->target_angles.tilt)
this->current_angles.tilt=this->target_angles.tilt;
if(this->target_dir.tilt==-1.0 && this->current_angles.tilt<this->target_angles.tilt)
......@@ -379,20 +382,20 @@ void CArmControl::move_joints(void)
moving=true;
if(this->current_speeds.roll!=(this->target_dir.roll*this->target_speeds.roll))// it is necessary to change the current speed
{
this->current_angles.roll+=this->current_speeds.roll*0.01/2.0;
this->current_angles.roll+=this->current_speeds.roll*MOTION_PERIOD/2.0;
if(this->current_speeds.roll>this->target_dir.roll*this->target_speeds.roll)
{
this->current_speeds.roll-=this->target_accels.roll*0.01;// reduce speed
this->current_speeds.roll-=this->target_accels.roll*MOTION_PERIOD;// reduce speed
if(this->current_speeds.roll<this->target_dir.roll*this->target_speeds.roll)
this->current_speeds.roll=this->target_dir.roll*this->target_speeds.roll;
}
else
{
this->current_speeds.roll+=this->target_accels.roll*0.01;// increase speed
this->current_speeds.roll+=this->target_accels.roll*MOTION_PERIOD;// increase speed
if(this->current_speeds.roll>this->target_dir.roll*this->target_speeds.roll)
this->current_speeds.roll=this->target_dir.roll*this->target_speeds.roll;
}
this->current_angles.roll+=this->current_speeds.roll*0.01/2.0;
this->current_angles.roll+=this->current_speeds.roll*MOTION_PERIOD/2.0;
if(this->target_dir.roll==1.0 && this->current_angles.roll>this->target_angles.roll)
this->current_angles.roll=this->target_angles.roll;
if(this->target_dir.roll==-1.0 && this->current_angles.roll<this->target_angles.roll)
......@@ -402,7 +405,7 @@ void CArmControl::move_joints(void)
{
if(abs(this->target_angles.roll-this->current_angles.roll)>this->target_stop_angles.roll)// constant speed phase
{
this->current_angles.roll+=this->current_speeds.roll*0.01;
this->current_angles.roll+=this->current_speeds.roll*MOTION_PERIOD;
if(this->target_dir.roll==1.0 && this->current_angles.roll>this->target_angles.roll)
this->current_angles.roll=this->target_angles.roll;
if(this->target_dir.roll==-1.0 && this->current_angles.roll<this->target_angles.roll)
......@@ -410,12 +413,12 @@ void CArmControl::move_joints(void)
}
else// deceleration phase
{
this->current_angles.roll=this->current_angles.roll+this->current_speeds.roll*0.01/2.0;
this->target_speeds.roll=this->target_speeds.roll-this->target_accels.roll*0.01;
this->current_angles.roll=this->current_angles.roll+this->current_speeds.roll*MOTION_PERIOD/2.0;
this->target_speeds.roll=this->target_speeds.roll-this->target_accels.roll*MOTION_PERIOD;
if(this->target_speeds.roll<0)
this->target_speeds.roll=0;
this->current_speeds.roll=this->target_dir.roll*this->target_speeds.roll;
this->current_angles.roll=this->current_angles.roll+this->current_speeds.roll*0.01/2.0;
this->current_angles.roll=this->current_angles.roll+this->current_speeds.roll*MOTION_PERIOD/2.0;
if(this->target_dir.roll==1.0 && this->current_angles.roll>this->target_angles.roll)
this->current_angles.roll=this->target_angles.roll;
if(this->target_dir.roll==-1.0 && this->current_angles.roll<this->target_angles.roll)
......@@ -786,6 +789,27 @@ void CArmControl::set_max_speed(double speed)
this->access.exit();
}
void CArmControl::enable_servo_power(void)
{
#ifdef PI
digitalWrite(POWERED,LOW);
#endif
this->power_enabled=true;
}
void CArmControl::disable_servo_power(void)
{
#ifdef PI
digitalWrite(POWERED,HIGH);
#endif
this->power_enabled=false;
}
bool CArmControl::is_servo_power_enabled(void)
{
return this->power_enabled;
}
void CArmControl::load_poses(const std::string &filename)
{
arm_motions_t::arm_motion_iterator iterator;
......@@ -1046,9 +1070,6 @@ void CArmControl::enable_imu(double pan,double tilt, double roll)
{
this->access.enter();
this->imu_enabled=true;
#ifdef PI
digitalWrite(IMU_MODE,HIGH);
#endif
this->imu_zero.pan=pan;
this->imu_last_angles.pan=this->current_angles.pan;
this->imu_zero.tilt=tilt;
......@@ -1062,9 +1083,6 @@ void CArmControl::disable_imu(void)
{
this->access.enter();
this->imu_enabled=false;
#ifdef PI
digitalWrite(IMU_MODE,LOW);
#endif
this->access.exit();
this->stop();
}
......@@ -1155,8 +1173,7 @@ CArmControl::~CArmControl()
this->motors[ROLL]=NULL;
}
#ifdef PI
digitalWrite(IMU_CAL,LOW);
digitalWrite(IMU_MODE,LOW);
digitalWrite(POWERED,LOW);
digitalWrite(OK_LED,LOW);
#endif
}
......
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