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

Solved some bugs.

Conditional compilation of Raspberry PI stuff.
parent cb90ed51
...@@ -30,7 +30,7 @@ TARGET_LINK_LIBRARIES(arm_control ${dynamixel_LIBRARY}) ...@@ -30,7 +30,7 @@ TARGET_LINK_LIBRARIES(arm_control ${dynamixel_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${dynamixel_motor_cont_LIBRARY}) TARGET_LINK_LIBRARIES(arm_control ${dynamixel_motor_cont_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${bno055_imu_driver_LIBRARY}) TARGET_LINK_LIBRARIES(arm_control ${bno055_imu_driver_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${XSD_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_DEPENDENCIES(arm_control xsd_files_gen)
......
...@@ -11,7 +11,9 @@ ...@@ -11,7 +11,9 @@
#include <errno.h> #include <errno.h>
#include <math.h> #include <math.h>
#include <linux/joystick.h> #include <linux/joystick.h>
#ifdef PI
#include <wiringPi.h> #include <wiringPi.h>
#endif
#include "xml/arm_motions_cfg_file.hxx" #include "xml/arm_motions_cfg_file.hxx"
#define OK_LED 25 #define OK_LED 25
...@@ -47,7 +49,7 @@ ...@@ -47,7 +49,7 @@
#endif #endif
#define MAX_PAN_ANGLE 80.0 #define MAX_PAN_ANGLE 80.0
#define MIN_PAN ANGLE -80.0 #define MIN_PAN_ANGLE -80.0
#define MAX_PAN_TORQUE 50.0 #define MAX_PAN_TORQUE 50.0
#define PAN_DIR 1.0 #define PAN_DIR 1.0
...@@ -135,13 +137,13 @@ void axis_callback(void *param,unsigned int axis_id, float value) ...@@ -135,13 +137,13 @@ void axis_callback(void *param,unsigned int axis_id, float value)
try{ try{
switch(axis_id) switch(axis_id)
{ {
case 0: if(arm->get_button_state(4)) case 0: if(arm->get_button_state(DEAD_MAN_SW))
arm->move(PAN,value); arm->move(PAN,value);
break; break;
case 1: if(arm->get_button_state(4)) case 1: if(arm->get_button_state(DEAD_MAN_SW))
arm->move(ROLL,value); arm->move(ROLL,value);
break; break;
case 3: if(arm->get_button_state(4)) case 3: if(arm->get_button_state(DEAD_MAN_SW))
arm->move(TILT,value); arm->move(TILT,value);
break; break;
} }
...@@ -154,6 +156,7 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de ...@@ -154,6 +156,7 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de
{ {
std::string name; std::string name;
#ifdef PI
wiringPiSetup(); wiringPiSetup();
pinMode(IMU_CAL,OUTPUT); pinMode(IMU_CAL,OUTPUT);
digitalWrite(IMU_CAL,LOW); digitalWrite(IMU_CAL,LOW);
...@@ -161,6 +164,7 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de ...@@ -161,6 +164,7 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de
digitalWrite(IMU_MODE,LOW); digitalWrite(IMU_MODE,LOW);
pinMode(OK_LED,OUTPUT); pinMode(OK_LED,OUTPUT);
digitalWrite(OK_LED,LOW); digitalWrite(OK_LED,LOW);
#endif
this->exit_flag=false; this->exit_flag=false;
// detect and initialize servos // detect and initialize servos
this->dyn_server=CDynamixelServerSerial::instance(); this->dyn_server=CDynamixelServerSerial::instance();
...@@ -190,14 +194,17 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de ...@@ -190,14 +194,17 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de
this->target_angles.pan=this->motors[PAN]->get_current_angle(); this->target_angles.pan=this->motors[PAN]->get_current_angle();
this->target_speeds.pan=this->max_speed; this->target_speeds.pan=this->max_speed;
this->target_accels.pan=this->max_accel; this->target_accels.pan=this->max_accel;
this->target_torques.pan=0.0;
this->target_dir.pan=PAN_DIR; this->target_dir.pan=PAN_DIR;
this->target_angles.tilt=this->motors[TILT]->get_current_angle(); this->target_angles.tilt=this->motors[TILT]->get_current_angle();
this->target_speeds.tilt=this->max_speed; this->target_speeds.tilt=this->max_speed;
this->target_accels.tilt=this->max_accel; this->target_accels.tilt=this->max_accel;
this->target_torques.tilt=0.0;
this->target_dir.tilt=TILT_DIR; this->target_dir.tilt=TILT_DIR;
this->target_angles.roll=this->motors[ROLL]->get_current_angle(); this->target_angles.roll=this->motors[ROLL]->get_current_angle();
this->target_speeds.roll=this->max_speed; this->target_speeds.roll=this->max_speed;
this->target_accels.roll=this->max_accel; this->target_accels.roll=this->max_accel;
this->target_torques.roll=0.0;
this->target_dir.roll=ROLL_DIR; this->target_dir.roll=ROLL_DIR;
this->target_run=false; this->target_run=false;
this->enable_torque(true); this->enable_torque(true);
...@@ -234,7 +241,9 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de ...@@ -234,7 +241,9 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de
this->finish_thread_event_id="arm_control_finish_thread_event"; this->finish_thread_event_id="arm_control_finish_thread_event";
this->event_server->create_event(this->finish_thread_event_id); this->event_server->create_event(this->finish_thread_event_id);
this->thread_server->start_thread(this->position_monitor_thread_id); this->thread_server->start_thread(this->position_monitor_thread_id);
#ifdef PI
digitalWrite(OK_LED,HIGH); digitalWrite(OK_LED,HIGH);
#endif
} }
void *CArmControl::position_monitor_thread(void *param) void *CArmControl::position_monitor_thread(void *param)
...@@ -274,8 +283,10 @@ void *CArmControl::position_monitor_thread(void *param) ...@@ -274,8 +283,10 @@ void *CArmControl::position_monitor_thread(void *param)
{ {
save_cal=false; save_cal=false;
arm->imu.save_calibration("/home/pi/bno055.cal"); arm->imu.save_calibration("/home/pi/bno055.cal");
} }
#ifdef PI
digitalWrite(IMU_CAL,HIGH); digitalWrite(IMU_CAL,HIGH);
#endif
} }
}catch(CException &e){ }catch(CException &e){
std::cout << e.what() << std::endl; std::cout << e.what() << std::endl;
...@@ -295,21 +306,21 @@ void *CArmControl::position_monitor_thread(void *param) ...@@ -295,21 +306,21 @@ void *CArmControl::position_monitor_thread(void *param)
else if(current_pos>arm->max_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); arm->motors[PAN]->move_absolute_angle(current_pos,50.0);
else else
arm->motors[PAN]->move_torque(arm->target_torques.pan); arm->motors[PAN]->move_pwm(arm->target_torques.pan);
current_pos=arm->motors[TILT]->get_current_angle(); current_pos=arm->motors[TILT]->get_current_angle();
if(current_pos>arm->max_angles.tilt && arm->target_torques.tilt<0.0) if(current_pos>arm->max_angles.tilt && arm->target_torques.tilt<0.0)
arm->motors[TILT]->move_absolute_angle(current_pos,50.0); arm->motors[TILT]->move_absolute_angle(current_pos,50.0);
else if(current_pos<arm->min_angles.tilt && arm->target_torques.tilt>0.0) else if(current_pos<arm->min_angles.tilt && arm->target_torques.tilt>0.0)
arm->motors[TILT]->move_absolute_angle(current_pos,50.0); arm->motors[TILT]->move_absolute_angle(current_pos,50.0);
else else
arm->motors[TILT]->move_torque(-arm->target_torques.tilt); arm->motors[TILT]->move_pwm(-arm->target_torques.tilt);
current_pos=arm->motors[ROLL]->get_current_angle(); current_pos=arm->motors[ROLL]->get_current_angle();
if(current_pos>arm->max_angles.roll && arm->target_torques.roll<0.0) 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) else if(current_pos<arm->min_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 else
arm->motors[ROLL]->move_torque(-arm->target_torques.roll); arm->motors[ROLL]->move_pwm(-arm->target_torques.roll);
arm->access.exit(); arm->access.exit();
} }
else if(arm->imu_enabled && imu_calibrated) else if(arm->imu_enabled && imu_calibrated)
...@@ -333,7 +344,7 @@ void *CArmControl::position_monitor_thread(void *param) ...@@ -333,7 +344,7 @@ void *CArmControl::position_monitor_thread(void *param)
else if(pan<arm->min_angles.pan) else if(pan<arm->min_angles.pan)
pan=arm->min_angles.pan; pan=arm->min_angles.pan;
current_pos=arm->motors[PAN]->get_current_angle(); current_pos=arm->motors[PAN]->get_current_angle();
arm->motors[PAN]->move_torque(pan-current_pos); arm->motors[PAN]->move_pwm(pan-current_pos);
tilt=0; tilt=0;
for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++) for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
tilt+=-euler[i][1]; tilt+=-euler[i][1];
...@@ -343,7 +354,7 @@ void *CArmControl::position_monitor_thread(void *param) ...@@ -343,7 +354,7 @@ void *CArmControl::position_monitor_thread(void *param)
else if(tilt<arm->min_angles.tilt) else if(tilt<arm->min_angles.tilt)
tilt=arm->min_angles.tilt; tilt=arm->min_angles.tilt;
current_pos=arm->motors[TILT]->get_current_angle(); current_pos=arm->motors[TILT]->get_current_angle();
arm->motors[TILT]->move_torque(tilt-current_pos); arm->motors[TILT]->move_pwm(tilt-current_pos);
roll=0; roll=0;
for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++) for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
roll+=euler[i][2]; roll+=euler[i][2];
...@@ -353,14 +364,14 @@ void *CArmControl::position_monitor_thread(void *param) ...@@ -353,14 +364,14 @@ void *CArmControl::position_monitor_thread(void *param)
else if(roll<arm->min_angles.roll) else if(roll<arm->min_angles.roll)
roll=arm->min_angles.roll; roll=arm->min_angles.roll;
current_pos=arm->motors[ROLL]->get_current_angle(); current_pos=arm->motors[ROLL]->get_current_angle();
arm->motors[ROLL]->move_torque(roll-current_pos); arm->motors[ROLL]->move_pwm(roll-current_pos);
arm->access.exit(); arm->access.exit();
} }
} }
else else
arm->move_joints(); arm->move_joints();
}catch(CException &e){ }catch(CException &e){
std::cout << e.what() << std::endl; std::cout << e.what() << std::endl;
arm->access.exit(); arm->access.exit();
} }
if(arm->get_button_state(EXIT)) if(arm->get_button_state(EXIT))
...@@ -598,8 +609,8 @@ void CArmControl::set_max_torque(unsigned int axis_id, double max) ...@@ -598,8 +609,8 @@ void CArmControl::set_max_torque(unsigned int axis_id, double max)
{ {
try{ try{
this->access.enter(); this->access.enter();
this->motors[PAN]->set_max_torque(max); this->motors[PAN]->set_max_pwm(max);
this->motors[PAN]->set_limit_torque(max); this->motors[PAN]->set_pwm_limit(max);
this->access.exit(); this->access.exit();
this->max_torques.pan=max; this->max_torques.pan=max;
}catch(CException &e){ }catch(CException &e){
...@@ -611,8 +622,8 @@ void CArmControl::set_max_torque(unsigned int axis_id, double max) ...@@ -611,8 +622,8 @@ void CArmControl::set_max_torque(unsigned int axis_id, double max)
{ {
try{ try{
this->access.enter(); this->access.enter();
this->motors[TILT]->set_max_torque(max); this->motors[TILT]->set_max_pwm(max);
this->motors[TILT]->set_limit_torque(max); this->motors[TILT]->set_pwm_limit(max);
this->access.exit(); this->access.exit();
this->max_torques.tilt=max; this->max_torques.tilt=max;
}catch(CException &e){ }catch(CException &e){
...@@ -624,8 +635,8 @@ void CArmControl::set_max_torque(unsigned int axis_id, double max) ...@@ -624,8 +635,8 @@ void CArmControl::set_max_torque(unsigned int axis_id, double max)
{ {
try{ try{
this->access.enter(); this->access.enter();
this->motors[ROLL]->set_max_torque(max); this->motors[ROLL]->set_max_pwm(max);
this->motors[ROLL]->set_limit_torque(max); this->motors[ROLL]->set_pwm_limit(max);
this->access.exit(); this->access.exit();
this->max_torques.roll=max; this->max_torques.roll=max;
}catch(CException &e){ }catch(CException &e){
...@@ -984,13 +995,17 @@ void CArmControl::toggle_imu(void) ...@@ -984,13 +995,17 @@ void CArmControl::toggle_imu(void)
if(this->imu_enabled) if(this->imu_enabled)
{ {
this->imu_enabled=false; this->imu_enabled=false;
#ifdef PI
digitalWrite(IMU_MODE,LOW); digitalWrite(IMU_MODE,LOW);
#endif
this->stop(); this->stop();
} }
else else
{ {
this->imu_enabled=true; this->imu_enabled=true;
#ifdef PI
digitalWrite(IMU_MODE,HIGH); digitalWrite(IMU_MODE,HIGH);
#endif
euler=this->imu.get_orientation_euler(); euler=this->imu.get_orientation_euler();
this->imu_zero.pan=euler[0]; this->imu_zero.pan=euler[0];
this->imu_zero.tilt=euler[1]; this->imu_zero.tilt=euler[1];
...@@ -1033,8 +1048,10 @@ CArmControl::~CArmControl() ...@@ -1033,8 +1048,10 @@ CArmControl::~CArmControl()
delete this->motors[ROLL]; delete this->motors[ROLL];
this->motors[ROLL]=NULL; this->motors[ROLL]=NULL;
} }
#ifdef PI
digitalWrite(IMU_CAL,LOW); digitalWrite(IMU_CAL,LOW);
digitalWrite(IMU_MODE,LOW); digitalWrite(IMU_MODE,LOW);
digitalWrite(OK_LED,LOW); digitalWrite(OK_LED,LOW);
#endif
} }
...@@ -2,16 +2,24 @@ ...@@ -2,16 +2,24 @@
#include "exceptions.h" #include "exceptions.h"
#include <iostream> #include <iostream>
#ifdef PI
std::string dyn_device="/dev/ttyUSB0"; std::string dyn_device="/dev/ttyUSB0";
std::string joy_device="/dev/input/js0"; std::string joy_device="/dev/input/js0";
std::string imu_device="/dev/ttyAMA0"; std::string imu_device="/dev/ttyAMA0";
std::string motion_file_path="/home/pi/code/arm_control/src/xml/poses.xml";
#else
std::string dyn_device="/dev/ttyUSB0";
std::string joy_device="/dev/input/js0";
std::string imu_device="/dev/ttyUSB2";
std::string motion_file_path="/home/sergi/Downloads/arm_control/src/xml/poses.xml";
#endif
int main(int argc, char *argv[]) int main(int argc, char *argv[])
{ {
try{ try{
CArmControl arm(joy_device,imu_device,dyn_device,1000000,6,5,4); 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.load_poses(motion_file_path);
arm.set_max_angles(PAN,130.0,-130.0); arm.set_max_angles(PAN,130.0,-130.0);
arm.set_max_torque(PAN,30); arm.set_max_torque(PAN,30);
arm.set_max_angles(TILT,90.0,-90.0); arm.set_max_angles(TILT,90.0,-90.0);
......
...@@ -2,9 +2,9 @@ ...@@ -2,9 +2,9 @@
<arm_motions xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="arm_motions_cfg.xsd"> <arm_motions xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="arm_motions_cfg.xsd">
<arm_motion> <arm_motion>
<button_name>triangle</button_name> <button_name>triangle</button_name>
<pan>95.7478</pan> <pan>-11.2903</pan>
<tilt>-1.31965</tilt> <tilt>-7.47801</tilt>
<roll>-10.9971</roll> <roll>-4.83871</roll>
</arm_motion> </arm_motion>
<arm_motion> <arm_motion>
<button_name>circle</button_name> <button_name>circle</button_name>
...@@ -14,14 +14,14 @@ ...@@ -14,14 +14,14 @@
</arm_motion> </arm_motion>
<arm_motion> <arm_motion>
<button_name>cross</button_name> <button_name>cross</button_name>
<pan>-109.824</pan> <pan>-11.2903</pan>
<tilt>3.66569</tilt> <tilt>-7.47801</tilt>
<roll>-40.0293</roll> <roll>-4.83871</roll>
</arm_motion> </arm_motion>
<arm_motion> <arm_motion>
<button_name>square</button_name> <button_name>square</button_name>
<pan>-99.2669</pan> <pan>-11.2903</pan>
<tilt>-4.2522</tilt> <tilt>-7.47801</tilt>
<roll>-4.54545</roll> <roll>-4.83871</roll>
</arm_motion> </arm_motion>
</arm_motions> </arm_motions>
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