Skip to content

GitLab

Commit 648eb435 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added support for IMU control. 

Added a mutex.
Solved some problems and bugs.
parent 6f464e5d
Hide whitespace changes
Inline Side-by-side
include/arm_control.h
View file @ 648eb435
......@@ -5,7 +5,9 @@
#include "dynamixel_motor.h"
#include "threadserver.h"
#include "eventserver.h"
#include "mutex.h"
#include "joystick.h"
#include "bno055_imu_driver.h"
typedef enum {PAN=0,TILT=1,ROLL=2} motor_id_t;
......@@ -19,6 +21,7 @@ typedef struct
class CArmControl
{
private:
CMutex access;
bool exit_flag;
CThreadServer *thread_server;
std::string position_monitor_thread_id;
......@@ -35,10 +38,13 @@ class CArmControl
TJoints target_torques;
std::vector<TJoints> stored_poses;
std::string poses_file;
CBNO055IMUDriver imu;
bool imu_enabled;
TJoints imu_zero;
protected:
static void *position_monitor_thread(void *param);
public:
CArmControl(const std::string &joy_device,std::string &dyn_serial, unsigned int baudrate,unsigned char pan_id, unsigned char tilt_id, unsigned char roll_id);
CArmControl(const std::string &joy_device,const std::string &imu_device,std::string &dyn_serial, unsigned int baudrate,unsigned char pan_id, unsigned char tilt_id, unsigned char roll_id);
bool get_button_state(unsigned int button_id);
bool exit(void);
// servo functionis
......@@ -54,6 +60,7 @@ class CArmControl
void save_poses(const std::string &filename);
void update_pose(unsigned int pose_id);
void execute_pose(unsigned int pose_id);
void toggle_imu(void);
~CArmControl();
};
......
src/CMakeLists.txt
View file @ 648eb435
......@@ -10,6 +10,7 @@ FIND_PACKAGE(joystick REQUIRED)
FIND_PACKAGE(comm REQUIRED)
FIND_PACKAGE(dynamixel REQUIRED)
FIND_PACKAGE(dynamixel_motor_cont REQUIRED)
FIND_PACKAGE(bno055_imu_driver REQUIRED)
# add the necessary include directories
INCLUDE_DIRECTORIES(../include)
INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
......@@ -17,6 +18,7 @@ INCLUDE_DIRECTORIES(${joystick_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${dynamixel_motor_cont_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${bno055_imu_driver_INCLUDE_DIR})
SET_SOURCE_FILES_PROPERTIES(${XSD_SOURCES} PROPERTIES GENERATED 1)
# create the shared library
......@@ -26,7 +28,9 @@ TARGET_LINK_LIBRARIES(arm_control ${joystick_LIBRARY})
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 ${bno055_imu_driver_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${XSD_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control libwiringPi.so)
ADD_DEPENDENCIES(arm_control xsd_files_gen)
......
src/arm_control.cpp
View file @ 648eb435
......@@ -10,8 +10,13 @@
#include <errno.h>
#include <math.h>
#include <linux/joystick.h>
#include <wiringPi.h>
#include "xml/arm_motions_cfg_file.hxx"
#define OK_LED 25
#define IMU_MODE 23
#define IMU_CAL 21
void button_callback(void *param,unsigned int button_id,bool level)
{
CArmControl *arm=(CArmControl *)param;
......@@ -19,6 +24,9 @@ void button_callback(void *param,unsigned int button_id,bool level)
try{
switch(button_id)
{
case 3: if(level)
arm->toggle_imu();
break;
case 4: if(level)
arm->increase_max_speed();
break;
......@@ -96,10 +104,17 @@ void axis_callback(void *param,unsigned int axis_id, float value)
}
}
CArmControl::CArmControl(const std::string &joy_device,std::string &dyn_serial, unsigned int baudrate,unsigned char pan_id, unsigned char tilt_id, unsigned char roll_id) : joy("arm_control_jpy")
CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_device,std::string &dyn_serial, unsigned int baudrate,unsigned char pan_id, unsigned char tilt_id, unsigned char roll_id) : joy("arm_control_jpy"), imu("arm_control_imu")
{
std::string name;
wiringPiSetup();
pinMode(IMU_CAL,OUTPUT);
digitalWrite(IMU_CAL,LOW);
pinMode(IMU_MODE,OUTPUT);
digitalWrite(IMU_MODE,LOW);
pinMode(OK_LED,OUTPUT);
digitalWrite(OK_LED,LOW);
this->exit_flag=false;
// detect and initialize servos
this->dyn_server=CDynamixelServerFTDI::instance();
......@@ -126,12 +141,19 @@ CArmControl::CArmControl(const std::string &joy_device,std::string &dyn_serial,
this->stored_poses.resize(4);
this->max_speed=50.0;
this->enable_torque(true);
// initialize IMU
this->imu.open(imu_device);
this->imu_enabled=false;
this->imu_zero.pan=0.0;
this->imu_zero.tilt=0.0;
this->imu_zero.roll=0.0;
// open the joystick device
this->joy.open(joy_device);
this->current_button_state.resize(this->joy.get_num_buttons());
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(3,button_callback,this);
this->joy.enable_button_callback(4,button_callback,this);
this->joy.enable_button_callback(6,button_callback,this);
this->joy.enable_button_callback(8,button_callback,this);
......@@ -152,16 +174,49 @@ CArmControl::CArmControl(const std::string &joy_device,std::string &dyn_serial,
this->finish_thread_event_id="arm_control_finish_thread_event";
this->event_server->create_event(this->finish_thread_event_id);
this->thread_server->start_thread(this->position_monitor_thread_id);
digitalWrite(OK_LED,HIGH);
}
void *CArmControl::position_monitor_thread(void *param)
{
bool end=false,imu_calibrated=false,save_cal=false;
double current_pos,pan,tilt,roll;
bool accel_cal,mag_cal,gyro_cal;
CArmControl *arm=(CArmControl *)param;
double current_pos;
bool end=false;
std::vector<double> euler;
// initiate IMU calibration
try{
arm->imu.load_calibration("/home/pi/bno055.cal");
arm->imu.set_operation_mode(ndof_mode);
save_cal=false;
}catch(CException &e){
arm->imu.set_operation_mode(ndof_mode);
save_cal=true;
}
while(!end)
{
if(!imu_calibrated)
{
try{
accel_cal=arm->imu.is_accelerometer_calibrated();
mag_cal=arm->imu.is_magnetometer_calibrated();
gyro_cal=arm->imu.is_gyroscope_calibrated();
if(accel_cal && mag_cal && gyro_cal)
{
imu_calibrated=true;
if(save_cal)
{
save_cal=false;
arm->imu.save_calibration("/home/pi/bno055.cal");
}
digitalWrite(IMU_CAL,HIGH);
}
}catch(CException &e){
std::cout << e.what() << std::endl;
}
}
if(arm->event_server->event_is_set(arm->finish_thread_event_id))
end=true;
else
......@@ -169,7 +224,7 @@ void *CArmControl::position_monitor_thread(void *param)
try{
if(arm->get_button_state(10))
{
std::cout << "thread" << std::endl;
arm->access.enter();
current_pos=arm->motors[PAN]->get_current_angle();
if(current_pos>arm->max_angles.pan && arm->target_torques.pan>0.0)
arm->motors[PAN]->move_absolute_angle(current_pos,50.0);
......@@ -191,13 +246,39 @@ void *CArmControl::position_monitor_thread(void *param)
arm->motors[ROLL]->move_absolute_angle(current_pos,50.0);
else
arm->motors[ROLL]->move_torque(arm->target_torques.roll);
arm->access.exit();
}
else if(arm->imu_enabled && imu_calibrated)
{
arm->access.enter();
euler=arm->imu.get_orientation_euler();
pan=arm->imu_zero.pan-euler[0];
tilt=-euler[2];
roll=euler[1];
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);
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);
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);
arm->access.exit();
}
if(arm->get_button_state(0))
arm->exit_flag=true;
}catch(CException &e){
arm->access.exit();
}
}
usleep(30000);
usleep(100000);
}
pthread_exit(NULL);
......@@ -220,21 +301,42 @@ void CArmControl::set_max_angles(unsigned int axis_id, double max, double min)
{
if(axis_id==PAN)
{
this->max_angles.pan=max;
this->min_angles.pan=min;
this->motors[PAN]->set_position_range(min,max);
try{
this->max_angles.pan=max;
this->min_angles.pan=min;
this->access.enter();
this->motors[PAN]->set_position_range(min,max);
this->access.exit();
}catch(CException &e){
this->access.exit();
throw e;
}
}
else if(axis_id==TILT)
{
this->max_angles.tilt=max;
this->min_angles.tilt=min;
this->motors[TILT]->set_position_range(min,max);
try{
this->max_angles.tilt=max;
this->min_angles.tilt=min;
this->access.enter();
this->motors[TILT]->set_position_range(min,max);
this->access.exit();
}catch(CException &e){
this->access.exit();
throw e;
}
}
else if(axis_id==ROLL)
{
this->max_angles.roll=max;
this->min_angles.roll=min;
this->motors[ROLL]->set_position_range(min,max);
try{
this->max_angles.roll=max;
this->min_angles.roll=min;
this->access.enter();
this->motors[ROLL]->set_position_range(min,max);
this->access.exit();
}catch(CException &e){
this->access.exit();
throw e;
}
}
else
throw CException(_HERE_,"Invalid axis ID");
......@@ -249,21 +351,42 @@ void CArmControl::set_max_torque(unsigned int axis_id, double max)
if(axis_id==PAN)
{
this->motors[PAN]->set_max_torque(max);
this->motors[PAN]->set_limit_torque(max);
this->max_torques.pan=max;
try{
this->access.enter();
this->motors[PAN]->set_max_torque(max);
this->motors[PAN]->set_limit_torque(max);
this->access.exit();
this->max_torques.pan=max;
}catch(CException &e){
this->access.exit();
throw e;
}
}
else if(axis_id==TILT)
{
this->motors[TILT]->set_max_torque(max);
this->motors[TILT]->set_limit_torque(max);
this->max_torques.tilt=max;
try{
this->access.enter();
this->motors[TILT]->set_max_torque(max);
this->motors[TILT]->set_limit_torque(max);
this->access.exit();
this->max_torques.tilt=max;
}catch(CException &e){
this->access.exit();
throw e;
}
}
else if(axis_id==ROLL)
{
this->motors[ROLL]->set_max_torque(max);
this->motors[ROLL]->set_limit_torque(max);
this->max_torques.roll=max;
try{
this->access.enter();
this->motors[ROLL]->set_max_torque(max);
this->motors[ROLL]->set_limit_torque(max);
this->access.exit();
this->max_torques.roll=max;
}catch(CException &e){
this->access.exit();
throw e;
}
}
else
throw CException(_HERE_,"Invalid axis ID");
......@@ -275,45 +398,48 @@ void CArmControl::enable_torque(bool enable)
throw CException(_HERE_,"Not enough servos avaialble");
if(this->motors[PAN]!=NULL)
{
if(enable)
{
std::cout << "enable torque pan" << std::endl;
this->motors[PAN]->enable();
}
else
{
std::cout << "disable torque pan" << std::endl;
this->motors[PAN]->disable();
try{
this->access.enter();
if(enable)
this->motors[PAN]->enable();
else
this->motors[PAN]->disable();
this->access.exit();
}catch(CException &e){
this->access.exit();
throw e;
}
}
else
throw CException(_HERE_,"PAN motor not properly initialized");
if(this->motors[TILT]!=NULL)
{
if(enable)
{
std::cout << "enable torque tilt" << std::endl;
this->motors[TILT]->enable();
}
else
{
std::cout << "disable torque tilt" << std::endl;
this->motors[TILT]->disable();
try{
this->access.enter();
if(enable)
this->motors[TILT]->enable();
else
this->motors[TILT]->disable();
this->access.exit();
}catch(CException &e){
this->access.exit();
throw e;
}
}
else
throw CException(_HERE_,"TILT motor not properly initialized");
if(this->motors[ROLL]!=NULL)
{
if(enable)
{
std::cout << "enable torque roll" << std::endl;
this->motors[ROLL]->enable();
}
else
{
std::cout << "disable torque roll" << std::endl;
this->motors[ROLL]->disable();
try{
this->access.enter();
if(enable)
this->motors[ROLL]->enable();
else
this->motors[ROLL]->disable();
this->access.exit();
}catch(CException &e){
this->access.exit();
throw e;
}
}
else
......@@ -323,20 +449,11 @@ void CArmControl::enable_torque(bool enable)
void CArmControl::move(unsigned int axis_id,double torque)
{
if(axis_id==PAN)
{
std::cout << "move pan" << std::endl;
this->target_torques.pan=(torque*this->max_torques.pan)/32767.0;
}
else if(axis_id==TILT)
{
std::cout << "move tilt" << std::endl;
this->target_torques.tilt=(torque*this->max_torques.tilt)/32767.0;
}
else if(axis_id==ROLL)
{
std::cout << "move roll" << std::endl;
this->target_torques.roll=(torque*this->max_torques.roll)/32767.0;
}
else
throw CException(_HERE_,"Invalid axis ID");
}
......@@ -345,12 +462,19 @@ void CArmControl::stop(void)
{
double pos;
pos=this->motors[PAN]->get_current_angle();
this->motors[PAN]->move_absolute_angle(pos,50.0);
pos=this->motors[TILT]->get_current_angle();
this->motors[TILT]->move_absolute_angle(pos,50.0);
pos=this->motors[ROLL]->get_current_angle();
this->motors[ROLL]->move_absolute_angle(pos,50.0);
try{
this->access.enter();
pos=this->motors[PAN]->get_current_angle();
this->motors[PAN]->move_absolute_angle(pos,50.0);
pos=this->motors[TILT]->get_current_angle();
this->motors[TILT]->move_absolute_angle(pos,50.0);
pos=this->motors[ROLL]->get_current_angle();
this->motors[ROLL]->move_absolute_angle(pos,50.0);
this->access.exit();
}catch(CException &e){
this->access.exit();
throw e;
}
}
void CArmControl::increase_max_speed(void)
......@@ -358,7 +482,6 @@ void CArmControl::increase_max_speed(void)
this->max_speed+=10.0;
if(this->max_speed>100.0)
this->max_speed=100.0;
std::cout << "speed: " << this->max_speed << std::endl;
}
void CArmControl::decrease_max_speed(void)
......@@ -366,7 +489,6 @@ void CArmControl::decrease_max_speed(void)
this->max_speed-=10.0;
if(this->max_speed<10.0)
this->max_speed=10.0;
std::cout << "speed: " << this->max_speed << std::endl;
}
void CArmControl::load_poses(const std::string &filename)
......@@ -456,13 +578,19 @@ void CArmControl::update_pose(unsigned int pose_id)
throw CException(_HERE_,"Invalid pose ID");
else
{
std::cout << "update pose" << std::endl;
pos=this->motors[PAN]->get_current_angle();
this->stored_poses[pose_id].pan=pos;
pos=this->motors[TILT]->get_current_angle();
this->stored_poses[pose_id].tilt=pos;
pos=this->motors[ROLL]->get_current_angle();
this->stored_poses[pose_id].roll=pos;
try{
this->access.enter();
pos=this->motors[PAN]->get_current_angle();
this->stored_poses[pose_id].pan=pos;
pos=this->motors[TILT]->get_current_angle();
this->stored_poses[pose_id].tilt=pos;
pos=this->motors[ROLL]->get_current_angle();
this->stored_poses[pose_id].roll=pos;
this->access.exit();
}catch(CException &e){
this->access.exit();
throw e;
}
if(this->poses_file!="")
this->save_poses(this->poses_file);
else
......@@ -479,43 +607,68 @@ void CArmControl::execute_pose(unsigned int pose_id)
throw CException(_HERE_,"Invalid pose ID");
else
{
std::cout << "execute pose" << std::endl;
diff_pan=fabs(this->motors[PAN]->get_current_angle()-this->stored_poses[pose_id].pan);
diff_tilt=fabs(this->motors[TILT]->get_current_angle()-this->stored_poses[pose_id].tilt);
diff_roll=fabs(this->motors[ROLL]->get_current_angle()-this->stored_poses[pose_id].roll);
if(diff_pan>diff_tilt)
{
if(diff_pan>diff_roll)
{
vel_pan=this->max_speed;
vel_tilt=(diff_tilt/diff_pan)*this->max_speed;
vel_roll=(diff_roll/diff_pan)*this->max_speed;
}
else
{
vel_tilt=this->max_speed;
vel_pan=(diff_pan/diff_tilt)*this->max_speed;
vel_roll=(diff_roll/diff_tilt)*this->max_speed;
}
}
else
{
if(diff_tilt>diff_roll)
try{
this->access.enter();
diff_pan=fabs(this->motors[PAN]->get_current_angle()-this->stored_poses[pose_id].pan);
diff_tilt=fabs(this->motors[TILT]->get_current_angle()-this->stored_poses[pose_id].tilt);
diff_roll=fabs(this->motors[ROLL]->get_current_angle()-this->stored_poses[pose_id].roll);
if(diff_pan>diff_tilt)
{
vel_tilt=this->max_speed;
vel_pan=(diff_pan/diff_tilt)*this->max_speed;
vel_roll=(diff_roll/diff_tilt)*this->max_speed;
if(diff_pan>diff_roll)
{
vel_pan=this->max_speed;
vel_tilt=(diff_tilt/diff_pan)*this->max_speed;
vel_roll=(diff_roll/diff_pan)*this->max_speed;
}
else
{
vel_tilt=this->max_speed;
vel_pan=(diff_pan/diff_tilt)*this->max_speed;
vel_roll=(diff_roll/diff_tilt)*this->max_speed;
}
}
else
{
vel_roll=this->max_speed;
vel_pan=(diff_pan/diff_roll)*this->max_speed;
vel_tilt=(diff_tilt/diff_roll)*this->max_speed;
if(diff_tilt>diff_roll)
{
vel_tilt=this->max_speed;
vel_pan=(diff_pan/diff_tilt)*this->max_speed;
vel_roll=(diff_roll/diff_tilt)*this->max_speed;
}
else
{
vel_roll=this->max_speed;
vel_pan=(diff_pan/diff_roll)*this->max_speed;
vel_tilt=(diff_tilt/diff_roll)*this->max_speed;
}
}
this->motors[PAN]->move_absolute_angle(this->stored_poses[pose_id].pan,vel_pan);
this->motors[TILT]->move_absolute_angle(this->stored_poses[pose_id].tilt,vel_tilt);
this->motors[ROLL]->move_absolute_angle(this->stored_poses[pose_id].roll,vel_roll);
this->access.exit();
}catch(CException &e){
this->access.exit();
throw e;
}
this->motors[PAN]->move_absolute_angle(this->stored_poses[pose_id].pan,vel_pan);
this->motors[TILT]->move_absolute_angle(this->stored_poses[pose_id].tilt,vel_tilt);
this->motors[ROLL]->move_absolute_angle(this->stored_poses[pose_id].roll,vel_roll);
}
}
void CArmControl::toggle_imu(void)
{
std::vector<double> euler;
if(this->imu_enabled)
{
this->imu_enabled=false;
digitalWrite(IMU_MODE,LOW);
}
else
{
this->imu_enabled=true;
digitalWrite(IMU_MODE,HIGH);
euler=this->imu.get_orientation_euler();
this->imu_zero.pan=euler[0];
this->imu_zero.tilt=euler[1];
this->imu_zero.roll=euler[2];
}
}
......@@ -554,5 +707,8 @@ CArmControl::~CArmControl()
delete this->motors[ROLL];
this->motors[ROLL]=NULL;
}
digitalWrite(IMU_CAL,LOW);
digitalWrite(IMU_MODE,LOW);
digitalWrite(OK_LED,LOW);
}
src/examples/arm_control_test.cpp
View file @ 648eb435
......@@ -4,13 +4,14 @@
std::string dyn_serial="FT2FWOYO";
std::string joy_device="/dev/input/js0";
std::string imu_device="/dev/ttyAMA0";
int main(int argc, char *argv[])