diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index da30e7f95c7395f1b535ce6d7d738162a009e4a8..e9174d4be1c4018c739c37e7ac80234c3f7d68d4 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -1301,7 +1301,7 @@ double CDynamixelMotor::get_current_voltage(void)
double CDynamixelMotor::get_current_effort(void)
{
- unsigned char data;
+ unsigned short int data;
double c_effort;
if(this->dynamixel_dev==NULL)
@@ -1311,12 +1311,15 @@ double CDynamixelMotor::get_current_effort(void)
}
else
{
- try{
- this->dynamixel_dev->read_byte_register(this->registers[current_load],&data);
+ try
+ {
+ this->dynamixel_dev->read_word_register(this->registers[current_load],&data);
c_effort = (double)((data&0x3FF)*100.0/1023);
- if(data&0x0400)
+ if(this->get_current_speed() < 0)
c_effort *= -1.0;
- }catch(CDynamixelAlarmException &e){
+ }
+ catch(CDynamixelAlarmException &e)
+ {
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
this->reset_motor();
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 0e48af04f65daca15748f568362d00b1b1f8b610..1ad63f80d50691cd00c1cb83351f03eee4873cfa 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -1226,7 +1226,7 @@ void CDynamixelMotorGroup::get_current_speed(std::vector<double> &speeds)
void CDynamixelMotorGroup::get_current_effort(std::vector<double> &efforts)
{
- unsigned short int c_effort;
+ unsigned short int c_effort, c_speed;
efforts.resize(this->servo_id.size());
@@ -1241,8 +1241,9 @@ void CDynamixelMotorGroup::get_current_effort(std::vector<double> &efforts)
{
try{
this->dynamixel_dev[i]->read_word_register(this->registers[i][current_load],&c_effort);
+ this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed],&c_speed);
efforts[i] = (double)((c_effort&0x3FF)*100.0/1023);
- if (c_effort&0x0400)
+ if (c_speed&0x0400)
efforts[i] *= -1.0;
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
diff --git a/src/dynamixel_registers.cpp b/src/dynamixel_registers.cpp
index 3aaf496294c24543e567a2bf2b978492cc1bdad1..5619600f6d3bac8832db5a677ca4ccde0a29aee0 100644
--- a/src/dynamixel_registers.cpp
+++ b/src/dynamixel_registers.cpp
@@ -35,9 +35,9 @@ unsigned short int std_compl_reg[39]={std_model_number,
std_current_voltage,
std_current_temp,
std_reg_inst,
+ (unsigned short int)-1,
std_moving,
std_lock,
- (unsigned short int)-1,
std_punch};
unsigned short int std_pid_reg[39]={std_model_number,
@@ -75,9 +75,9 @@ unsigned short int std_pid_reg[39]={std_model_number,
std_current_voltage,
std_current_temp,
std_reg_inst,
+ (unsigned short int)-1,
std_moving,
std_lock,
- (unsigned short int)-1,
std_punch};
unsigned short int xl_reg[39]={xl_model_number,
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index f096687ac1f02802a63274bd071cd9e50d2c85ae..11a716745759900ca37a62aef13ddbc037a3a0c0 100755
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -3,15 +3,20 @@
#include "exceptions.h"
#include "dynamixel_motor.h"
#include <iostream>
+#include <math.h>
std::string cont_name="RX-28";
std::string config_file="../src/xml/dyn_servo_config.xml";
int main(int argc, char *argv[])
{
+ std::string serial="A600cVjj"; //extracted from dynamixel library's test_dynamixel_server_no_scan or with 'dmesg' command: "SerialNumber: A600cVjj"
+ int baudrate = 57600; //1000000; //57600 or 1000000
+ int device = 1; //extracted from dynamixel library's test_dynamixel_server_no_scan
+
CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
- CDynamixelMotor *cont;
- std::string serial="A400gavq";
+ CDynamixelMotor *cont = NULL;
+
TDynamixel_info info;
double max_angle,min_angle;
unsigned char led_alarms,sd_alarms;
@@ -22,90 +27,212 @@ int main(int argc, char *argv[])
{
if(dyn_server->get_num_buses()>0)
{
- dyn_server->config_bus(serial,1000000);
- cont = new CDynamixelMotor(cont_name, dyn_server, 1);
+ dyn_server->config_bus(serial,baudrate);
+ cont = new CDynamixelMotor(cont_name, dyn_server, device);
+ /////////////////////////////////////////////////////////////////////////
+ ////INFO
+ /////////////////////////////////////////////////////////////////////////
+ ///*
+ std::cout << "-----------------------------------------------------------" << std::endl;
cont->get_servo_info(info);
- std::cout << "[INFO]: Servo model: " << info.model << std::endl;
- std::cout << "[INFO]: Firmware version: " << (int)info.firmware_ver << std::endl;
- std::cout << "[INFO]: Gear ratio: " << info.gear_ratio << std::endl;
+ std::cout << "[INFO]: Servo model: " << info.model << std::endl;
+ std::cout << "[INFO]: Firmware version: " << (int)info.firmware_ver << std::endl;
+ std::cout << "[INFO]: Gear ratio: " << info.gear_ratio << std::endl;
std::cout << "[INFO]: Encoder resolution: " << info.encoder_resolution << " counts" << std::endl;
if(info.pid_control)
{
- std::cout << "[INFO]: PID control capable" << std::endl;
- pid.p=10;
- pid.i=20;
- pid.d=30;
- cont->set_pid_control(pid);
- cont->get_pid_control(pid);
- std::cout << "[INFO]: PID control: (" << (int)pid.p << "," << (int)pid.i << "," << (int)pid.d << ")" << std::endl;
+ std::cout << "[INFO]: PID control capable" << std::endl;
+ pid.p=10;
+ pid.i=20;
+ pid.d=30;
+ cont->set_pid_control(pid);
+ cont->get_pid_control(pid);
+ std::cout << "[INFO]: PID control: (" << (int)pid.p << "," << (int)pid.i << "," << (int)pid.d << ")" << std::endl;
}
else
{
- std::cout << "[INFO]: Compliance control capable" << std::endl;
- cont->get_compliance_control(compliance);
- std::cout << "[INFO]: Compliance margin: (" << std::dec << (int)compliance.cw_compliance_margin << "," << (int)compliance.ccw_compliance_margin << ")" << std::endl;
- std::cout << "[INFO]: Compliance slope: (" << std::dec << (int)compliance.cw_compliance_slope << "," << (int)compliance.ccw_compliance_slope << ")" << std::endl;
+ std::cout << "[INFO]: Compliance control capable" << std::endl;
+ cont->get_compliance_control(compliance);
+ std::cout << "[INFO]: Compliance margin: (" << std::dec << (int)compliance.cw_compliance_margin << "," << (int)compliance.ccw_compliance_margin << ")" << std::endl;
+ std::cout << "[INFO]: Compliance slope: (" << std::dec << (int)compliance.cw_compliance_slope << "," << (int)compliance.ccw_compliance_slope << ")" << std::endl;
}
std::cout << "[INFO]: Maximum angle: " << info.max_angle << " degrees" << std::endl;
- std::cout << "[INFO]: Center angle: " << info.center_angle << " degrees" << std::endl;
+ std::cout << "[INFO]: Center angle: " << info.center_angle << " degrees" << std::endl;
std::cout << "[INFO]: Maximum speed: " << info.max_speed << " degress/s" << std::endl;
- std::cout << "[INFO]: Bus baudrate: " << info.baudrate << " bps" << std::endl;
+ std::cout << "[INFO]: Bus baudrate: " << info.baudrate << " bps" << std::endl;
std::cout << "[INFO]: Servo identifier: " << (int)info.id << std::endl;
cont->get_position_range(&min_angle,&max_angle);
std::cout << "[INFO]: Angle range: (" << min_angle << "," << max_angle << ")" << std::endl;
sd_alarms = cont->get_turn_off_alarms();
- std::cout << "[INFO]: Shutdown alarm flags: " << std::hex << (int)sd_alarms << std::endl;
+ std::cout << "[INFO]: Shutdown alarm flags: " << std::hex << (int)sd_alarms << std::endl;
std::cout << "[INFO]: - Input Voltage Error:\t" << bool(sd_alarms&0x01) << std::endl;
std::cout << "[INFO]: - Angle Limit Error:\t" << bool(sd_alarms&0x02) << std::endl;
std::cout << "[INFO]: - OverHeating Error:\t" << bool(sd_alarms&0x04) << std::endl;
std::cout << "[INFO]: - Range Error:\t\t" << bool(sd_alarms&0x08) << std::endl;
- std::cout << "[INFO]: - CheckSum Error:\t" << bool(sd_alarms&0x10) << std::endl;
- std::cout << "[INFO]: - Overload Error:\t" << bool(sd_alarms&0x20) << std::endl;
+ std::cout << "[INFO]: - CheckSum Error:\t" << bool(sd_alarms&0x10) << std::endl;
+ std::cout << "[INFO]: - Overload Error:\t" << bool(sd_alarms&0x20) << std::endl;
std::cout << "[INFO]: - Instruction Error:\t" << bool(sd_alarms&0x40) << std::endl;
led_alarms=cont->get_led_alarms();
std::cout << "[INFO]: LED alarm flags: " << std::hex << (int)led_alarms << std::endl;
+ std::cout << "[INFO]: max_torque: " << std::dec << cont->get_max_torque() << std::endl;
+ std::cout << "[INFO]: limit_torque: " << std::dec << cont->get_limit_torque() << std::endl;
std::cout << "[INFO]: Punch: " << std::dec << cont->get_punch() << std::endl;
cont->turn_led_on();
sleep(1);
cont->turn_led_off();
- // cont->move_absolute_angle(-90,300,0);
- // sleep(2);
- // cont->move_absolute_angle(90,100,0);
- // sleep(2);
- // cont->move_relative_angle(-5,100,0);
- // sleep(1);
- // cont->move_relative_angle(-5,100,0);
- // sleep(1);
- // cont->move_relative_angle(-5,100,0);
- // sleep(1);
- // cont->move_relative_angle(-5,100,0);
- // sleep(1);
- // cont->move_relative_angle(-5,100,0);
- // sleep(1);
- // cont->move_relative_angle(-5,100,0);
- // sleep(1);
- // cont->move_torque(50);
- // sleep(5);
- // cont->move_torque(-50);
- // sleep(5);
- cont->move_torque(100);
- sleep(5);
+ //*/
+
+
+ double desired_speed = 100.0; //chosen speed when moving angle
+ double max_angle_error = 0.5; //max angle error permitted
+ double time_interval = 0.1; //time in secs between checks
+ int max_time_sec = 2.0; //max time to wait until timeout
+
+ double desired_angle;
+ int t;
+ double uperiod = time_interval*1000000.0;
+ double timeout = max_time_sec/(uperiod/1000000.0);
+
+
+
+ /////////////////////////////////////////////////////////////////////////
+ /////MOVE RELATIVE ANGLE
+ /////////////////////////////////////////////////////////////////////////
+ ///*
+ std::cout << "-----------------------------------------------------------" << std::endl;
+ double relative_angle = -45;
+ double current_rel_angle;
+ std::cout << "MOVE RELATIVE ANGLE: "<< relative_angle << std::endl;
+
+ current_rel_angle = cont->get_current_angle();
+ desired_angle=current_rel_angle+relative_angle;
+ std::cout << "Desired angle: " << desired_angle << std::endl;
+ std::cout << "Current angle: " << current_rel_angle << std::endl;
+
+ cont->move_relative_angle(relative_angle,desired_speed);
+ std::cout << "Moving..." << std::endl;
+
+ t=0;
+ while(fabs(current_rel_angle-desired_angle)>max_angle_error && t<timeout)
+ {
+ current_rel_angle = cont->get_current_angle();
+ usleep(uperiod);
+ t++;
+ }
+
+ if(t==timeout)
+ std::cout << "Reached " << max_time_sec << "sec timeout"<< std::endl;
+
+ std::cout << "Desired angle: " << desired_angle << std::endl;
+ std::cout << "Reached angle: " << current_rel_angle << std::endl;
+ std::cout << "Error angle: " << current_rel_angle-desired_angle << std::endl;
+ std::cout << "Done" << std::endl;
+ sleep(1);
+ //*/
+
+ /////////////////////////////////////////////////////////////////////////
+ ////MOVE ABSOLUTE ANGLE
+ /////////////////////////////////////////////////////////////////////////
+ ///*
+ std::cout << "-----------------------------------------------------------" << std::endl;
+ double absolute_angle=0.0;
+ double current_abs_angle;
+ std::cout << "MOVE ABSOLUTE ANGLE: " << absolute_angle << std::endl;
+
+ current_abs_angle = cont->get_current_angle();
+ desired_angle=absolute_angle;
+ std::cout << "Desired angle: " << desired_angle << std::endl;
+ std::cout << "Current angle: " << current_abs_angle << std::endl;
+
+ cont->move_absolute_angle(absolute_angle, desired_speed);
+ std::cout << "Moving..." << std::endl;
+
+ t=0;
+ while(fabs(current_abs_angle)>max_angle_error && t<timeout)
+ {
+ current_abs_angle = cont->get_current_angle();
+ usleep(uperiod);
+ t++;
+ }
+
+ if(t==timeout)
+ std::cout << "Reached " << max_time_sec << "sec timeout"<< std::endl;
+
+ std::cout << "Desired angle: " << desired_angle << std::endl;
+ std::cout << "Reached angle: " << current_abs_angle << std::endl;
+ std::cout << "Error angle: " << current_abs_angle-desired_angle << std::endl;
+ std::cout << "Done" << std::endl;
+ sleep(1);
+ //*/
+
+ /////////////////////////////////////////////////////////////////////////
+ ////MOVE TORQUE
+ /////////////////////////////////////////////////////////////////////////
+ ///*
+ std::cout << "-----------------------------------------------------------" << std::endl;
+ double max_effort;
+ if(argc==2)
+ max_effort = atoi(argv[1]);
+ else
+ max_effort = 50;
+
+ double desired_effort=max_effort;
+ std::cout << "MOVE EFFORT: " << desired_effort << std::endl;
+
+ double current_effort = cont->get_current_effort();
+ std::cout << "Desired effort: " << desired_effort << std::endl;
+ std::cout << "Current effort: " << current_effort << std::endl;
+
+ cont->move_torque(desired_effort);
+ std::cout << "Moving..." << std::endl;
+
+ t=0;
+ while(t<timeout)
+ {
+ current_effort = cont->get_current_effort();
+ //std::cout << "Current effort: " << current_effort << std::endl;
+ usleep(uperiod);
+ t++;
+ }
cont->move_torque(0);
- for (size_t aa = 0; aa < 100; ++aa)
+
+ std::cout << "Done" << std::endl;
+ sleep(1);
+ std::cout << "----------------------------" << std::endl;
+
+ desired_effort=-1.0*max_effort;
+ std::cout << "MOVE EFFORT: " << desired_effort << std::endl;
+
+ current_effort = cont->get_current_effort();
+ std::cout << "Desired effort: " << desired_effort << std::endl;
+ std::cout << "Current effort: " << current_effort << std::endl;
+
+ cont->move_torque(desired_effort);
+ std::cout << "Moving..." << std::endl;
+
+ t=0;
+ while(t<timeout)
{
- try
- {
- cont->move_absolute_angle(125.0, 250);
- sleep(2);
- cont->move_absolute_angle(-125.0, 250);
- sleep(2);
- }catch(CDynamixelAlarmException &e){
- std::cout << "[Alarm exception]: " << e.what() << std::endl;
- }
+ current_effort = cont->get_current_effort();
+ //std::cout << "Current effort: " << current_effort << std::endl;
+ usleep(uperiod);
+ t++;
}
+ cont->move_torque(0);
+
+ std::cout << "Done" << std::endl;
+ std::cout << "-----------------------------------------------------------" << std::endl;
+ sleep(1);
+ //*/
+
}
- }catch(CException &e){
- std::cout << "[Genereal exception]: " << e.what() << std::endl; }
+ }
+ catch(CException &e)
+ {
+ std::cout << "[Genereal exception]: " << e.what() << std::endl;
+ }
+ if(cont!=NULL)
+ delete cont;
return 0;
}