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

Solved some bugs. 

Conditional compilation of Raspberry PI stuff.
parent cb90ed51
Hide whitespace changes
Inline Side-by-side
src/CMakeLists.txt
View file @ ea0a24ef
......@@ -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 ${bno055_imu_driver_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)
......
src/arm_control.cpp
View file @ ea0a24ef
......@@ -11,7 +11,9 @@
#include <errno.h>
#include <math.h>
#include <linux/joystick.h>
#ifdef PI
#include <wiringPi.h>
#endif
#include "xml/arm_motions_cfg_file.hxx"
#define OK_LED 25
......@@ -47,7 +49,7 @@
#endif
#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 PAN_DIR 1.0
......@@ -135,13 +137,13 @@ void axis_callback(void *param,unsigned int axis_id, float value)
try{
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);
break;
case 1: if(arm->get_button_state(4))
case 1: if(arm->get_button_state(DEAD_MAN_SW))
arm->move(ROLL,value);
break;
case 3: if(arm->get_button_state(4))
case 3: if(arm->get_button_state(DEAD_MAN_SW))
arm->move(TILT,value);
break;
}
......@@ -154,6 +156,7 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de
{
std::string name;
#ifdef PI
wiringPiSetup();
pinMode(IMU_CAL,OUTPUT);
digitalWrite(IMU_CAL,LOW);
......@@ -161,6 +164,7 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de
digitalWrite(IMU_MODE,LOW);
pinMode(OK_LED,OUTPUT);
digitalWrite(OK_LED,LOW);
#endif
this->exit_flag=false;
// detect and initialize servos
this->dyn_server=CDynamixelServerSerial::instance();
......@@ -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_speeds.pan=this->max_speed;
this->target_accels.pan=this->max_accel;
this->target_torques.pan=0.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_torques.tilt=0.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_torques.roll=0.0;
this->target_dir.roll=ROLL_DIR;
this->target_run=false;
this->enable_torque(true);
......@@ -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->event_server->create_event(this->finish_thread_event_id);
this->thread_server->start_thread(this->position_monitor_thread_id);
#ifdef PI
digitalWrite(OK_LED,HIGH);
#endif
}
void *CArmControl::position_monitor_thread(void *param)
......@@ -274,8 +283,10 @@ void *CArmControl::position_monitor_thread(void *param)
{
save_cal=false;
arm->imu.save_calibration("/home/pi/bno055.cal");
}
}
#ifdef PI
digitalWrite(IMU_CAL,HIGH);
#endif
}
}catch(CException &e){
std::cout << e.what() << std::endl;
......@@ -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)
arm->motors[PAN]->move_absolute_angle(current_pos,50.0);
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();
if(current_pos>arm->max_angles.tilt && arm->target_torques.tilt<0.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)
arm->motors[TILT]->move_absolute_angle(current_pos,50.0);
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();
if(current_pos>arm->max_angles.roll && arm->target_torques.roll<0.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
arm->motors[ROLL]->move_torque(-arm->target_torques.roll);
arm->motors[ROLL]->move_pwm(-arm->target_torques.roll);
arm->access.exit();
}
else if(arm->imu_enabled && imu_calibrated)
......@@ -333,7 +344,7 @@ void *CArmControl::position_monitor_thread(void *param)
else if(pan<arm->min_angles.pan)
pan=arm->min_angles.pan;
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;
for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
tilt+=-euler[i][1];
......@@ -343,7 +354,7 @@ void *CArmControl::position_monitor_thread(void *param)
else if(tilt<arm->min_angles.tilt)
tilt=arm->min_angles.tilt;
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;
for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
roll+=euler[i][2];
......@@ -353,14 +364,14 @@ void *CArmControl::position_monitor_thread(void *param)
else if(roll<arm->min_angles.roll)
roll=arm->min_angles.roll;
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();
}
}
else
arm->move_joints();
}catch(CException &e){
std::cout << e.what() << std::endl;
std::cout << e.what() << std::endl;
arm->access.exit();
}
if(arm->get_button_state(EXIT))
......@@ -598,8 +609,8 @@ void CArmControl::set_max_torque(unsigned int axis_id, double max)
{
try{
this->access.enter();
this->motors[PAN]->set_max_torque(max);
this->motors[PAN]->set_limit_torque(max);
this->motors[PAN]->set_max_pwm(max);
this->motors[PAN]->set_pwm_limit(max);
this->access.exit();
this->max_torques.pan=max;
}catch(CException &e){
......@@ -611,8 +622,8 @@ void CArmControl::set_max_torque(unsigned int axis_id, double max)
{
try{
this->access.enter();
this->motors[TILT]->set_max_torque(max);
this->motors[TILT]->set_limit_torque(max);
this->motors[TILT]->set_max_pwm(max);
this->motors[TILT]->set_pwm_limit(max);
this->access.exit();
this->max_torques.tilt=max;
}catch(CException &e){
......@@ -624,8 +635,8 @@ void CArmControl::set_max_torque(unsigned int axis_id, double max)
{
try{
this->access.enter();
this->motors[ROLL]->set_max_torque(max);
this->motors[ROLL]->set_limit_torque(max);
this->motors[ROLL]->set_max_pwm(max);
this->motors[ROLL]->set_pwm_limit(max);
this->access.exit();
this->max_torques.roll=max;
}catch(CException &e){
......@@ -984,13 +995,17 @@ void CArmControl::toggle_imu(void)
if(this->imu_enabled)
{
this->imu_enabled=false;
#ifdef PI
digitalWrite(IMU_MODE,LOW);
#endif
this->stop();
}
else
{
this->imu_enabled=true;
#ifdef PI
digitalWrite(IMU_MODE,HIGH);
#endif
euler=this->imu.get_orientation_euler();
this->imu_zero.pan=euler[0];
this->imu_zero.tilt=euler[1];
......@@ -1033,8 +1048,10 @@ CArmControl::~CArmControl()
delete this->motors[ROLL];
this->motors[ROLL]=NULL;
}
#ifdef PI
digitalWrite(IMU_CAL,LOW);
digitalWrite(IMU_MODE,LOW);
digitalWrite(OK_LED,LOW);
#endif
}
src/examples/arm_control_test.cpp
View file @ ea0a24ef
......@@ -2,16 +2,24 @@
#include "exceptions.h"
#include <iostream>
#ifdef PI
std::string dyn_device="/dev/ttyUSB0";
std::string joy_device="/dev/input/js0";
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[])
{
try{
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_torque(PAN,30);
arm.set_max_angles(TILT,90.0,-90.0);
......
src/xml/poses.xml
View file @ ea0a24ef
......@@ -2,9 +2,9 @@
<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>95.7478</pan>
<tilt>-1.31965</tilt>
<roll>-10.9971</roll>
<pan>-11.2903</pan>
<tilt>-7.47801</tilt>
<roll>-4.83871</roll>
</arm_motion>
<arm_motion>
<button_name>circle</button_name>
......@@ -14,14 +14,14 @@
</arm_motion>
<arm_motion>
<button_name>cross</button_name>
<pan>-109.824</pan>
<tilt>3.66569</tilt>
<roll>-40.0293</roll>
<pan>-11.2903</pan>
<tilt>-7.47801</tilt>
<roll>-4.83871</roll>
</arm_motion>
<arm_motion>
<button_name>square</button_name>
<pan>-99.2669</pan>
<tilt>-4.2522</tilt>
<roll>-4.54545</roll>
<pan>-11.2903</pan>
<tilt>-7.47801</tilt>
<roll>-4.83871</roll>
</arm_motion>
</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