From dc6fb5f062de8abf98d2efb45a25274a040b0bf0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sun, 7 Aug 2016 12:53:16 +0200
Subject: [PATCH] Changed the angle range and motion limits of the emmulated
dynamixel servos to coincide with the real ones.
---
.../include/bioloid_controller_cm510_impl.h | 3 ++-
bioloid_controller_cm510/src/sim/dynamixel_master.c | 6 +++---
2 files changed, 5 insertions(+), 4 deletions(-)
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
index 27d1a06..065a80f 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+++ b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
@@ -369,7 +369,7 @@ namespace bioloid_controller_cm510
for (unsigned int i = 0; i < this->joints_.size(); ++i)
{
this->compliances_[i].slope=1<<(manager_servos[i+1].slope&0x0F);
- target_angles[i] = ((((manager_servos[i+1].current_value+balance_offsets[i+1]-manager_servos[i+1].center_value)*300.0/1023.0)*3.14159)/180.0);
+ target_angles[i] = ((((manager_servos[i+1].current_value+balance_offsets[i+1]-manager_servos[i+1].center_value)*manager_servos[i+1].angle/1023.0)*3.14159)/180.0);
const double command = this->compliance_control(this->compliances_[i],target_angles[i]-real_angles[i]);
this->joints_[i].setCommand(command);
}
@@ -542,6 +542,7 @@ namespace bioloid_controller_cm510
else if(frame_name==this->exp_gpio18_frame)
exp_gpio_values[18]=threshold;
}
+
}
template <class HardwareInterface>
diff --git a/bioloid_controller_cm510/src/sim/dynamixel_master.c b/bioloid_controller_cm510/src/sim/dynamixel_master.c
index ab59668..7ab8bd8 100644
--- a/bioloid_controller_cm510/src/sim/dynamixel_master.c
+++ b/bioloid_controller_cm510/src/sim/dynamixel_master.c
@@ -179,11 +179,11 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
if(address==MODEL_NUM)
*data=0x0C00;
else if(address==CURRENT_POS)
- *data=((((real_angles[id-1]*180.0)/3.143159)+150.0)*1023.0)/300.0;
+ *data=((((real_angles[id-1]*180.0)/3.143159)+90.0)*1023.0)/180.0;
else if(address==CW_ANGLE_LIM)
- *data=205;
+ *data=0;
else if(address==CCW_ANGLE_LIM)
- *data=818;
+ *data=1023;
else
*data=0x0000;
}
--
GitLab