dynamixel_motor_cont:

 - modified example test_dynamixel_motor, now showing relative angle, absolute angle and effort movements
 - get_current_effort takes direction from speeds (current load data does not seem to have signum)

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();

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 */

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,

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; 
 }