Added the registers with the current servo angles.

Updated the current servo angles with actual feedback or the target angle depending on the servo.

dynamixel_manager/include/modules/motion_manager_registers.h

@@ -3,7 +3,7 @@
 
 #include "dyn_module_registers.h"
 
-#define RAM_MM_LENGTH                        (4+DYN_MANAGER_MAX_NUM_DEVICES/2)
+#define RAM_MM_LENGTH                        (4+DYN_MANAGER_MAX_NUM_DEVICES/2+2*DYN_MANAGER_MAX_NUM_DEVICES)
 
 #define MM_PRESENT_SERVOS_OFFSET             0
 #define MM_ENABLE_MODULE_OFFSET              4
@@ -11,6 +11,7 @@
   #define MM_EVEN_SER_MOD                    0x70
   #define MM_ODD_SER_EN                      0x08
   #define MM_ODD_SER_MOD                     0x07
+#define MM_CURRENT_ANGLES_OFFSET             (4+DYN_MANAGER_MAX_NUM_DEVICES/2)
 
 #define EEPROM_MM_LENGTH                     (DYN_MANAGER_MAX_NUM_DEVICES)
 

dynamixel_manager/src/modules/motion_manager.c

@@ -97,6 +97,11 @@ void mmanager_compute_targets(TMotionManager *mmanager)
       angle=(mmanager->servo_values[i].target_angle>>9)+offsets[i]+(mmanager->servo_values[i].offset<<3);
       //>>16 from the action codification, <<7 from the manager codification
       mmanager->servo_values[i].target_value=mmanager_angle_to_value(mmanager,i,angle);
+      if(mmanager->servo_configs[i]->protocol_ver==1 || mmanager->servo_configs[i]->model!=29 || mmanager->servo_configs[i]->model!=310 || mmanager->servo_configs[i]->model!=320 || mmanager->servo_configs[i]->model!=360)// get the target value as the feedback
+      {
+        mmanager->memory->data[mmanager->ram_base_address+MM_CURRENT_ANGLES_OFFSET+i*2]=angle&0x00FF;
+        mmanager->memory->data[mmanager->ram_base_address+MM_CURRENT_ANGLES_OFFSET+i*2+1]=(angle>>8)&0x00FF;
+      }
     }
   }
 }
@@ -104,12 +109,21 @@ void mmanager_compute_targets(TMotionManager *mmanager)
 void mmanager_compute_angles(TMotionManager *mmanager)
 {
   unsigned char i;
+  short int angle;
 
   // convert the digital values to angles
   for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
   {
     if(mmanager->servo_values[i].enabled)// servo is enabled and present
-      mmanager->servo_values[i].current_angle=(mmanager_value_to_angle(mmanager,i,mmanager->servo_values[i].current_value)<<9);
+    {
+      angle=mmanager_value_to_angle(mmanager,i,mmanager->servo_values[i].current_value);
+      mmanager->servo_values[i].current_angle=angle<<9;
+      if(mmanager->servo_configs[i]->protocol_ver==2 || mmanager->servo_configs[i]->model==29 || mmanager->servo_configs[i]->model==310 || mmanager->servo_configs[i]->model==320 || mmanager->servo_configs[i]->model==360)// get the actual feedback
+      {
+        mmanager->memory->data[mmanager->ram_base_address+MM_CURRENT_ANGLES_OFFSET+i*2]=angle&0x00FF;
+        mmanager->memory->data[mmanager->ram_base_address+MM_CURRENT_ANGLES_OFFSET+i*2+1]=(angle>>8)&0x00FF;
+      }
+    }
   }
 }
 
@@ -207,7 +221,7 @@ void mmanager_setup(TMotionManager *mmanager)
   {
     if(mmanager->dyn_module.assigned_ids[i]==0x01)
     {
-      if(mmanager->servo_configs[i]->protocol_ver==2)
+      if(mmanager->servo_configs[i]->protocol_ver==2 || mmanager->servo_configs[i]->model==29 || mmanager->servo_configs[i]->model==310 || mmanager->servo_configs[i]->model==320 || mmanager->servo_configs[i]->model==360)// MX series with version 1 or version 2
       {
         address[num]=mmanager->servo_configs[i]->registers[current_pos].address;
         data[num]=(unsigned char *)&mmanager->servo_values[i].current_value;