Commit e2f712ae authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Read the control parameters from the servo when initializing them.

Added a return value to the function to read servo registers.
parent 367c510a
......@@ -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;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment