From 2e269b809ca18bfc3a73ad7237337aba8ee1aad5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ferran=20Mart=C3=ADnez=20Felipe?= <fmartinez@iri.upc.edu>
Date: Mon, 24 Jan 2022 16:15:15 +0100
Subject: [PATCH] Added support to MX28 v2 and MX540
---
include/dynamixel_motor.h | 28 ++++++-------
include/dynamixel_registers.h | 4 +-
src/dynamixel_motor.cpp | 23 +++++++++++
src/dynamixel_registers.cpp | 76 ++++++++++++++++++++++++++++++++++-
4 files changed, 115 insertions(+), 16 deletions(-)
diff --git a/include/dynamixel_motor.h b/include/dynamixel_motor.h
index 185a7dc..9329a7c 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 5e32a30..dd29dcb 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 6648e98..31dada7 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 576f611..29cd4f9 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
--
GitLab