From e2f712ae3dbbef1683cb2381b6c6d57d952f2800 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sat, 18 Apr 2020 22:29:33 +0200
Subject: [PATCH] Read the control parameters from the servo when initializing
them. Added a return value to the function to read servo registers.
---
.../src/modules/motion_manager.c | 24 +++++++++++++++----
1 file changed, 20 insertions(+), 4 deletions(-)
diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c
index 524cdbf..b8c8bbb 100644
--- a/dynamixel_manager/src/modules/motion_manager.c
+++ b/dynamixel_manager/src/modules/motion_manager.c
@@ -4,7 +4,7 @@
#include "ram.h"
/* private functions */
-void mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int *value)
+unsigned char mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int *value)
{
unsigned char reg_value[4]={0};
@@ -17,7 +17,10 @@ void mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *r
(*value)=reg_value[0]+(reg_value[1]<<8);
else if(reg->size==4)
(*value)=reg_value[0]+(reg_value[1]<<8)+(reg_value[2]<<16)+(reg_value[3]<<24);
+ return 0x01;
}
+ else
+ return 0x00;
}
void mmanager_write_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int value)
@@ -97,7 +100,7 @@ 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
+ 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;
@@ -139,14 +142,27 @@ void mmanager_add_device(TMotionManager *mmanager,unsigned char id,unsigned shor
mmanager->servo_configs[id]=&servo_data[i];
mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[current_pos],&mmanager->servo_values[id].current_value);
- mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[min_angle_limit],&cw_value);
- mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[max_angle_limit],&ccw_value);
mmanager->servo_values[id].target_value=mmanager->servo_values[id].current_value;
mmanager->servo_values[id].current_angle=(mmanager_value_to_angle(mmanager,id,mmanager->servo_values[id].current_value)<<9);
mmanager->servo_values[id].target_angle=mmanager->servo_values[id].current_angle;
// read the servo limits
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[min_angle_limit],&cw_value);
mmanager->servo_configs[id]->cw_angle_limit=(mmanager_value_to_angle(mmanager,id,cw_value)<<9);
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[max_angle_limit],&ccw_value);
mmanager->servo_configs[id]->ccw_angle_limit=(mmanager_value_to_angle(mmanager,id,ccw_value)<<9);
+ // read the servo control values
+ if(mmanager->servo_configs[id]->pid)
+ {
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[pos_pid_p],&cw_value);
+ mmanager->servo_values[id].cw_compliance=cw_value;
+ }
+ else
+ {
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[cw_comp_slope],&cw_value);
+ mmanager->servo_values[id].cw_compliance=cw_value;
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[ccw_comp_slope],&ccw_value);
+ mmanager->servo_values[id].ccw_compliance=ccw_value;
+ }
mmanager->present_servos|=(0x00000001<<id);
mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET]=mmanager->present_servos&0x000000FF;
mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET+1]=(mmanager->present_servos>>8)&0x000000FF;
--
GitLab