Skip to content
Snippets Groups Projects
Commit 9953b35a authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merge branch 'new_servos_support'

parents 7ebe214e 48e0dd1e
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 1685 additions and 1826 deletions
src/dynamixel_motor.cpp
+ 629
− 804
View file @ 9953b35a
This diff is collapsed.
src/dynamixel_motor.h
+ 95
− 19
View file @ 9953b35a
...@@ -2,6 +2,7 @@ ...@@ -2,6 +2,7 @@
#define _DYNAMIXEL_MOTOR_H #define _DYNAMIXEL_MOTOR_H
#include "dynamixelserver.h" #include "dynamixelserver.h"
#include "dynamixel_registers.h"
#include "threadserver.h" #include "threadserver.h"
#include "eventserver.h" #include "eventserver.h"
#include "ftdimodule.h" #include "ftdimodule.h"
...@@ -11,6 +12,7 @@ ...@@ -11,6 +12,7 @@
#include <string> #include <string>
#include <vector> #include <vector>
#include <memory> #include <memory>
#include <limits>
typedef enum typedef enum
{ {
...@@ -30,11 +32,14 @@ typedef struct ...@@ -30,11 +32,14 @@ typedef struct
unsigned int gear_ratio; unsigned int gear_ratio;
unsigned int encoder_resolution; unsigned int encoder_resolution;
bool pid_control; bool pid_control;
bool multi_turn;
double max_angle; double max_angle;
double center_angle; double center_angle;
double max_speed; double max_speed;
double max_current;
unsigned int baudrate; unsigned int baudrate;
unsigned char id; unsigned char id;
bool ext_memory_map;
}TDynamixel_info; }TDynamixel_info;
typedef struct typedef struct
...@@ -47,9 +52,9 @@ typedef struct ...@@ -47,9 +52,9 @@ typedef struct
typedef struct typedef struct
{ {
unsigned char p; unsigned short int p;
unsigned char i; unsigned short int i;
unsigned char d; unsigned short int d;
}TDynamixel_pid; }TDynamixel_pid;
typedef struct typedef struct
...@@ -59,11 +64,12 @@ typedef struct ...@@ -59,11 +64,12 @@ typedef struct
double max_temperature; double max_temperature;
double max_voltage; double max_voltage;
double min_voltage; double min_voltage;
double max_torque; double max_pwm;
double max_current;
unsigned short int punch; unsigned short int punch;
}TDynamixel_config; }TDynamixel_config;
typedef enum{angle_ctrl=2,torque_ctrl=1} control_mode; typedef enum{current_ctrl=0,velocity_ctrl=1,position_ctrl=3,ext_position_ctrl=4,current_pos_ctrl=5,pwm_ctrl=6} control_mode;
/** /**
* \brief * \brief
...@@ -112,7 +118,12 @@ class CDynamixelMotor ...@@ -112,7 +118,12 @@ class CDynamixelMotor
* \brief * \brief
* *
*/ */
unsigned short int *registers; TDynReg *registers;
/**
* \brief
*
*/
bool enabled;
protected: protected:
/** /**
* \brief * \brief
...@@ -128,12 +139,12 @@ class CDynamixelMotor ...@@ -128,12 +139,12 @@ class CDynamixelMotor
* \brief * \brief
* *
*/ */
double to_angles(unsigned short int counts); double to_angles(unsigned int counts);
/** /**
* \brief * \brief
* *
*/ */
double to_speeds(unsigned short int counts); double to_speeds(unsigned int counts);
/** /**
* \brief * \brief
* *
...@@ -149,6 +160,16 @@ class CDynamixelMotor ...@@ -149,6 +160,16 @@ class CDynamixelMotor
* *
*/ */
void set_control_mode(control_mode mode); void set_control_mode(control_mode mode);
/**
* \brief
*
*/
void read_register(TDynReg reg, unsigned int &value);
/**
* \brief
*
*/
void write_register(TDynReg reg, unsigned int value);
public: public:
/** /**
* \brief * \brief
...@@ -237,22 +258,42 @@ class CDynamixelMotor ...@@ -237,22 +258,42 @@ class CDynamixelMotor
* \brief * \brief
* *
*/ */
double get_max_torque(void); double get_max_pwm(void);
/** /**
* \brief * \brief
* *
*/ */
double get_limit_torque(void); double get_pwm_limit(void);
/** /**
* \brief * \brief
* *
*/ */
void set_max_torque(double torque_ratio); void set_max_pwm(double torque_ratio);
/** /**
* \brief * \brief
* *
*/ */
void set_limit_torque(double torque_ratio); void set_pwm_limit(double torque_ratio);
/**
* \brief
*
*/
double get_current_limit(void);
/**
* \brief
*
*/
void set_current_limit(double current);
/**
* \brief
*
*/
double get_speed_limit(void);
/**
* \brief
*
*/
void set_speed_limit(double speed);
/** /**
* \brief * \brief
* *
...@@ -273,6 +314,26 @@ class CDynamixelMotor ...@@ -273,6 +314,26 @@ class CDynamixelMotor
* *
*/ */
void set_pid_control(TDynamixel_pid &config); void set_pid_control(TDynamixel_pid &config);
/**
* \brief
*
*/
void get_vel_pi_control(TDynamixel_pid &config);
/**
* \brief
*
*/
void set_vel_pi_control(TDynamixel_pid &config);
/**
* \brief
*
*/
void get_feedfwd_control(double &acc_gain,double &vel_gain);
/**
* \brief
*
*/
void set_feedfwd_control(double acc_gain,double vel_gain);
/* control functions */ /* control functions */
/** /**
* \brief * \brief
...@@ -308,20 +369,30 @@ class CDynamixelMotor ...@@ -308,20 +369,30 @@ class CDynamixelMotor
* \brief * \brief
* *
*/ */
void move_absolute_angle(double angle,double speed); void move_absolute_angle(double angle,double speed,double current=std::numeric_limits<double>::infinity());
/**
* \brief
*
*/
void move_relative_angle(double angle,double speed,double current=std::numeric_limits<double>::infinity());
/**
* \brief
*
*/
void move_speed(double speed);
/** /**
* \brief * \brief
* *
*/ */
void move_relative_angle(double angle,double speed); void move_pwm(double pwm_ratio);
/** /**
* \brief * \brief
* *
*/ */
void move_torque(double torque_ratio); void move_current(double current);
/** /**
* \brief * \brief
* *
*/ */
void stop(void); void stop(void);
/** /**
...@@ -338,7 +409,12 @@ class CDynamixelMotor ...@@ -338,7 +409,12 @@ class CDynamixelMotor
* \brief * \brief
* *
*/ */
double get_current_effort(void); double get_current_pwm(void);
/**
* \brief
*
*/
double get_current_current(void);
/** /**
* \brief * \brief
* *
...@@ -353,12 +429,12 @@ class CDynamixelMotor ...@@ -353,12 +429,12 @@ class CDynamixelMotor
* \brief * \brief
* *
*/ */
unsigned short int get_punch(void); unsigned int get_punch(void);
/** /**
* \brief * \brief
* *
*/ */
void set_punch(unsigned short int punch_value); void set_punch(unsigned int punch_value);
/** /**
* \brief * \brief
* *
......
src/dynamixel_motor_exceptions.cpp
+ 8
− 0
View file @ 9953b35a
...@@ -5,6 +5,7 @@ ...@@ -5,6 +5,7 @@
const std::string dynamixel_motor_error_message="DynamixelMotor error - "; const std::string dynamixel_motor_error_message="DynamixelMotor error - ";
const std::string dynamixel_motor_group_error_message="DynamixelMotorGroup error - "; const std::string dynamixel_motor_group_error_message="DynamixelMotorGroup error - ";
const std::string dynamixel_motor_unsupported_feature_error_message="DynamixelMotorUnsupportedFeature error - ";
const std::string dynamixel_motion_sequence_error_message="DynamixelMotionSeq error - "; const std::string dynamixel_motion_sequence_error_message="DynamixelMotionSeq error - ";
CDynamixelMotorException::CDynamixelMotorException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motor_error_message) CDynamixelMotorException::CDynamixelMotorException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motor_error_message)
...@@ -21,6 +22,13 @@ CDynamixelMotorGroupException::CDynamixelMotorGroupException(const std::string& ...@@ -21,6 +22,13 @@ CDynamixelMotorGroupException::CDynamixelMotorGroupException(const std::string&
this->error_msg+=error_msg; this->error_msg+=error_msg;
} }
CDynamixelMotorUnsupportedFeatureException::CDynamixelMotorUnsupportedFeatureException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motor_unsupported_feature_error_message)
{
std::stringstream text;
this->error_msg+=error_msg;
}
CDynamixelMotionSequenceException::CDynamixelMotionSequenceException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motion_sequence_error_message) CDynamixelMotionSequenceException::CDynamixelMotionSequenceException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motion_sequence_error_message)
{ {
std::stringstream text; std::stringstream text;
......
src/dynamixel_motor_exceptions.h
+ 13
− 0
View file @ 9953b35a
...@@ -33,6 +33,19 @@ class CDynamixelMotorGroupException : public CException ...@@ -33,6 +33,19 @@ class CDynamixelMotorGroupException : public CException
CDynamixelMotorGroupException(const std::string& where,const std::string &error_msg); CDynamixelMotorGroupException(const std::string& where,const std::string &error_msg);
}; };
/**
* \brief
*/
class CDynamixelMotorUnsupportedFeatureException : public CException
{
public:
/**
* \brief
*
*/
CDynamixelMotorUnsupportedFeatureException(const std::string& where,const std::string &error_msg);
};
/** /**
* \brief * \brief
*/ */
......
src/dynamixel_motor_group.cpp
+ 403
− 723
View file @ 9953b35a
This diff is collapsed.
src/dynamixel_motor_group.h
+ 15
− 5
View file @ 9953b35a
...@@ -66,7 +66,7 @@ class CDynamixelMotorGroup ...@@ -66,7 +66,7 @@ class CDynamixelMotorGroup
* \brief * \brief
* *
*/ */
std::vector<unsigned short int *>registers; std::vector<TDynReg *>registers;
protected: protected:
/** /**
* \brief * \brief
...@@ -142,7 +142,7 @@ class CDynamixelMotorGroup ...@@ -142,7 +142,7 @@ class CDynamixelMotorGroup
* \brief * \brief
* *
*/ */
void set_max_torque(unsigned int index,double torque_ratio); void set_max_pwm(unsigned int index,double torque_ratio);
/** /**
* \brief * \brief
* *
...@@ -152,7 +152,7 @@ class CDynamixelMotorGroup ...@@ -152,7 +152,7 @@ class CDynamixelMotorGroup
* \brief * \brief
* *
*/ */
double get_max_torque(unsigned int index); double get_max_pwm(unsigned int index);
/** /**
* \brief * \brief
* *
...@@ -178,6 +178,16 @@ class CDynamixelMotorGroup ...@@ -178,6 +178,16 @@ class CDynamixelMotorGroup
* *
*/ */
void set_control_mode(control_mode mode); void set_control_mode(control_mode mode);
/**
* \brief
*
*/
void read_register(unsigned int index,TDynReg reg, unsigned int &value);
/**
* \brief
*
*/
void write_register(unsigned int index,TDynReg reg, unsigned int value);
public: public:
/** /**
* \brief * \brief
...@@ -286,12 +296,12 @@ class CDynamixelMotorGroup ...@@ -286,12 +296,12 @@ class CDynamixelMotorGroup
* \brief * \brief
* *
*/ */
void get_punch(std::vector<unsigned short int> &punches); void get_punch(std::vector<unsigned int> &punches);
/** /**
* \brief * \brief
* *
*/ */
void set_punch(std::vector<unsigned short int> &punches); void set_punch(std::vector<unsigned int> &punches);
/** /**
* \brief * \brief
* *
......
src/dynamixel_registers.cpp
+ 342
− 117
View file @ 9953b35a
This diff is collapsed.
src/dynamixel_registers.h
+ 63
− 109
View file @ 9953b35a
#ifndef _DYNAMIXEL_REGISTERS_H #ifndef _DYNAMIXEL_REGISTERS_H
#define _DYNAMIXEL_REGISTERS_H #define _DYNAMIXEL_REGISTERS_H
extern unsigned short int std_compl_reg[39]; #define NUM_REG 62
extern unsigned short int std_pid_reg[39];
extern unsigned short int xl_reg[39]; typedef struct{
unsigned short int address;
unsigned char size;
bool eeprom;
}TDynReg;
extern TDynReg ax_reg[NUM_REG];
extern TDynReg mx_1_0_reg[NUM_REG];
extern TDynReg mx_106_1_0_reg[NUM_REG];
extern TDynReg xl_reg[NUM_REG];
extern TDynReg xm_reg[NUM_REG];
typedef enum { typedef enum {
// [Access] [Description] // [Access] [Description]
// EEPROM Area // Info
model_number=0, // (R) Model Number model_number=0, // (R) Model Number
model_info, // (R)
firmware_version, // (R) Version of the Firmware firmware_version, // (R) Version of the Firmware
id, // (RW) ID of Dynamixel id, // (RW) ID of Dynamixel
baudrate, // (RW) Baud Rate of Dynamixel baudrate, // (RW) Baud Rate of Dynamixel
return_delay_time, // (RW) Return Delay Time return_delay_time, // (RW) Return Delay Time
cw_angle_limit, // (RW) Clockwise Angle Limit second_id, // (RW)
ccw_angle_limit, // (RW) Counterclockwise Angle Limit protocol, // (RW)
dyn_control_mode, // (RW) Joint or wheel mode
temp_limit, // (RW) Internal Temperature Limit
low_voltage_limit, // (RW) Lowest Voltage Limit
high_voltage_limit, // (RW) Highest Voltage Limit
max_torque, // (RW) Maximum Torque
return_level, // (RW) Status Return Level return_level, // (RW) Status Return Level
// Configuration
drive_mode, // (RW) Drive mode
op_mode, // (RW) Joint or wheel mode
homming_offset, // (RW)
moving_threshold, // (RW)
min_angle_limit, // (RW) Clockwise Angle Limit
max_angle_limit, // (RW) Counterclockwise Angle Limit
temp_limit, // (RW) Internal Temperature Limit
min_voltage_limit, // (RW) Lowest Voltage Limit
max_voltage_limit, // (RW) Highest Voltage Limit
pwm_limit, // (RW) Maximum Torque
max_pwm, // (RW) Maximum Torque
current_limit, // (RW)
max_current, // (RW)
velocity_limit, // (RW)
multi_turn_off, // (RW) Multi turn offset position
resolution_div, // (RW) Resolution divider
// Status
alarm_led, // (RW) LED for Alarm alarm_led, // (RW) LED for Alarm
alarm_shtdwn, // (RW) Shutdown for Alarm alarm_shtdwn, // (RW) Shutdown for Alarm
down_cal, // (RW)
up_cal, // (RW)
// RAM Area
torque_en, // (RW) Torque On/Off
led, // (RW) LED On/Off led, // (RW) LED On/Off
pid_p, // (RW) bus_watchdog, // (RW)
pid_i, // (RW) moving, // (R) Means if there is any movement
pid_d, // (RW) moving_status, // (R)
lock, // (RW) Locking EEPROM
reg_inst, // (RW) Means if Instruction is registered
hardware_error, // (R) Means if there is any movement
// control
torque_en, // (RW) Torque On/Off
vel_pid_i, // (RW)
vel_pid_p, // (RW)
pos_pid_p, // (RW)
pos_pid_i, // (RW)
pos_pid_d, // (RW)
feedfwd_gain_2, // (RW)
feedfwd_gain_1, // (RW)
cw_comp_margin, // (RW) CW Compliance Margin cw_comp_margin, // (RW) CW Compliance Margin
ccw_comp_margin, // (RW) CCW Compliance Margin ccw_comp_margin, // (RW) CCW Compliance Margin
cw_comp_slope, // (RW) CW Compliance Slope cw_comp_slope, // (RW) CW Compliance Slope
ccw_comp_slope, // (RW) CCW Compliance Slope ccw_comp_slope, // (RW) CCW Compliance Slope
punch, // (RW) Punch
// Set point
goal_pos, // (RW) Goal Position goal_pos, // (RW) Goal Position
goal_speed, // (RW) Moving Speed goal_speed, // (RW) Moving Speed
torque_limit, // (RW) Torque Limit goal_pwm, // (RW)
goal_current, // (RW)
profile_accel, // (RW)
profile_vel, // (RW)
// Feedback
current_pos, // (R) Current Position current_pos, // (R) Current Position
current_speed, // (R) Current Speed current_speed, // (R) Current Speed
current_load, // (R) Current Load current_load, // (R) Current Load
current_voltage, // (R) Current Voltage current_voltage, // (R) Current Voltage
current_temp, // (RW) Current Temperature current_temp, // (R) Current Temperature
reg_inst, // (RW) Means if Instruction is registered current_pwm, // (R)
moving, // (R) Means if there is any movement realtime_tick, // (R) Realtime tick
lock, // (RW) Locking EEPROM velocity_traj, // (R)
hardware_error, // (R) Means if there is any movement pos_traj // (R)
punch // (RW) Punch
} reg_id; } reg_id;
typedef enum {
// [Access] [Description]
// EEPROM Area
std_model_number=0x00, // (R) Model Number
std_firmware_version=0x02, // (R) Version of the Firmware
std_id=0x03, // (RW) ID of Dynamixel
std_baudrate=0x04, // (RW) Baud Rate of Dynamixel
std_return_delay_time=0x05, // (RW) Return Delay Time
std_cw_angle_limit=0x06, // (RW) Clockwise Angle Limit
std_ccw_angle_limit=0x08, // (RW) Counterclockwise Angle Limit
std_temp_limit=0x0B, // (RW) Internal Temperature Limit
std_low_voltage_limit=0x0C, // (RW) Lowest Voltage Limit
std_high_voltage_limit=0x0D, // (RW) Highest Voltage Limit
std_max_torque=0x0E, // (RW) Maximum Torque
std_return_level=0x10, // (RW) Status Return Level
std_alarm_led=0x11, // (RW) LED for Alarm
std_alarm_shtdwn=0x12, // (RW) Shutdown for Alarm
std_down_cal=0x14, // (RW)
std_up_cal=0x16, // (RW)
// RAM Area
std_torque_en=0x18, // (RW) Torque On/Off
std_led=0x19, // (RW) LED On/Off
std_pid_p=0x1A, // (RW)
std_pid_i=0x1B, // (RW)
std_pid_d=0x1C, // (RW)
std_cw_comp_margin=0x1A, // (RW) CW Compliance Margin
std_ccw_comp_margin=0x1B, // (RW) CCW Compliance Margin
std_cw_comp_slope=0x1C, // (RW) CW Compliance Slope
std_ccw_comp_slope=0x1D, // (RW) CCW Compliance Slope
std_goal_pos=0x1E, // (RW) Goal Position
std_goal_speed=0x20, // (RW) Moving Speed
std_torque_limit=0x22, // (RW) Torque Limit
std_current_pos=0x24, // (R) Current Position
std_current_speed=0x26, // (R) Current Speed
std_current_load=0x28, // (R) Current Load
std_current_voltage=0x2A, // (R) Current Voltage
std_current_temp=0x2B, // (RW) Current Temperature
std_reg_inst=0x2C, // (RW) Means if Instruction is registered
std_moving=0x2E, // (R) Means if there is any movement
std_lock=0x2F, // (RW) Locking EEPROM
std_punch=0x30 // (RW) Punch
} std_reg_id;
typedef enum {
// [Access] [Description]
// EEPROM Area
xl_model_number=0x00, // (R) Model Number
xl_firmware_version=0x02, // (R) Version of the Firmware
xl_id=0x03, // (RW) ID of Dynamixel
xl_baudrate=0x04, // (RW) Baud Rate of Dynamixel
xl_return_delay_time=0x05, // (RW) Return Delay Time
xl_cw_angle_limit=0x06, // (RW) Clockwise Angle Limit
xl_ccw_angle_limit=0x08, // (RW) Counterclockwise Angle Limit
xl_control_mode=0x0B, // (RW) Joint or wheel mode
xl_temp_limit=0x0C, // (RW) Internal Temperature Limit
xl_low_voltage_limit=0x0D, // (RW) Lowest Voltage Limit
xl_high_voltage_limit=0x0E, // (RW) Highest Voltage Limit
xl_max_torque=0x0F, // (RW) Maximum Torque
xl_return_level=0x11, // (RW) Status Return Level
xl_alarm_shtdwn=0x12, // (RW) Shutdown for Alarm
xl_down_cal=0x14, // (RW)
xl_up_cal=0x16, // (RW)
// RAM Area
xl_torque_en=0x18, // (RW) Torque On/Off
xl_led=0x19, // (RW) LED On/Off
xl_pid_d=0x1B, // (RW)
xl_pid_i=0x1C, // (RW)
xl_pid_p=0x1D, // (RW)
xl_goal_pos=0x1E, // (RW) Goal Position
xl_goal_speed=0x20, // (RW) Moving Speed
xl_goal_torque=0x23, // (RW) Torque Limit
xl_current_pos=0x25, // (R) Current Position
xl_current_speed=0x27, // (R) Current Speed
xl_current_load=0x29, // (R) Current Load
xl_current_voltage=0x2D, // (R) Current Voltage
xl_current_temp=0x2E, // (RW) Current Temperature
xl_reg_inst=0x2F, // (RW) Means if Instruction is registered
xl_moving=0x31, // (R) Means if there is any movement
xl_hardware_error=0x32, // (R) Means if there is any movement
xl_punch=0x33 // (RW) Punch
} xl_reg_id;
#endif #endif
src/examples/test_dynamixel_motor.cpp
+ 116
− 48
View file @ 9953b35a
...@@ -11,9 +11,9 @@ std::string config_file="../src/xml/dyn_servo_config.xml"; ...@@ -11,9 +11,9 @@ std::string config_file="../src/xml/dyn_servo_config.xml";
int main(int argc, char *argv[]) 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" std::string serial="FT3M9M97"; //extracted from dynamixel library's test_dynamixel_server_no_scan or with 'dmesg' command: "SerialNumber: A600cVjj"
int baudrate = 57600; //57600 or 1000000 int baudrate = 57600; //57600 or 1000000
int device = 1; //extracted from dynamixel library's test_dynamixel_server_no_scan int device = 2; //extracted from dynamixel library's test_dynamixel_server_no_scan
CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance(); CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CDynamixelMotor *cont = NULL; CDynamixelMotor *cont = NULL;
...@@ -29,12 +29,12 @@ int main(int argc, char *argv[]) ...@@ -29,12 +29,12 @@ int main(int argc, char *argv[])
if(dyn_server->get_num_buses()>0) if(dyn_server->get_num_buses()>0)
{ {
dyn_server->config_bus(serial,baudrate); dyn_server->config_bus(serial,baudrate);
cont = new CDynamixelMotor(cont_name, dyn_server, device); cont = new CDynamixelMotor(cont_name, dyn_server, device,dyn_version2);
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
////INFO ////INFO
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
/* /*
std::cout << "-----------------------------------------------------------" << std::endl; std::cout << "-----------------------------------------------------------" << std::endl;
cont->get_servo_info(info); cont->get_servo_info(info);
std::cout << "[INFO]: Servo model: " << info.model << std::endl; std::cout << "[INFO]: Servo model: " << info.model << std::endl;
...@@ -44,10 +44,6 @@ int main(int argc, char *argv[]) ...@@ -44,10 +44,6 @@ int main(int argc, char *argv[])
if(info.pid_control) if(info.pid_control)
{ {
std::cout << "[INFO]: PID control capable" << 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); 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: (" << (int)pid.p << "," << (int)pid.i << "," << (int)pid.d << ")" << std::endl;
} }
...@@ -76,15 +72,15 @@ int main(int argc, char *argv[]) ...@@ -76,15 +72,15 @@ int main(int argc, char *argv[])
std::cout << "[INFO]: - Instruction Error:\t" << bool(sd_alarms&0x40) << std::endl; std::cout << "[INFO]: - Instruction Error:\t" << bool(sd_alarms&0x40) << std::endl;
led_alarms=cont->get_led_alarms(); led_alarms=cont->get_led_alarms();
std::cout << "[INFO]: LED alarm flags: " << std::hex << (int)led_alarms << std::endl; 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]: max_pwm: " << std::dec << cont->get_max_pwm() << std::endl;
std::cout << "[INFO]: limit_torque: " << std::dec << cont->get_limit_torque() << std::endl; std::cout << "[INFO]: pwm_limit: " << std::dec << cont->get_pwm_limit() << std::endl;
std::cout << "[INFO]: Punch: " << std::dec << cont->get_punch() << std::endl; //std::cout << "[INFO]: Punch: " << std::dec << cont->get_punch() << std::endl;
cont->turn_led_on(); cont->turn_led_on();
sleep(1); sleep(1);
cont->turn_led_off(); cont->turn_led_off();
//*/ */
/*
double desired_speed = 100.0; //chosen speed when moving angle double desired_speed = 100.0; //chosen speed when moving angle
double max_angle_error = 0.5; //max angle error permitted double max_angle_error = 0.5; //max angle error permitted
double time_interval = 0.1; //time in secs between checks double time_interval = 0.1; //time in secs between checks
...@@ -94,13 +90,13 @@ int main(int argc, char *argv[]) ...@@ -94,13 +90,13 @@ int main(int argc, char *argv[])
int t; int t;
double uperiod = time_interval*1000000.0; double uperiod = time_interval*1000000.0;
double timeout = max_time_sec/(uperiod/1000000.0); double timeout = max_time_sec/(uperiod/1000000.0);
*/
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
/////MOVE RELATIVE ANGLE /////MOVE RELATIVE ANGLE
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
/* /*
std::cout << "-----------------------------------------------------------" << std::endl; std::cout << "-----------------------------------------------------------" << std::endl;
double relative_angle = -45; double relative_angle = -45;
double current_rel_angle; double current_rel_angle;
...@@ -130,29 +126,35 @@ int main(int argc, char *argv[]) ...@@ -130,29 +126,35 @@ int main(int argc, char *argv[])
std::cout << "Error angle: " << current_rel_angle-desired_angle << std::endl; std::cout << "Error angle: " << current_rel_angle-desired_angle << std::endl;
std::cout << "Done" << std::endl; std::cout << "Done" << std::endl;
sleep(1); sleep(1);
//*/ */
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
////MOVE ABSOLUTE ANGLE ////MOVE ABSOLUTE ANGLE
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
/*
double time_interval = 0.1; //time in secs between checks
int max_time_sec = 10.0; //max time to wait until timeout
int t;
double uperiod = time_interval*1000000.0;
double timeout = max_time_sec/(uperiod/1000000.0);
double desired_angle,absolute_angle=0.0,desired_speed=1.0,current_angle,max_angle_error=0.5,desired_current=0.1;
std::cout << "-----------------------------------------------------------" << std::endl; 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(); cont->enable();
desired_angle=absolute_angle; desired_angle=absolute_angle;
std::cout << "Desired angle: " << desired_angle << std::endl; std::cout << "Desired angle: " << desired_angle << std::endl;
std::cout << "Current angle: " << current_abs_angle << std::endl; current_angle=cont->get_current_angle();
std::cout << "Current angle: " << current_angle << std::endl;
cont->move_absolute_angle(absolute_angle, desired_speed); cont->move_absolute_angle(absolute_angle, desired_speed,desired_current);
std::cout << "Moving..." << std::endl; std::cout << "Moving..." << std::endl;
t=0; t=0;
while(fabs(current_abs_angle)>max_angle_error && t<timeout) while(fabs(current_angle)>max_angle_error && t<timeout)
{ {
current_abs_angle = cont->get_current_angle(); current_angle=cont->get_current_angle();
std::cout << current_angle << std::endl;
usleep(uperiod); usleep(uperiod);
t++; t++;
} }
...@@ -161,67 +163,134 @@ int main(int argc, char *argv[]) ...@@ -161,67 +163,134 @@ int main(int argc, char *argv[])
std::cout << "Reached " << max_time_sec << "sec timeout"<< std::endl; std::cout << "Reached " << max_time_sec << "sec timeout"<< std::endl;
std::cout << "Desired angle: " << desired_angle << std::endl; std::cout << "Desired angle: " << desired_angle << std::endl;
std::cout << "Reached angle: " << current_abs_angle << std::endl; std::cout << "Reached angle: " << current_angle << std::endl;
std::cout << "Error angle: " << current_abs_angle-desired_angle << std::endl; std::cout << "Error angle: " << current_angle-desired_angle << std::endl;
std::cout << "Done" << std::endl; std::cout << "Done" << std::endl;
sleep(1); sleep(1);
//*/ cont->disable();
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
////MOVE TORQUE ////MOVE PWM
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
/* /*
double time_interval = 0.1; //time in secs between checks
int max_time_sec = 10.0; //max time to wait until timeout
int t;
double uperiod = time_interval*1000000.0;
double timeout = max_time_sec/(uperiod/1000000.0);
double max_pwm,desired_pwm;
cont->enable();
std::cout << "-----------------------------------------------------------" << std::endl; std::cout << "-----------------------------------------------------------" << std::endl;
double max_effort; std::cout << "PWM limit " << cont->get_pwm_limit() << std::endl;
if(argc==2) if(argc==2)
max_effort = atoi(argv[1]); max_pwm = atoi(argv[1]);
else else
max_effort = 50; max_pwm = 50;
double desired_effort=max_effort; desired_pwm=max_pwm;
std::cout << "MOVE EFFORT: " << desired_effort << std::endl; std::cout << "MOVE PWM: " << desired_pwm << std::endl;
cont->move_torque(desired_effort); cont->move_pwm(desired_pwm);
std::cout << "Moving..." << std::endl; std::cout << "Moving..." << std::endl;
t=0; t=0;
while(t<timeout) while(t<timeout)
{ {
std::cout << "Current speed: " << cont->get_current_speed() << std::endl; std::cout << "Current pwm: " << cont->get_current_pwm() << std::endl;
usleep(uperiod); usleep(uperiod);
t++; t++;
} }
cont->move_torque(0); cont->move_pwm(0);
std::cout << "Done" << std::endl; std::cout << "Done" << std::endl;
sleep(1); sleep(1);
std::cout << "----------------------------" << std::endl; std::cout << "----------------------------" << std::endl;
desired_effort=-1.0*max_effort; desired_pwm=-1.0*max_pwm;
std::cout << "MOVE EFFORT: " << desired_effort << std::endl; std::cout << "MOVE PWM: " << desired_pwm << std::endl;
cont->move_torque(desired_effort); cont->move_pwm(desired_pwm);
std::cout << "Moving..." << std::endl; std::cout << "Moving..." << std::endl;
t=0; t=0;
while(t<timeout) while(t<timeout)
{ {
std::cout << "Current speed: " << cont->get_current_speed() << std::endl; std::cout << "Current pwm: " << cont->get_current_pwm() << std::endl;
usleep(uperiod); usleep(uperiod);
t++; t++;
} }
cont->move_torque(0); cont->move_pwm(0);
std::cout << "Done" << std::endl; std::cout << "Done" << std::endl;
std::cout << "-----------------------------------------------------------" << std::endl; std::cout << "-----------------------------------------------------------" << std::endl;
sleep(1); sleep(1);
//*/ cont->disable();
*/
/////////////////////////////////////////////////////////////////////////
////MOVE Current
/////////////////////////////////////////////////////////////////////////
/*
double time_interval = 0.1; //time in secs between checks
int max_time_sec = 10.0; //max time to wait until timeout
int t;
double uperiod = time_interval*1000000.0;
double timeout = max_time_sec/(uperiod/1000000.0);
double max_current,desired_current;
cont->enable();
std::cout << "-----------------------------------------------------------" << std::endl;
std::cout << "Current limit " << cont->get_current_limit() << std::endl;
if(argc==2)
max_current = atof(argv[1]);
else
max_current = 0.5;
desired_current=max_current;
std::cout << "MOVE current: " << desired_current << std::endl;
cont->move_current(desired_current);
std::cout << "Moving..." << std::endl;
t=0;
while(t<timeout)
{
std::cout << "Current current: " << cont->get_current_current() << std::endl;
usleep(uperiod);
t++;
}
cont->move_current(0);
std::cout << "Done" << std::endl;
sleep(1);
std::cout << "----------------------------" << std::endl;
desired_current=-1.0*max_current;
std::cout << "MOVE current: " << desired_current << std::endl;
cont->move_current(desired_current);
std::cout << "Moving..." << std::endl;
t=0;
while(t<timeout)
{
std::cout << "Current current: " << cont->get_current_current() << std::endl;
usleep(uperiod);
t++;
}
cont->move_current(0);
std::cout << "Done" << std::endl;
std::cout << "-----------------------------------------------------------" << std::endl;
sleep(1);
cont->disable();
*/
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
////MOVE RANDOM TORQUES AND OUTPUT SPEEDS ////MOVE RANDOM TORQUES AND OUTPUT SPEEDS
///////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////
///* ///*
/*
std::cout << "MOVE RANDOM EFFORTS" << std::endl; std::cout << "MOVE RANDOM EFFORTS" << std::endl;
int min=-100; int min=-100;
int max=100; int max=100;
...@@ -250,8 +319,7 @@ int main(int argc, char *argv[]) ...@@ -250,8 +319,7 @@ int main(int argc, char *argv[])
std::cout << "Done" << std::endl; std::cout << "Done" << std::endl;
std::cout << "-----------------------------------------------------------" << std::endl; std::cout << "-----------------------------------------------------------" << std::endl;
sleep(1); sleep(1);
//*/ */
} }
......
src/xml/dynamixel_motor_cfg_file.xsd
+ 1
− 1
View file @ 9953b35a
...@@ -36,7 +36,7 @@ copyright : not copyrighted - public domain ...@@ -36,7 +36,7 @@ copyright : not copyrighted - public domain
</xsd:element> </xsd:element>
<xsd:element name="min_voltage" type="xsd:float"> <xsd:element name="min_voltage" type="xsd:float">
</xsd:element> </xsd:element>
<xsd:element name="max_torque" type="xsd:float"> <xsd:element name="max_pwm" type="xsd:float">
</xsd:element> </xsd:element>
<xsd:element name="cw_comp_margin" type="xsd:unsignedByte"> <xsd:element name="cw_comp_margin" type="xsd:unsignedByte">
</xsd:element> </xsd:element>
......
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