diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index 11a716745759900ca37a62aef13ddbc037a3a0c0..533af63ce4189d29abb467978b3050c0cc03aeb0 100755
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -4,15 +4,16 @@
#include "dynamixel_motor.h"
#include <iostream>
#include <math.h>
+#include <time.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
+ std::string serial="A600cVjj"; //extracted from dynamixel library's test_dynamixel_server_no_scan or with 'dmesg' command: "SerialNumber: A600cVjj"
+ int baudrate = 57600; //57600 or 1000000
+ int device = 1; //extracted from dynamixel library's test_dynamixel_server_no_scan
CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CDynamixelMotor *cont = NULL;
@@ -33,7 +34,7 @@ int main(int argc, char *argv[])
/////////////////////////////////////////////////////////////////////////
////INFO
/////////////////////////////////////////////////////////////////////////
- ///*
+ /*
std::cout << "-----------------------------------------------------------" << std::endl;
cont->get_servo_info(info);
std::cout << "[INFO]: Servo model: " << info.model << std::endl;
@@ -87,7 +88,7 @@ int main(int argc, char *argv[])
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
+ int max_time_sec = 10.0; //max time to wait until timeout
double desired_angle;
int t;
@@ -99,7 +100,7 @@ int main(int argc, char *argv[])
/////////////////////////////////////////////////////////////////////////
/////MOVE RELATIVE ANGLE
/////////////////////////////////////////////////////////////////////////
- ///*
+ /*
std::cout << "-----------------------------------------------------------" << std::endl;
double relative_angle = -45;
double current_rel_angle;
@@ -134,7 +135,7 @@ int main(int argc, char *argv[])
/////////////////////////////////////////////////////////////////////////
////MOVE ABSOLUTE ANGLE
/////////////////////////////////////////////////////////////////////////
- ///*
+ /*
std::cout << "-----------------------------------------------------------" << std::endl;
double absolute_angle=0.0;
double current_abs_angle;
@@ -169,7 +170,7 @@ int main(int argc, char *argv[])
/////////////////////////////////////////////////////////////////////////
////MOVE TORQUE
/////////////////////////////////////////////////////////////////////////
- ///*
+ /*
std::cout << "-----------------------------------------------------------" << std::endl;
double max_effort;
if(argc==2)
@@ -180,18 +181,13 @@ int main(int argc, char *argv[])
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;
+ std::cout << "Current speed: " << cont->get_current_speed() << std::endl;
usleep(uperiod);
t++;
}
@@ -204,18 +200,13 @@ int main(int argc, char *argv[])
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)
{
- current_effort = cont->get_current_effort();
- //std::cout << "Current effort: " << current_effort << std::endl;
+ std::cout << "Current speed: " << cont->get_current_speed() << std::endl;
usleep(uperiod);
t++;
}
@@ -226,13 +217,52 @@ int main(int argc, char *argv[])
sleep(1);
//*/
+
+ /////////////////////////////////////////////////////////////////////////
+ ////MOVE RANDOM TORQUES AND OUTPUT SPEEDS
+ /////////////////////////////////////////////////////////////////////////
+ ///*
+ std::cout << "MOVE RANDOM EFFORTS" << std::endl;
+ int min=-100;
+ int max=100;
+ int output = min + (rand() % (int)(max - min + 1));
+ cont->move_torque(output);
+ std::cout << "Effort: " << output << std::endl;
+
+ double tt=0;
+ double time=10;
+ double step_time=0.5;
+ while(tt<time)
+ {
+ //std::cout << "tt: " << tt << std::endl;
+ double a1 = cont->get_current_angle();
+ usleep(step_time*1000000);
+ double a2 = cont->get_current_angle();
+ double speed = (a2-a1)/step_time;
+ std::cout << "- Speed from angle diff: " << speed << std::endl;
+ std::cout << "- Current speed: " << cont->get_current_speed() << std::endl;
+ int output = min + (rand() % (int)(max - min + 1));
+ cont->move_torque(output);
+ std::cout << "Effort: " << output << std::endl;
+ tt+=step_time;
+ }
+ 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;
}
+
if(cont!=NULL)
delete cont;
+
return 0;
}