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})
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)
......
......@@ -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)
......@@ -275,7 +284,9 @@ 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,7 +364,7 @@ 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();
}
}
......@@ -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
}
......@@ -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);
......
......@@ -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