Skip to content
GitLab
Commit 9953b35a authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merge branch 'new_servos_support'

parents 7ebe214e 48e0dd1e
Expand all Hide whitespace changes
Inline Side-by-side
src/dynamixel_motor.cpp
View file @ 9953b35a
This diff is collapsed.
src/dynamixel_motor.h
View file @ 9953b35a
......@@ -2,6 +2,7 @@
#define _DYNAMIXEL_MOTOR_H
#include "dynamixelserver.h"
#include "dynamixel_registers.h"
#include "threadserver.h"
#include "eventserver.h"
#include "ftdimodule.h"
......@@ -11,6 +12,7 @@
#include <string>
#include <vector>
#include <memory>
#include <limits>
typedef enum
{
......@@ -30,11 +32,14 @@ typedef struct
unsigned int gear_ratio;
unsigned int encoder_resolution;
bool pid_control;
bool multi_turn;
double max_angle;
double center_angle;
double max_speed;
double max_current;
unsigned int baudrate;
unsigned char id;
bool ext_memory_map;
}TDynamixel_info;
typedef struct
......@@ -47,9 +52,9 @@ typedef struct
typedef struct
{
unsigned char p;
unsigned char i;
unsigned char d;
unsigned short int p;
unsigned short int i;
unsigned short int d;
}TDynamixel_pid;
typedef struct
......@@ -59,11 +64,12 @@ typedef struct
double max_temperature;
double max_voltage;
double min_voltage;
double max_torque;
double max_pwm;
double max_current;
unsigned short int punch;
}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
......@@ -112,7 +118,12 @@ class CDynamixelMotor
* \brief
*
*/
unsigned short int *registers;
TDynReg *registers;
/**
* \brief
*
*/
bool enabled;
protected:
/**
* \brief
......@@ -128,12 +139,12 @@ class CDynamixelMotor
* \brief
*
*/
double to_angles(unsigned short int counts);
double to_angles(unsigned int counts);
/**
* \brief
*
*/
double to_speeds(unsigned short int counts);
double to_speeds(unsigned int counts);
/**
* \brief
*
......@@ -149,6 +160,16 @@ class CDynamixelMotor
*
*/
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:
/**
* \brief
......@@ -237,22 +258,42 @@ class CDynamixelMotor
* \brief
*
*/
double get_max_torque(void);
double get_max_pwm(void);
/**
* \brief
*
*/
double get_limit_torque(void);
double get_pwm_limit(void);
/**
* \brief
*
*/
void set_max_torque(double torque_ratio);
void set_max_pwm(double torque_ratio);
/**
* \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
*
......@@ -273,6 +314,26 @@ class CDynamixelMotor
*
*/
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 */
/**
* \brief
......@@ -308,20 +369,30 @@ class CDynamixelMotor
* \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
*
*/
void move_relative_angle(double angle,double speed);
void move_pwm(double pwm_ratio);
/**
* \brief
*
*/
void move_torque(double torque_ratio);
void move_current(double current);
/**
* \brief
*
*
*/
void stop(void);
/**
......@@ -338,7 +409,12 @@ class CDynamixelMotor
* \brief
*
*/
double get_current_effort(void);
double get_current_pwm(void);
/**
* \brief
*
*/
double get_current_current(void);
/**
* \brief
*
......@@ -353,12 +429,12 @@ class CDynamixelMotor
* \brief
*
*/
unsigned short int get_punch(void);
unsigned int get_punch(void);
/**
* \brief
*
*/
void set_punch(unsigned short int punch_value);
void set_punch(unsigned int punch_value);
/**
* \brief
*
......
src/dynamixel_motor_exceptions.cpp
View file @ 9953b35a
......@@ -5,6 +5,7 @@
const std::string dynamixel_motor_error_message="DynamixelMotor 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 - ";
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&
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)
{
std::stringstream text;
......
src/dynamixel_motor_exceptions.h
View file @ 9953b35a
......@@ -33,6 +33,19 @@ class CDynamixelMotorGroupException : public CException
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
*/
......
src/dynamixel_motor_group.cpp
View file @ 9953b35a
This diff is collapsed.
src/dynamixel_motor_group.h
View file @ 9953b35a
......@@ -66,7 +66,7 @@ class CDynamixelMotorGroup
* \brief
*
*/
std::vector<unsigned short int *>registers;
std::vector<TDynReg *>registers;
protected:
/**
* \brief
......@@ -142,7 +142,7 @@ class CDynamixelMotorGroup
* \brief
*
*/
void set_max_torque(unsigned int index,double torque_ratio);
void set_max_pwm(unsigned int index,double torque_ratio);
/**
* \brief
*
......@@ -152,7 +152,7 @@ class CDynamixelMotorGroup
* \brief
*
*/
double get_max_torque(unsigned int index);
double get_max_pwm(unsigned int index);
/**
* \brief
*
......@@ -178,6 +178,16 @@ class CDynamixelMotorGroup
*
*/
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:
/**
* \brief
......@@ -286,12 +296,12 @@ class CDynamixelMotorGroup
* \brief
*
*/
void get_punch(std::vector<unsigned short int> &punches);
void get_punch(std::vector<unsigned int> &punches);
/**
* \brief
*
*/
void set_punch(std::vector<unsigned short int> &punches);
void set_punch(std::vector<unsigned int> &punches);
/**
* \brief
*
......
src/dynamixel_registers.cpp
View file @ 9953b35a
This diff is collapsed.
src/dynamixel_registers.h
View file @ 9953b35a
#ifndef _DYNAMIXEL_REGISTERS_H
#define _DYNAMIXEL_REGISTERS_H
extern unsigned short int std_compl_reg[39];
extern unsigned short int std_pid_reg[39];
extern unsigned short int xl_reg[39];
#define NUM_REG 62
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 {
// [Access] [Description]
// EEPROM Area
model_number=0, // (R) Model Number
// Info
model_number=0, // (R) Model Number
model_info, // (R)
firmware_version, // (R) Version of the Firmware
id, // (RW) ID of Dynamixel
baudrate, // (RW) Baud Rate of Dynamixel
return_delay_time, // (RW) Return Delay Time
cw_angle_limit, // (RW) Clockwise Angle Limit
ccw_angle_limit, // (RW) Counterclockwise Angle Limit
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
second_id, // (RW)
protocol, // (RW)
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_shtdwn, // (RW) Shutdown for Alarm
down_cal, // (RW)
up_cal, // (RW)
// RAM Area
torque_en, // (RW) Torque On/Off
led, // (RW) LED On/Off
pid_p, // (RW)
pid_i, // (RW)
pid_d, // (RW)
bus_watchdog, // (RW)
moving, // (R) Means if there is any movement
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
ccw_comp_margin, // (RW) CCW Compliance Margin
cw_comp_slope, // (RW) CW Compliance Slope
ccw_comp_slope, // (RW) CCW Compliance Slope
punch, // (RW) Punch
// Set point
goal_pos, // (RW) Goal Position
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_speed, // (R) Current Speed
current_load, // (R) Current Load
current_voltage, // (R) Current Voltage
current_temp, // (RW) Current Temperature
reg_inst, // (RW) Means if Instruction is registered
moving, // (R) Means if there is any movement
lock, // (RW) Locking EEPROM
hardware_error, // (R) Means if there is any movement
punch // (RW) Punch
current_temp, // (R) Current Temperature
current_pwm, // (R)
realtime_tick, // (R) Realtime tick
velocity_traj, // (R)
pos_traj // (R)
} 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
src/examples/test_dynamixel_motor.cpp
View file @ 9953b35a
......@@ -11,9 +11,9 @@ 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"
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 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();
CDynamixelMotor *cont = NULL;
......@@ -29,12 +29,12 @@ int main(int argc, char *argv[])
if(dyn_server->get_num_buses()>0)
{
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
/////////////////////////////////////////////////////////////////////////
/*
/*
std::cout << "-----------------------------------------------------------" << std::endl;
cont->get_servo_info(info);
std::cout << "[INFO]: Servo model: " << info.model << std::endl;
......@@ -44,10 +44,6 @@ int main(int argc, char *argv[])
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;
}
......@@ -76,15 +72,15 @@ int main(int argc, char *argv[])
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;
//std::cout << "[INFO]: max_pwm: " << std::dec << cont->get_max_pwm() << 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;
cont->turn_led_on();
sleep(1);
cont->turn_led_off();
//*/
*/
/*
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
......@@ -94,13 +90,13 @@ int main(int argc, char *argv[])
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;
......@@ -130,29 +126,35 @@ int main(int argc, char *argv[])
std::cout << "Error angle: " << current_rel_angle-desired_angle << std::endl;
std::cout << "Done" << std::endl;
sleep(1);
//*/
*/
/////////////////////////////////////////////////////////////////////////
////MOVE ABSOLUTE ANGLE
/////////////////////////////////////////////////////////////////////////
/*