diff --git a/include/dynamixel_motor.h b/include/dynamixel_motor.h
index 185a7dc2b06716345e291e052d044b60b7602fc8..9329a7cdc8eefe07c95fe958e104c81abab3f7ef 100644
--- a/include/dynamixel_motor.h
+++ b/include/dynamixel_motor.h
@@ -129,48 +129,48 @@ class CDynamixelMotor
* \brief
*
*/
- unsigned int from_angles(double angle);
+ double to_angles(unsigned int counts);
/**
* \brief
*
*/
- unsigned int from_speeds(double speed);
+ double to_speeds(unsigned int counts);
/**
- * \brief
+ * \brief
*
*/
- double to_angles(unsigned int counts);
+ void reset_motor(void);
/**
- * \brief
+ * \brief
*
*/
- double to_speeds(unsigned int counts);
+ void get_model(void);
/**
* \brief
*
*/
- void reset_motor(void);
+ void read_register(TDynReg reg, unsigned int &value);
/**
* \brief
*
*/
- void get_model(void);
+ void write_register(TDynReg reg, unsigned int value);
+ public:
/**
* \brief
*
*/
void set_control_mode(control_mode mode);
/**
- * \brief
+ * \brief
*
*/
- void read_register(TDynReg reg, unsigned int &value);
+ unsigned int from_angles(double angle);
/**
- * \brief
+ * \brief
*
*/
- void write_register(TDynReg reg, unsigned int value);
- public:
+ unsigned int from_speeds(double speed);
/**
* \brief
*
@@ -181,7 +181,7 @@ class CDynamixelMotor
*
*/
void get_servo_info(TDynamixel_info &info);
- /* configuration functions */
+ /* configuration functions */
/**
* \brief
*
diff --git a/include/dynamixel_registers.h b/include/dynamixel_registers.h
index 5e32a305a56317005c53c21cba49e1e5d767e499..dd29dcb99f8817fa9e50588f199d602deee6ef13 100644
--- a/include/dynamixel_registers.h
+++ b/include/dynamixel_registers.h
@@ -1,7 +1,7 @@
#ifndef _DYNAMIXEL_REGISTERS_H
#define _DYNAMIXEL_REGISTERS_H
-#define NUM_REG 62
+#define NUM_REG 63
typedef struct{
unsigned short int address;
@@ -14,6 +14,7 @@ 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];
+extern TDynReg mx_28_2_0_reg[NUM_REG];
typedef enum {
// [Access] [Description]
@@ -44,6 +45,7 @@ typedef enum {
velocity_limit, // (RW)
multi_turn_off, // (RW) Multi turn offset position
resolution_div, // (RW) Resolution divider
+ acceleration_limit, // (RW)
// Status
alarm_led, // (RW) LED for Alarm
alarm_shtdwn, // (RW) Shutdown for Alarm
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 6648e9813929c37978e943a00954e7d4a521a2b4..31dada7c1f647af797594afa8e4474e55e69c154 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -237,6 +237,18 @@ void CDynamixelMotor::get_model(void)
this->info.multi_turn=true;
this->info.ext_memory_map=false;
break;
+ case 0x001E: this->info.model="MX-28 v2";
+ this->info.max_angle=360.0;
+ this->info.center_angle=180.0;
+ this->info.max_speed=330;
+ this->info.max_current=0.0;
+ this->info.encoder_resolution=4095;
+ this->info.gear_ratio=193;
+ this->registers=mx_28_2_0_reg;
+ this->info.pid_control=true;
+ this->info.multi_turn=true;
+ this->info.ext_memory_map=false;
+ break;
case 0x0136: this->info.model="MX-64";
this->info.max_angle=360.0;
this->info.center_angle=180.0;
@@ -285,6 +297,17 @@ void CDynamixelMotor::get_model(void)
this->info.multi_turn=true;
this->info.ext_memory_map=true;
break;
+ case 0x460: this->info.model="XM540-W270";
+ this->info.max_angle=350.0;
+ this->info.center_angle=180.0;
+ this->info.max_speed=1023 * 0.229;
+ this->info.max_current=0.0;
+ this->info.encoder_resolution=4095;
+ this->info.gear_ratio=272.5;
+ this->registers=xm_reg;
+ this->info.pid_control=true;
+ this->info.multi_turn=true;
+ break;
default: this->info.model="unknown";
break;
}
diff --git a/src/dynamixel_registers.cpp b/src/dynamixel_registers.cpp
index 576f611d5d8a3d409a83a6baaa02f3d974329806..29cd4f96406d3b3bd03814e420689369a95a24ac 100644
--- a/src/dynamixel_registers.cpp
+++ b/src/dynamixel_registers.cpp
@@ -27,6 +27,7 @@ TDynReg ax_reg[NUM_REG]={// Info
{0xFFFF,0,false},
{0xFFFF,0,false},
{0xFFFF,0,false},
+ {0x0028,4,false},
// Status
{0x0011,1,true},
{0x0012,1,true},
@@ -96,6 +97,7 @@ TDynReg mx_1_0_reg[NUM_REG]={// Info
{0x0020,2,false},
{0x0014,2,true},
{0x0016,1,true},
+ {0x0028,4,false},
// Status
{0x0011,1,true},
{0x0012,1,true},
@@ -165,6 +167,7 @@ TDynReg mx_106_1_0_reg[NUM_REG]={// Info
{0xFFFF,0,false},
{0x0014,2,true},
{0x0016,1,true},
+ {0x0028,4,false},
// Status
{0x0011,1,true},
{0x0012,1,true},
@@ -234,6 +237,7 @@ TDynReg xl_reg[NUM_REG]={// Info
{0xFFFF,0,false},
{0xFFFF,0,false},
{0xFFFF,0,false},
+ {0x0028,4,false},
// Status
{0x0012,1,true},
{0x0012,1,true},
@@ -297,12 +301,13 @@ TDynReg xm_reg[NUM_REG]={// Info
{0x0022,2,true},
{0x0020,2,true},
{0x0024,2,true},
- {0xFFFF,0,false},
+ {0x0024,2,false},
{0x0026,2,true},
{0xFFFF,0,false},
{0x002C,4,true},
{0xFFFF,0,false},
{0xFFFF,0,false},
+ {0x0028,4,false},
// Status
{0x003F,1,true},
{0x003F,1,true},
@@ -345,3 +350,72 @@ TDynReg xm_reg[NUM_REG]={// Info
{0x0088,5,false},
{0x008C,4,false}};
+TDynReg mx_28_2_0_reg[NUM_REG]={// Info
+ {0x0000,2,true},
+ {0x0002,4,true},
+ {0x0006,1,true},
+ {0x0007,1,true},
+ {0x0008,1,true},
+ {0x0009,1,true},
+ {0x000C,1,true},
+ {0x000D,1,true},
+ {0x0044,1,false},
+ // Configuration
+ {0x000A,1,true},
+ {0x000B,1,true},
+ {0x0014,4,true},
+ {0x0018,4,true},
+ {0x0034,4,true},
+ {0x0030,4,true},
+ {0x001F,1,true},
+ {0x0022,2,true},
+ {0x0020,2,true},
+ {0x0024,2,true},
+ {0x0024,2,true},
+ {0x0026,2,false},
+ {0xFFFF,0,false},
+ {0x002C,4,true},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0028,4,true},
+ // Status
+ {0x003F,1,true},
+ {0x003F,1,true},
+ {0x0041,1,false},
+ {0x0062,1,false},
+ {0x007A,1,false},
+ {0x007B,1,false},
+ {0xFFFF,0,false},
+ {0x0045,1,false},
+ {0x0046,1,false},
+ // Control
+ {0x0040,1,false},
+ {0x004C,2,false},
+ {0x004E,2,false},
+ {0x0054,2,false},
+ {0x0052,2,false},
+ {0x0050,2,false},
+ {0x0058,2,false},
+ {0x005A,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ // Set point
+ {0x0074,4,false},
+ {0x0068,4,false},
+ {0x0064,2,false},
+ {0x0066,2,false},
+ {0x006C,4,false},
+ {0x0070,4,false},
+ // Feedback
+ {0x0084,4,false},
+ {0x0080,4,false},
+ {0x007E,2,false},
+ {0x0090,2,false},
+ {0x0092,1,false},
+ {0x007C,2,false},
+ {0x0078,2,false},
+ {0x0088,5,false},
+ {0x008C,4,false}};
\ No newline at end of file