diff --git a/include/arm_control.h b/include/arm_control.h
index d4a7360e4e6ad18c80717dc1441e7c9f9ada887e..ac08784425122e20574dc9c205acf05adfcc6ede 100644
--- a/include/arm_control.h
+++ b/include/arm_control.h
@@ -1,7 +1,7 @@
#ifndef _ARM_CONTROL_H
#define _ARM_CONTROL_H
-#include "dynamixelserver_ftdi.h"
+#include "dynamixelserver_serial.h"
#include "dynamixel_motor.h"
#include "threadserver.h"
#include "eventserver.h"
@@ -28,14 +28,24 @@ class CArmControl
CEventServer *event_server;
std::string finish_thread_event_id;
std::vector<CDynamixelMotor *> motors;
- CDynamixelServerFTDI *dyn_server;
+ unsigned char pan_id;
+ unsigned char tilt_id;
+ unsigned char roll_id;
+ CDynamixelServerSerial *dyn_server;
CJoystick joy;
std::vector<bool> current_button_state;
double max_speed;
+ double max_accel;
TJoints max_angles;
TJoints min_angles;
TJoints max_torques;
TJoints target_torques;
+ TJoints target_angles;
+ TJoints target_stop_angles;
+ TJoints target_speeds;
+ TJoints target_accels;
+ TJoints target_dir;
+ bool target_run;
std::vector<TJoints> stored_poses;
std::string poses_file;
CBNO055IMUDriver imu;
@@ -43,6 +53,7 @@ class CArmControl
TJoints imu_zero;
protected:
static void *position_monitor_thread(void *param);
+ void move_joints(void);
public:
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);
diff --git a/src/arm_control.cpp b/src/arm_control.cpp
index 7d9552849e5dec94defe53f527708d1895b36c66..efa834ddc9cd3e2704a78d06be3123844cb2a1f3 100644
--- a/src/arm_control.cpp
+++ b/src/arm_control.cpp
@@ -1,5 +1,6 @@
#include "arm_control.h"
#include "exceptions.h"
+#include "eventexceptions.h"
#include <iostream>
#include <fstream>
#include <unistd.h>
@@ -17,6 +18,8 @@
#define IMU_MODE 23
#define IMU_CAL 21
+#define IMU_NUM_SAMPLES 5
+
void button_callback(void *param,unsigned int button_id,bool level)
{
CArmControl *arm=(CArmControl *)param;
@@ -24,58 +27,58 @@ void button_callback(void *param,unsigned int button_id,bool level)
try{
switch(button_id)
{
- case 3: if(level)
+ case 9: if(level)
arm->toggle_imu();
break;
- case 4: if(level)
- arm->increase_max_speed();
- break;
+ case 13: if(level)
+ arm->increase_max_speed();
+ break;
+ case 14: if(level)
+ arm->decrease_max_speed();
+ break;
case 6: if(level)
- arm->decrease_max_speed();
- break;
- case 8: if(level)
arm->enable_torque(false);
else
arm->enable_torque(true);
break;
- case 10: if(!level)
- {
- // button rear left 1 released -> keep position
- arm->stop();
- }
- break;
- case 12: if(level)
- {
- if(arm->get_button_state(11))
- arm->update_pose(0);// update circle pose
- else
- arm->execute_pose(0);// execute circle pose
- }
- break;
- case 13: if(level)
- {
- if(arm->get_button_state(11))
- arm->update_pose(1);// update circle pose
- else
- arm->execute_pose(1);// execute circle pose
- }
- break;
- case 14: if(level)
- {
- if(arm->get_button_state(11))
- arm->update_pose(2);// update circle pose
- else
- arm->execute_pose(2);// execute circle pose
- }
- break;
- case 15: if(level)
- {
- if(arm->get_button_state(11))
- arm->update_pose(3);// update circle pose
- else
- arm->execute_pose(3);// execute circle pose
- }
- break;
+ case 4: if(!level)
+ {
+ // button rear left 1 released -> keep position
+ arm->stop();
+ }
+ break;
+ case 2: if(level)
+ {
+ if(arm->get_button_state(5))
+ arm->update_pose(0);// update circle pose
+ else
+ arm->execute_pose(0);// execute circle pose
+ }
+ break;
+ case 1: if(level)
+ {
+ if(arm->get_button_state(5))
+ arm->update_pose(1);// update circle pose
+ else
+ arm->execute_pose(1);// execute circle pose
+ }
+ break;
+ case 0: if(level)
+ {
+ if(arm->get_button_state(5))
+ arm->update_pose(2);// update circle pose
+ else
+ arm->execute_pose(2);// execute circle pose
+ }
+ break;
+ case 3: if(level)
+ {
+ if(arm->get_button_state(5))
+ arm->update_pose(3);// update circle pose
+ else
+ arm->execute_pose(3);// execute circle pose
+ }
+ break;
}
}catch(CException &e){
std::cout << e.what() << std::endl;
@@ -89,13 +92,13 @@ void axis_callback(void *param,unsigned int axis_id, float value)
try{
switch(axis_id)
{
- case 0: if(arm->get_button_state(10))
+ case 0: if(arm->get_button_state(4))
arm->move(PAN,value);
break;
- case 1: if(arm->get_button_state(10))
+ case 1: if(arm->get_button_state(4))
arm->move(TILT,value);
break;
- case 2: if(arm->get_button_state(10))
+ case 3: if(arm->get_button_state(4))
arm->move(ROLL,value);
break;
}
@@ -117,29 +120,43 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de
digitalWrite(OK_LED,LOW);
this->exit_flag=false;
// detect and initialize servos
- this->dyn_server=CDynamixelServerFTDI::instance();
- if(this->dyn_server->get_num_buses()>0)
- {
- this->dyn_server->config_bus(dyn_serial,baudrate);
- sleep(1);
- this->motors.resize(3);
- name="arm_control_pan";
- this->motors[PAN] = new CDynamixelMotor(name,this->dyn_server,pan_id);
- this->set_max_angles(PAN,90.0,-90.0);
- this->set_max_torque(PAN,50.0);
- name="arm_control_tilt";
- this->motors[TILT] = new CDynamixelMotor(name,this->dyn_server,tilt_id);
- this->set_max_angles(TILT,90.0,-90.0);
- this->set_max_torque(TILT,50.0);
- name="arm_control_roll";
- this->motors[ROLL] = new CDynamixelMotor(name,this->dyn_server,roll_id);
- this->set_max_angles(ROLL,90.0,-90.0);
- this->set_max_torque(ROLL,50.0);
- }
- else
- throw CException(_HERE_,"No Dynamixel buses available");
+ this->dyn_server=CDynamixelServerSerial::instance();
+ this->dyn_server->config_bus(dyn_serial,baudrate);
+ this->motors.resize(3);
+ name="arm_control_pan";
+ this->motors[PAN] = new CDynamixelMotor(name,this->dyn_server,pan_id);
+ this->motors[PAN]->set_position_range(-150.0,150.0);
+ this->set_max_angles(PAN,90.0,-90.0);
+ this->set_max_torque(PAN,50.0);
+ this->pan_id=pan_id;
+ name="arm_control_tilt";
+ this->motors[TILT] = new CDynamixelMotor(name,this->dyn_server,tilt_id);
+ this->motors[PAN]->set_position_range(-150.0,150.0);
+ this->set_max_angles(TILT,90.0,-90.0);
+ this->set_max_torque(TILT,50.0);
+ this->tilt_id=tilt_id;
+ name="arm_control_roll";
+ this->motors[ROLL] = new CDynamixelMotor(name,this->dyn_server,roll_id);
+ this->motors[PAN]->set_position_range(-150.0,150.0);
+ this->set_max_angles(ROLL,90.0,-90.0);
+ this->set_max_torque(ROLL,50.0);
+ this->roll_id=roll_id;
this->stored_poses.resize(4);
this->max_speed=50.0;
+ this->max_accel=100.0;
+ 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_dir.pan=1.0;
+ 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_dir.tilt=1.0;
+ 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_dir.roll=1.0;
+ this->target_run=false;
this->enable_torque(true);
// initialize IMU
this->imu.open(imu_device);
@@ -153,18 +170,18 @@ CArmControl::CArmControl(const std::string &joy_device,const std::string &imu_de
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);
- this->joy.enable_button_callback(10,button_callback,this);
- this->joy.enable_button_callback(12,button_callback,this);
this->joy.enable_button_callback(13,button_callback,this);
this->joy.enable_button_callback(14,button_callback,this);
- this->joy.enable_button_callback(15,button_callback,this);
+ this->joy.enable_button_callback(6,button_callback,this);
+ this->joy.enable_button_callback(9,button_callback,this);
+ this->joy.enable_button_callback(0,button_callback,this);
+ this->joy.enable_button_callback(1,button_callback,this);
+ this->joy.enable_button_callback(2,button_callback,this);
+ this->joy.enable_button_callback(3,button_callback,this);
this->joy.enable_position_change_callback(0,axis_callback,this);
this->joy.enable_position_change_callback(1,axis_callback,this);
- this->joy.enable_position_change_callback(2,axis_callback,this);
+ this->joy.enable_position_change_callback(3,axis_callback,this);
// create threads and events
this->thread_server=CThreadServer::instance();
this->position_monitor_thread_id="arm_control_position_monitor_thread";
@@ -183,9 +200,13 @@ void *CArmControl::position_monitor_thread(void *param)
double current_pos,pan,tilt,roll;
bool accel_cal,mag_cal,gyro_cal;
CArmControl *arm=(CArmControl *)param;
- std::vector<double> euler;
+ std::vector<double> euler[IMU_NUM_SAMPLES];
+ unsigned int current_sample=0,count=0;
+ std::string name;
- // initiate IMU calibration
+ for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
+ euler[i].resize(3);
+ // initiate IMU calibration
try{
arm->imu.load_calibration("/home/pi/bno055.cal");
arm->imu.set_operation_mode(ndof_mode);
@@ -222,68 +243,270 @@ void *CArmControl::position_monitor_thread(void *param)
else
{
try{
- if(arm->get_button_state(10))
+ if(arm->get_button_state(4))
{
arm->access.enter();
current_pos=arm->motors[PAN]->get_current_angle();
- if(current_pos>arm->max_angles.pan && arm->target_torques.pan>0.0)
+ if(current_pos>arm->max_angles.pan && arm->target_torques.pan<0.0)
arm->motors[PAN]->move_absolute_angle(current_pos,50.0);
- else if(current_pos<arm->min_angles.pan && arm->target_torques.pan<0.0)
+ else if(current_pos<arm->min_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_torque(-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)
+ 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)
+ 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_torque(-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)
+ 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_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();
+ count++;
+ if(count==10)
+ {
+ count=0;
+ arm->access.enter();
+ euler[current_sample]=arm->imu.get_orientation_euler();
+ if(current_sample==(IMU_NUM_SAMPLES-1))
+ current_sample=0;
+ else
+ current_sample++;
+ pan=0;
+ for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
+ pan+=arm->imu_zero.pan-(euler[i])[0];
+ pan/=IMU_NUM_SAMPLES;
+ 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);
+ tilt=0;
+ for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
+ tilt+=-euler[i][2];
+ tilt/=IMU_NUM_SAMPLES;
+ 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);
+ roll=0;
+ for(unsigned int i=0;i<IMU_NUM_SAMPLES;i++)
+ roll+=euler[i][1];
+ roll/=IMU_NUM_SAMPLES;
+ 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;
+ else
+ arm->move_joints();
}catch(CException &e){
+ std::cout << e.what() << std::endl;
arm->access.exit();
}
+ if(arm->get_button_state(8))
+ arm->exit_flag=true;
}
- usleep(100000);
+ usleep(10000);
}
pthread_exit(NULL);
}
+void CArmControl::move_joints(void)
+{
+ static TJoints current_speeds={0.0};
+ static TJoints current_angles={0.0};
+ static bool first=true;
+ bool moving=false;
+ std::string name;
+
+ if(this->target_run)
+ {
+ if(first)
+ {
+ first=false;
+ current_angles.pan=this->motors[PAN]->get_current_angle();
+ current_angles.tilt=this->motors[TILT]->get_current_angle();
+ current_angles.roll=this->motors[ROLL]->get_current_angle();
+ }
+ if(abs(this->target_angles.pan-current_angles.pan)>=1.0)
+ {
+ moving=true;
+ if(current_speeds.pan!=(this->target_dir.pan*this->target_speeds.pan))// it is necessary to change the current speed
+ {
+ current_angles.pan+=current_speeds.pan*0.01/2.0;
+ if(current_speeds.pan>this->target_dir.pan*this->target_speeds.pan)
+ {
+ current_speeds.pan-=this->target_accels.pan*0.01;// reduce speed
+ if(current_speeds.pan<this->target_dir.pan*this->target_speeds.pan)
+ current_speeds.pan=this->target_dir.pan*this->target_speeds.pan;
+ }
+ else
+ {
+ current_speeds.pan+=this->target_accels.pan*0.01;// increase speed
+ if(current_speeds.pan>this->target_dir.pan*this->target_speeds.pan)
+ current_speeds.pan=this->target_dir.pan*this->target_speeds.pan;
+ }
+ current_angles.pan+=current_speeds.pan*0.01/2.0;
+ if(this->target_dir.pan==1.0 && current_angles.pan>this->target_angles.pan)
+ current_angles.pan=this->target_angles.pan;
+ if(this->target_dir.pan==-1.0 && current_angles.pan<this->target_angles.pan)
+ current_angles.pan=this->target_angles.pan;
+ }
+ else
+ {
+ if(abs(this->target_angles.pan-current_angles.pan)>this->target_stop_angles.pan)// constant speed phase
+ {
+ current_angles.pan+=current_speeds.pan*0.01;
+ if(this->target_dir.pan==1.0 && current_angles.pan>this->target_angles.pan)
+ current_angles.pan=this->target_angles.pan;
+ if(this->target_dir.pan==-1.0 && current_angles.pan<this->target_angles.pan)
+ current_angles.pan=this->target_angles.pan;
+ }
+ else// deceleration phase
+ {
+ current_angles.pan=current_angles.pan+current_speeds.pan*0.01/2.0;
+ this->target_speeds.pan=this->target_speeds.pan-this->target_accels.pan*0.01;
+ if(this->target_speeds.pan<0)
+ this->target_speeds.pan=0;
+ current_speeds.pan=this->target_dir.pan*this->target_speeds.pan;
+ current_angles.pan=current_angles.pan+current_speeds.pan*0.01/2.0;
+ if(this->target_dir.pan==1.0 && current_angles.pan>this->target_angles.pan)
+ current_angles.pan=this->target_angles.pan;
+ if(this->target_dir.pan==-1.0 && current_angles.pan<this->target_angles.pan)
+ current_angles.pan=this->target_angles.pan;
+ }
+ }
+ }
+ else
+ current_speeds.pan=0;
+ if(abs(this->target_angles.tilt-current_angles.tilt)>=1.0)
+ {
+ moving=true;
+ if(current_speeds.tilt!=(this->target_dir.tilt*this->target_speeds.tilt))// it is necessary to change the current speed
+ {
+ current_angles.tilt+=current_speeds.tilt*0.01/2.0;
+ if(current_speeds.tilt>this->target_dir.tilt*this->target_speeds.tilt)
+ {
+ current_speeds.tilt-=this->target_accels.tilt*0.01;// reduce speed
+ if(current_speeds.tilt<this->target_dir.tilt*this->target_speeds.tilt)
+ current_speeds.tilt=this->target_dir.tilt*this->target_speeds.tilt;
+ }
+ else
+ {
+ current_speeds.tilt+=this->target_accels.tilt*0.01;// increase speed
+ if(current_speeds.tilt>this->target_dir.tilt*this->target_speeds.tilt)
+ current_speeds.tilt=this->target_dir.tilt*this->target_speeds.tilt;
+ }
+ current_angles.tilt+=current_speeds.tilt*0.01/2.0;
+ if(this->target_dir.tilt==1.0 && current_angles.tilt>this->target_angles.tilt)
+ current_angles.tilt=this->target_angles.tilt;
+ if(this->target_dir.tilt==-1.0 && current_angles.tilt<this->target_angles.tilt)
+ current_angles.tilt=this->target_angles.tilt;
+ }
+ else
+ {
+ if(abs(this->target_angles.tilt-current_angles.tilt)>this->target_stop_angles.tilt)// constant speed phase
+ {
+ current_angles.tilt+=current_speeds.tilt*0.01;
+ if(this->target_dir.tilt==1.0 && current_angles.tilt>this->target_angles.tilt)
+ current_angles.tilt=this->target_angles.tilt;
+ if(this->target_dir.tilt==-1.0 && current_angles.tilt<this->target_angles.tilt)
+ current_angles.tilt=this->target_angles.tilt;
+ }
+ else// deceleration phase
+ {
+ current_angles.tilt=current_angles.tilt+current_speeds.tilt*0.01/2.0;
+ this->target_speeds.tilt=this->target_speeds.tilt-this->target_accels.tilt*0.01;
+ if(this->target_speeds.tilt<0)
+ this->target_speeds.tilt=0;
+ current_speeds.tilt=this->target_dir.tilt*this->target_speeds.tilt;
+ current_angles.tilt=current_angles.tilt+current_speeds.tilt*0.01/2.0;
+ if(this->target_dir.tilt==1.0 && current_angles.tilt>this->target_angles.tilt)
+ current_angles.tilt=this->target_angles.tilt;
+ if(this->target_dir.tilt==-1.0 && current_angles.tilt<this->target_angles.tilt)
+ current_angles.tilt=this->target_angles.tilt;
+ }
+ }
+ }
+ else
+ current_speeds.tilt=0;
+ if(abs(this->target_angles.roll-current_angles.roll)>=1.0)
+ {
+ moving=true;
+ if(current_speeds.roll!=(this->target_dir.roll*this->target_speeds.roll))// it is necessary to change the current speed
+ {
+ current_angles.roll+=current_speeds.roll*0.01/2.0;
+ if(current_speeds.roll>this->target_dir.roll*this->target_speeds.roll)
+ {
+ current_speeds.roll-=this->target_accels.roll*0.01;// reduce speed
+ if(current_speeds.roll<this->target_dir.roll*this->target_speeds.roll)
+ current_speeds.roll=this->target_dir.roll*this->target_speeds.roll;
+ }
+ else
+ {
+ current_speeds.roll+=this->target_accels.roll*0.01;// increase speed
+ if(current_speeds.roll>this->target_dir.roll*this->target_speeds.roll)
+ current_speeds.roll=this->target_dir.roll*this->target_speeds.roll;
+ }
+ current_angles.roll+=current_speeds.roll*0.01/2.0;
+ if(this->target_dir.roll==1.0 && current_angles.roll>this->target_angles.roll)
+ current_angles.roll=this->target_angles.roll;
+ if(this->target_dir.roll==-1.0 && current_angles.roll<this->target_angles.roll)
+ current_angles.roll=this->target_angles.roll;
+ }
+ else
+ {
+ if(abs(this->target_angles.roll-current_angles.roll)>this->target_stop_angles.roll)// constant speed phase
+ {
+ current_angles.roll+=current_speeds.roll*0.01;
+ if(this->target_dir.roll==1.0 && current_angles.roll>this->target_angles.roll)
+ current_angles.roll=this->target_angles.roll;
+ if(this->target_dir.roll==-1.0 && current_angles.roll<this->target_angles.roll)
+ current_angles.roll=this->target_angles.roll;
+ }
+ else// deceleration phase
+ {
+ current_angles.roll=current_angles.roll+current_speeds.roll*0.01/2.0;
+ this->target_speeds.roll=this->target_speeds.roll-this->target_accels.roll*0.01;
+ if(this->target_speeds.roll<0)
+ this->target_speeds.roll=0;
+ current_speeds.roll=this->target_dir.roll*this->target_speeds.roll;
+ current_angles.roll=current_angles.roll+current_speeds.roll*0.01/2.0;
+ if(this->target_dir.roll==1.0 && current_angles.roll>this->target_angles.roll)
+ current_angles.roll=this->target_angles.roll;
+ if(this->target_dir.roll==-1.0 && current_angles.roll<this->target_angles.roll)
+ current_angles.roll=this->target_angles.roll;
+ }
+ }
+ }
+ else
+ current_speeds.roll=0;
+ if(!moving)
+ {
+ this->target_run=false;
+ first=true;
+ }
+ this->motors[PAN]->move_absolute_angle(current_angles.pan,0);
+ this->motors[TILT]->move_absolute_angle(current_angles.tilt,0);
+ this->motors[ROLL]->move_absolute_angle(current_angles.roll,0);
+ }
+}
+
bool CArmControl::get_button_state(unsigned int button_id)
{
if(button_id>this->current_button_state.size())
@@ -301,42 +524,18 @@ void CArmControl::set_max_angles(unsigned int axis_id, double max, double min)
{
if(axis_id==PAN)
{
- 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;
- }
+ this->max_angles.pan=max;
+ this->min_angles.pan=min;
}
else if(axis_id==TILT)
{
- 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;
- }
+ this->max_angles.tilt=max;
+ this->min_angles.tilt=min;
}
else if(axis_id==ROLL)
{
- 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;
- }
+ this->max_angles.roll=max;
+ this->min_angles.roll=min;
}
else
throw CException(_HERE_,"Invalid axis ID");
@@ -480,8 +679,8 @@ void CArmControl::stop(void)
void CArmControl::increase_max_speed(void)
{
this->max_speed+=10.0;
- if(this->max_speed>100.0)
- this->max_speed=100.0;
+ if(this->max_speed>300.0)
+ this->max_speed=300.0;
}
void CArmControl::decrease_max_speed(void)
@@ -600,8 +799,8 @@ void CArmControl::update_pose(unsigned int pose_id)
void CArmControl::execute_pose(unsigned int pose_id)
{
- double diff_pan, diff_tilt, diff_roll;
- double vel_pan, vel_tilt, vel_roll;
+ double diff_pan,diff_tilt,diff_roll;
+ double current_pan,current_tilt,current_roll;
if(pose_id>3)
throw CException(_HERE_,"Invalid pose ID");
@@ -609,42 +808,122 @@ void CArmControl::execute_pose(unsigned int pose_id)
{
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);
+ current_pan=this->motors[PAN]->get_current_angle();
+ if(this->stored_poses[pose_id].pan>this->max_angles.pan)
+ this->target_angles.pan=this->max_angles.pan;
+ else if(this->stored_poses[pose_id].pan<this->min_angles.pan)
+ this->target_angles.pan=this->min_angles.pan;
+ else
+ this->target_angles.pan=this->stored_poses[pose_id].pan;
+ diff_pan=fabs(current_pan-this->stored_poses[pose_id].pan);
+ current_tilt=this->motors[TILT]->get_current_angle();
+ if(this->stored_poses[pose_id].tilt>this->max_angles.tilt)
+ this->target_angles.tilt=this->max_angles.tilt;
+ else if(this->stored_poses[pose_id].tilt<this->min_angles.tilt)
+ this->target_angles.tilt=this->min_angles.tilt;
+ else
+ this->target_angles.tilt=this->stored_poses[pose_id].tilt;
+ diff_tilt=fabs(current_tilt-this->stored_poses[pose_id].tilt);
+ current_roll=this->motors[ROLL]->get_current_angle();
+ if(this->stored_poses[pose_id].roll>this->max_angles.roll)
+ this->target_angles.roll=this->max_angles.roll;
+ else if(this->stored_poses[pose_id].roll<this->min_angles.roll)
+ this->target_angles.roll=this->min_angles.roll;
+ else
+ this->target_angles.roll=this->stored_poses[pose_id].roll;
+ diff_roll=fabs(current_roll-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;
+ this->target_speeds.pan=this->max_speed;
+ this->target_speeds.tilt=(diff_tilt/diff_pan)*this->max_speed;
+ this->target_speeds.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;
+ if(diff_tilt>diff_roll)
+ {
+ this->target_speeds.tilt=this->max_speed;
+ this->target_speeds.pan=(diff_pan/diff_tilt)*this->max_speed;
+ this->target_speeds.roll=(diff_roll/diff_tilt)*this->max_speed;
+ }
+ else
+ {
+ this->target_speeds.roll=this->max_speed;
+ this->target_speeds.pan=(diff_pan/diff_roll)*this->max_speed;
+ this->target_speeds.tilt=(diff_tilt/diff_roll)*this->max_speed;
+ }
}
}
else
{
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;
+ this->target_speeds.tilt=this->max_speed;
+ this->target_speeds.pan=(diff_pan/diff_tilt)*this->max_speed;
+ this->target_speeds.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_roll>diff_pan)
+ {
+ this->target_speeds.roll=this->max_speed;
+ this->target_speeds.pan=(diff_pan/diff_roll)*this->max_speed;
+ this->target_speeds.tilt=(diff_tilt/diff_roll)*this->max_speed;
+ }
+ else
+ {
+ this->target_speeds.pan=this->max_speed;
+ this->target_speeds.tilt=(diff_tilt/diff_pan)*this->max_speed;
+ this->target_speeds.roll=(diff_roll/diff_pan)*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);
+ // check the parameters
+ if(diff_pan>=1.0)
+ {
+ if((this->target_speeds.pan*this->target_speeds.pan)/this->max_accel>diff_pan)
+ this->target_accels.pan=(this->target_speeds.pan*this->target_speeds.pan)/diff_pan;
+ else
+ this->target_accels.pan=this->max_accel;
+ }
+ // stop angles
+ this->target_stop_angles.pan=this->target_speeds.pan*this->target_speeds.pan/(2.0*this->target_accels.pan);
+ // the current angles, speeds and accelerations are in RAM
+ if(this->target_angles.pan>=current_pan)
+ this->target_dir.pan=1.0;
+ else
+ this->target_dir.pan=-1.0;
+ if(diff_tilt>=1.0)
+ {
+ if((this->target_speeds.tilt*this->target_speeds.tilt)/this->max_accel>diff_tilt)
+ this->target_accels.tilt=(this->target_speeds.tilt*this->target_speeds.tilt)/diff_tilt;
+ else
+ this->target_accels.tilt=this->max_accel;
+ }
+ // stop angles
+ this->target_stop_angles.tilt=this->target_speeds.tilt*this->target_speeds.tilt/(2.0*this->target_accels.tilt);
+ // the current angles, speeds and accelerations are in RAM
+ if(this->target_angles.tilt>=current_tilt)
+ this->target_dir.tilt=1.0;
+ else
+ this->target_dir.tilt=-1.0;
+ if(diff_roll>=1.0)
+ {
+ if((this->target_speeds.roll*this->target_speeds.roll)/this->max_accel>diff_roll)
+ this->target_accels.roll=(this->target_speeds.roll*this->target_speeds.roll)/diff_roll;
+ else
+ this->target_accels.roll=this->max_accel;
+ }
+ // stop angles
+ this->target_stop_angles.roll=this->target_speeds.roll*this->target_speeds.roll/(2.0*this->target_accels.roll);
+ // the current angles, speeds and accelerations are in RAM
+ if(this->target_angles.roll>=current_roll)
+ this->target_dir.roll=1.0;
+ else
+ this->target_dir.roll=-1.0;
+ this->target_run=true;
this->access.exit();
}catch(CException &e){
this->access.exit();
diff --git a/src/examples/arm_control_test.cpp b/src/examples/arm_control_test.cpp
index 35538f97ca18ffa687c8b5d3cb1e604e9b9d8ebf..b496c5f2b4549b2ec786e410296a9426c225329b 100644
--- a/src/examples/arm_control_test.cpp
+++ b/src/examples/arm_control_test.cpp
@@ -2,14 +2,14 @@
#include "exceptions.h"
#include <iostream>
-std::string dyn_serial="FT2FWOYO";
+std::string dyn_device="/dev/ttyUSB0";
std::string joy_device="/dev/input/js0";
std::string imu_device="/dev/ttyAMA0";
int main(int argc, char *argv[])
{
try{
- CArmControl arm(joy_device,imu_device,dyn_serial,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.set_max_angles(PAN,90.0,-90.0);
diff --git a/src/xml/poses.xml b/src/xml/poses.xml
index 912bddcc0527c2d96319eb25fca3386a51316b79..d122a2b2e26b55089b646bba5955a5d0a01fb07e 100644
--- a/src/xml/poses.xml
+++ b/src/xml/poses.xml
@@ -2,26 +2,26 @@
<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>2.49267</pan>
- <tilt>1.6129</tilt>
- <roll>-64.6628</roll>
+ <pan>90.176</pan>
+ <tilt>2.78592</tilt>
+ <roll>-3.37243</roll>
</arm_motion>
<arm_motion>
<button_name>circle</button_name>
- <pan>-6.59824</pan>
- <tilt>-3.07918</tilt>
+ <pan>-98.9736</pan>
+ <tilt>-8.06452</tilt>
<roll>-2.49267</roll>
</arm_motion>
<arm_motion>
<button_name>cross</button_name>
- <pan>-82.5513</pan>
- <tilt>-7.77126</tilt>
- <roll>28.2991</roll>
+ <pan>-3.37243</pan>
+ <tilt>-4.2522</tilt>
+ <roll>-92.8152</roll>
</arm_motion>
<arm_motion>
<button_name>square</button_name>
- <pan>-42.9619</pan>
- <tilt>38.563</tilt>
- <roll>-58.2111</roll>
+ <pan>-4.54545</pan>
+ <tilt>41.4956</tilt>
+ <roll>-90.7625</roll>
</arm_motion>
</arm_motions>