From 4b38ed7d99c3fa37cf713a42024db8aa7b9d06e8 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 28 Feb 2023 16:51:18 +0100
Subject: [PATCH] Added support for the drive_mode. Solved some bugs when
computing speeds, accelerations and loads.
---
include/dynamixel_motor.h | 40 +++++++
src/dynamixel_motor.cpp | 221 +++++++++++++++++++++++++++++---------
2 files changed, 211 insertions(+), 50 deletions(-)
diff --git a/include/dynamixel_motor.h b/include/dynamixel_motor.h
index 71788bb..ec911da 100644
--- a/include/dynamixel_motor.h
+++ b/include/dynamixel_motor.h
@@ -114,6 +114,16 @@ class CDynamixelMotor
*
*/
control_mode mode;
+ /**
+ * \brief
+ *
+ */
+ bool time_based_mode;
+ /**
+ * \brief
+ *
+ */
+ bool reverse_mode;
/**
* \brief
*
@@ -151,6 +161,11 @@ class CDynamixelMotor
*
*/
void set_control_mode(control_mode mode);
+ /**
+ * \brief
+ *
+ */
+ void set_drive_mode(bool time_based,bool reverse);
/**
* \brief
*
@@ -161,6 +176,16 @@ class CDynamixelMotor
*
*/
double to_speeds(signed int counts);
+ /**
+ * \brief
+ *
+ */
+ double to_accelerations(signed int counts);
+ /**
+ * \brief
+ *
+ */
+ double to_currents(signed int counts);
/**
* \brief
*
@@ -171,6 +196,16 @@ class CDynamixelMotor
*
*/
signed int from_speeds(double speed);
+ /**
+ * \brief
+ *
+ */
+ signed int from_accelerations(double accel);
+ /**
+ * \brief
+ *
+ */
+ signed int from_currents(double current);
/**
* \brief
*
@@ -440,6 +475,11 @@ class CDynamixelMotor
*
*/
control_mode get_control_mode(void);
+ /**
+ * \brief
+ *
+ */
+ void get_drive_mode(bool &time_based,bool &reverse);
/**
* \brief
*
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index d417b52..e2f60a3 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -53,6 +53,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this->info.id=dev_id;
this->get_model();
this->get_control_mode();
+ this->get_drive_mode(this->time_based_mode,this->reverse_mode);
this->config.max_temperature=this->get_temperature_limit();
this->get_voltage_limits(&this->config.min_voltage,&this->config.max_voltage);
this->config.max_pwm=this->get_pwm_limit();
@@ -86,12 +87,52 @@ signed int CDynamixelMotor::from_speeds(double speed)
{
unsigned int counts;
- if(speed>this->info.max_speed)
- speed=this->info.max_speed;
+ if(!this->time_based_mode)
+ {
+ if(speed>this->info.max_speed)
+ speed=this->info.max_speed;
+ if(this->info.ext_memory_map)
+ counts=(speed/6.0)/0.229;
+ else
+ counts=fabs(speed/6.0)/0.111;
+ }
+ else
+ {
+ counts=speed*1000;
+ }
+
+ return counts;
+}
+
+signed int CDynamixelMotor::from_accelerations(double accel)
+{
+ unsigned int counts;
+
+ if(!this->time_based_mode)
+ {
+ if(this->info.ext_memory_map)
+ counts=(accel*10.0)/214.577;
+ else
+ counts=0;
+ }
+ else
+ counts=accel*1000;
+
+ return counts;
+}
+
+signed int CDynamixelMotor::from_currents(double current)
+{
+ unsigned int counts;
+
+ if(current>this->info.max_current)
+ current=this->info.max_current;
+ else if(current<-this->info.max_current)
+ current=-this->info.max_current;
if(this->info.ext_memory_map)
- counts=speed/0.229;
+ counts=current/0.00269;
else
- counts=fabs(speed)/0.666;
+ counts=1000.0*(current/this->info.max_current);
return counts;
}
@@ -109,14 +150,48 @@ double CDynamixelMotor::to_speeds(signed int counts)
{
double speed;
- if(this->info.ext_memory_map)
- speed=counts*0.229;
+ if(!this->time_based_mode)
+ {
+ if(this->info.ext_memory_map)
+ speed=(counts*0.229)*6.0;
+ else
+ speed=(counts*0.111)*6.0; // Taken from the datasheets (sfoix)
+ }
else
- speed=counts*0.666; // Taken from the datasheets (sfoix)
+ speed=counts/1000.0;
return speed;
}
+double CDynamixelMotor::to_accelerations(signed int counts)
+{
+ double accel;
+
+ if(!this->time_based_mode)
+ {
+ if(this->info.ext_memory_map)
+ accel=(counts*214.577)*10.0;
+ else
+ accel=0.0;
+ }
+ else
+ accel=counts/1000.0;
+
+ return accel;
+}
+
+double CDynamixelMotor::to_currents(signed int counts)
+{
+ double current;
+
+ if(this->info.ext_memory_map)
+ current=(counts*0.00269);
+ else
+ current=(this->info.max_current*counts)/1000.0;
+
+ return current;
+}
+
void CDynamixelMotor::reset_motor(void)
{
unsigned int current_position;
@@ -169,73 +244,73 @@ void CDynamixelMotor::get_model(void)
this->info.max_angle=300.0;
this->info.center_angle=150.0;
this->info.max_speed=354;
- this->info.max_current=0.0;
+ this->info.max_current=0.0;
this->info.encoder_resolution=1023;
this->info.gear_ratio=254;
- this->registers=ax_reg;
+ this->registers=ax_reg;
this->info.pid_control=false;
- this->info.multi_turn=false;
- this->info.ext_memory_map=false;
+ this->info.multi_turn=false;
+ this->info.ext_memory_map=false;
break;
case 0x012C: this->info.model="AX-12W";
this->info.max_angle=300.0;
this->info.center_angle=150.0;
this->info.max_speed=2830;
- this->info.max_current=0.0;
+ this->info.max_current=0.0;
this->info.encoder_resolution=1023;
this->info.gear_ratio=32;
- this->registers=ax_reg;
+ this->registers=ax_reg;
this->info.pid_control=false;
- this->info.multi_turn=false;
- this->info.ext_memory_map=false;
+ this->info.multi_turn=false;
+ this->info.ext_memory_map=false;
break;
case 0x0012: this->info.model="AX-18A";
this->info.max_angle=300.0;
this->info.center_angle=150.0;
this->info.max_speed=582;
- this->info.max_current=0.0;
+ this->info.max_current=0.0;
this->info.encoder_resolution=1023;
this->info.gear_ratio=254;
- this->registers=ax_reg;
+ this->registers=ax_reg;
this->info.pid_control=false;
- this->info.multi_turn=false;
- this->info.ext_memory_map=false;
+ this->info.multi_turn=false;
+ this->info.ext_memory_map=false;
break;
case 0x001C: this->info.model="RX-28";
this->info.max_angle=300.0;
this->info.center_angle=150.0;
this->info.max_speed=510;
- this->info.max_current=0.0;
+ this->info.max_current=0.0;
this->info.encoder_resolution=1023;
this->info.gear_ratio=193;
- this->registers=ax_reg;
+ this->registers=ax_reg;
this->info.pid_control=false;
- this->info.multi_turn=false;
- this->info.ext_memory_map=false;
+ this->info.multi_turn=false;
+ this->info.ext_memory_map=false;
break;
case 0x0168: this->info.model="MX-12";
this->info.max_angle=360.0;
this->info.center_angle=180.0;
this->info.max_speed=2820;
- this->info.max_current=0.0;
+ this->info.max_current=0.0;
this->info.encoder_resolution=4095;
this->info.gear_ratio=32;
- this->registers=mx_1_0_reg;
+ this->registers=mx_1_0_reg;
this->info.pid_control=true;
- this->info.multi_turn=true;
- this->info.ext_memory_map=false;
+ this->info.multi_turn=true;
+ this->info.ext_memory_map=false;
break;
case 0x001D: this->info.model="MX-28";
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.max_current=0.0;
this->info.encoder_resolution=4095;
this->info.gear_ratio=193;
- this->registers=mx_1_0_reg;
+ this->registers=mx_1_0_reg;
this->info.pid_control=true;
- this->info.multi_turn=true;
- this->info.ext_memory_map=false;
+ 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;
@@ -246,56 +321,56 @@ void CDynamixelMotor::get_model(void)
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;
+ this->info.multi_turn=true;
+ this->info.ext_memory_map=true;
break;
case 0x0136: this->info.model="MX-64";
this->info.max_angle=360.0;
this->info.center_angle=180.0;
this->info.max_speed=378;
- this->info.max_current=0.0;
+ this->info.max_current=0.0;
this->info.encoder_resolution=4095;
this->info.gear_ratio=200;
- this->registers=mx_1_0_reg;
+ this->registers=mx_1_0_reg;
this->info.pid_control=true;
- this->info.multi_turn=true;
- this->info.ext_memory_map=false;
+ this->info.multi_turn=true;
+ this->info.ext_memory_map=false;
break;
case 0x0140: this->info.model="MX-106";
this->info.max_angle=360.0;
this->info.center_angle=180.0;
this->info.max_speed=270;
- this->info.max_current=0.0;
+ this->info.max_current=0.0;
this->info.encoder_resolution=4095;
this->info.gear_ratio=225;
- this->registers=mx_106_1_0_reg;
+ this->registers=mx_106_1_0_reg;
this->info.pid_control=true;
- this->info.multi_turn=true;
- this->info.ext_memory_map=false;
+ this->info.multi_turn=true;
+ this->info.ext_memory_map=false;
break;
case 0x015E: this->info.model="XL_320";
this->info.max_angle=300.0;
this->info.center_angle=150;
this->info.max_speed=684;
- this->info.max_current=0.0;
+ this->info.max_current=0.0;
this->info.encoder_resolution=1023;
this->info.gear_ratio=238;
- this->registers=xl_reg;
+ this->registers=xl_reg;
this->info.pid_control=true;
- this->info.multi_turn=false;
- this->info.ext_memory_map=false;
+ this->info.multi_turn=false;
+ this->info.ext_memory_map=false;
break;
case 0x0406: this->info.model="XM-430-W210";
this->info.max_angle=360.0;
this->info.center_angle=180;
- this->info.max_speed=4620;
- this->info.max_current=2.3;
+ this->info.max_speed=4620;
+ this->info.max_current=2.3;
this->info.encoder_resolution=4095;
this->info.gear_ratio=213;
- this->registers=xm_reg;
+ this->registers=xm_reg;
this->info.pid_control=true;
- this->info.multi_turn=true;
- this->info.ext_memory_map=true;
+ 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;
@@ -307,6 +382,7 @@ void CDynamixelMotor::get_model(void)
this->registers=xm_reg;
this->info.pid_control=true;
this->info.multi_turn=true;
+ this->info.ext_memory_map=true;
break;
default: this->info.model="unknown";
break;
@@ -365,6 +441,17 @@ void CDynamixelMotor::set_control_mode(control_mode mode)
}
}
+void CDynamixelMotor::set_drive_mode(bool time_based,bool reverse)
+{
+ unsigned int value=0;
+
+ if(time_based)
+ value|=0x04;
+ if(reverse)
+ value|=0x01;
+ this->write_register(this->registers[drive_mode],value);
+}
+
void CDynamixelMotor::read_register(TDynReg reg, unsigned int &value)
{
unsigned char reg_value[4]={0};
@@ -1283,6 +1370,40 @@ control_mode CDynamixelMotor::get_control_mode(void)
return this->mode;
}
+void CDynamixelMotor::get_drive_mode(bool &time_based,bool &reverse)
+{
+ unsigned int value;
+
+ try{
+ this->read_register(this->registers[drive_mode],value);
+ if(value&0x04)
+ {
+ this->time_based_mode=true;
+ time_based=true;
+ }
+ else
+ {
+ this->time_based_mode=false;
+ time_based=false;
+ }
+ if(value&0x01)
+ {
+ this->reverse_mode=true;
+ reverse=true;
+ }
+ else
+ {
+ this->reverse_mode=false;
+ reverse=false;
+ }
+ }catch(CException &e){
+ this->time_based_mode=false;
+ time_based=false;
+ this->reverse_mode=false;
+ reverse=false;
+ }
+}
+
CDynamixelMotor::~CDynamixelMotor()
{
/* stop the motor */
--
GitLab