Skip to content
Snippets Groups Projects
Commit 702c672f authored by Fernando Herrero's avatar Fernando Herrero
Browse files

dynamixel_motor_cont: updated test_dynamixel_motor example

parent 4fb236de
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/examples/test_dynamixel_motor.cpp
+ 50
− 20
View file @ 702c672f
......@@ -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;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment