From ea0a24ef4ab658a8d8c7753c441e458f3fe77a86 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 18 Jun 2020 13:01:45 +0200
Subject: [PATCH] Solved some bugs. Conditional compilation of Raspberry PI
stuff.
---
src/CMakeLists.txt | 2 +-
src/arm_control.cpp | 53 ++++++++++++++++++++-----------
src/examples/arm_control_test.cpp | 10 +++++-
src/xml/poses.xml | 18 +++++------
4 files changed, 54 insertions(+), 29 deletions(-)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index d67a485..c3444b4 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -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)
diff --git a/src/arm_control.cpp b/src/arm_control.cpp
index 278b53f..89b6ecb 100644
--- a/src/arm_control.cpp
+++ b/src/arm_control.cpp
@@ -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
}
diff --git a/src/examples/arm_control_test.cpp b/src/examples/arm_control_test.cpp
index a2145de..4af2add 100644
--- a/src/examples/arm_control_test.cpp
+++ b/src/examples/arm_control_test.cpp
@@ -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);
diff --git a/src/xml/poses.xml b/src/xml/poses.xml
index e6919cb..71826c2 100644
--- a/src/xml/poses.xml
+++ b/src/xml/poses.xml
@@ -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>
--
GitLab