Homec4science
Diffusion Oncilla Motordriver Firmware (load-bad-commit) Browse

oncilla-motordriver-fw/src/sbcp_mdv.c
edc3f097732eload-bad-commit

Last Change
Raw File

sbcp_mdv.c
View Options

#include "sbcp_mdv.h"
#include "motordriver.h"
#include "magnetic_encoder.h"
#include "load_cell.h"
#include <packet.h>
#include "misc_math.h"
/*******************************************************************************
* C A L L B A C K S
*******************************************************************************/
typedef mdv_error (*set_param_fptr)(motordriver * mdv, int value);
typedef int (*get_param_fptr)(motordriver * mdv);
struct sbcp_mdv_setter_and_getter {
set_param_fptr setter;
get_param_fptr getter;
};
struct sbcp_mdv_setter_and_getter mdv_gAs[MX_REGISTER_SIZE];
#ifndef NDEBUG
enum sbcp_mdv_communication_error{
SBCP_MDV_CERR_NOT_A_MDV_REGISTER = MDV_ERR_NB_ERRORS, //6
SBCP_MDV_CERR_GETTER_UNSET, //7
SBCP_MDV_CERR_SETTER_UNSET // 8
};
#endif
sbcp_error sbcp_mdv_generic_clbk(sbcp_reg_address address, sbcp_reg_val * value){
#ifndef NDEBUG
if(address < MDV_M1 || address > MDV_M2 + MX_REGISTER_SIZE){
return SBCP_MDV_CERR_NOT_A_MDV_REGISTER;
}
#endif
enum sbcp_mdv_mX_register reg = address < MDV_M2 ? address - MDV_M1 : address - MDV_M2;
motordriver * mdv = address < MDV_M2 ? mdv1 : mdv2;
struct sbcp_mdv_setter_and_getter gAs = mdv_gAs[reg];
#ifndef NDEBUG
if(gAs.setter == 0x00){
return SBCP_MDV_CERR_SETTER_UNSET;
}
if(gAs.getter == 0x00){
return SBCP_MDV_CERR_GETTER_UNSET;
}
#endif
sbcp_error err = (*(gAs.setter))(mdv,value->i);
value->i = (*(gAs.getter))(mdv);
return err;
}
sbcp_error sbcp_mdv_generic_param_clbk(sbcp_reg_address address, sbcp_reg_val * value){
///\todo add min max value
mdv_parameters * params = (address < MDV_M2) ? mdv_get_parameters(mdv1) : mdv_get_parameters(mdv2);
switch( address < MDV_M2 ? address - MDV_M1 : address - MDV_M2){
case MX_PID_PGAIN :
params->p_gain = value->u;
break;
case MX_PID_IGAIN :
params->i_gain = value->u;
break;
case MX_PID_DGAIN :
params->d_gain = value->u;
break;
case MX_COMPLIANCE_PRELOAD :
params->preload = value->i;
break;
case MX_COMPLIANCE_STIFFNESS :
params->stiffness = value->u;
break;
case MX_COMPLIANCE_DAMPING :
params->damping = value->u;
break;
case MX_SMOOTH_POSITION_UPDATE :
params->smooth_interval = value->u;
break;
default :
return 42;
}
return SBCP_NO_ERROR;
}
sbcp_error sbcp_mdv_gear_clbk(sbcp_reg_address address, sbcp_reg_val * value){
motordriver * mdv = address < MDV_M2 ? mdv1 : mdv2;
enum sbcp_mdv_mX_register reg = address < MDV_M2 ? address - MDV_M1 : address - MDV_M2;
//get the gear either from the table, or the new value if this the one being set
int gear_mult = reg == MX_GEAR_MULT ? value->i : sbcp_reg_int(address);
int gear_div = reg == MX_GEAR_DIV ? value->i : sbcp_reg_int(address);
return mdv_set_gear_ratio(mdv,gear_mult,gear_div);
}
void init_sbcp_register_for_mdv(motordriver * mdv , sbcp_reg_address address_start){
#ifndef NDEBUG
unsigned int i;
for(i = 0; i < MX_REGISTER_SIZE; ++i){
mdv_gAs[i].setter = (set_param_fptr) 0x00;
mdv_gAs[i].getter = (get_param_fptr) 0x00;
}
#endif
mdv_gAs[MX_STATUS_AND_CONTROL_MODE].setter = & mdv_set_control_mode;
mdv_gAs[MX_STATUS_AND_CONTROL_MODE].getter = & mdv_get_control_mode;
mdv_gAs[MX_GOAL_POSITION].setter = &mdv_set_goal_position;
mdv_gAs[MX_GOAL_POSITION].getter = &mdv_get_goal_position;
mdv_gAs[MX_GOAL_SPEED].setter = &mdv_set_goal_speed;
mdv_gAs[MX_GOAL_SPEED].getter = &mdv_get_goal_speed;
mdv_gAs[MX_MAX_ACCELERATION].setter = &mdv_set_maximal_acceleration;
mdv_gAs[MX_MAX_ACCELERATION].getter = &mdv_get_maximal_acceleration;
mdv_gAs[MX_MAX_SPEED].setter = &mdv_set_maximal_speed;
mdv_gAs[MX_MAX_SPEED].getter = &mdv_get_maximal_speed;
mdv_gAs[MX_MAX_TORQUE].setter = &mdv_set_maximal_torque;
mdv_gAs[MX_MAX_TORQUE].getter = &mdv_get_maximal_torque;
sbcp_add_register(address_start + MX_STATUS_AND_CONTROL_MODE,
mdv_get_status_and_control_mode(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_GOAL_POSITION,
mdv_get_goal_position(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_TARGETED_POSITION,
mdv_get_virtual_goal_position(mdv),
SBCP_REG_READABLE ,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_PRESENT_POSITION,
mdv_get_position(mdv),
SBCP_REG_READABLE ,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_GOAL_SPEED,
mdv_get_goal_speed(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_TARGETED_SPEED,
mdv_get_virtual_goal_speed(mdv),
SBCP_REG_READABLE ,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_PRESENT_SPEED,
mdv_get_speed(mdv),
SBCP_REG_READABLE ,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_TARGETED_TORQUE,
mdv_get_targeted_torque(mdv),
SBCP_REG_READABLE ,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_PID_PGAIN,
mdv_get_parameters(mdv)->p_gain,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_PID_IGAIN,
mdv_get_parameters(mdv)->i_gain,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_PID_DGAIN,
mdv_get_parameters(mdv)->d_gain,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_COMPLIANCE_PRELOAD,
mdv_get_parameters(mdv)->preload,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_COMPLIANCE_STIFFNESS,
mdv_get_parameters(mdv)->stiffness,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_COMPLIANCE_DAMPING,
mdv_get_parameters(mdv)->damping,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_MAX_ACCELERATION,
mdv_get_maximal_acceleration(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_MAX_SPEED,
mdv_get_maximal_speed(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_MAX_TORQUE,
mdv_get_maximal_torque(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_GEAR_MULT,
mdv_get_gear_mult(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
&sbcp_mdv_gear_clbk);
sbcp_add_register(address_start + MX_GEAR_DIV,
mdv_get_gear_div(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
&sbcp_mdv_gear_clbk);
sbcp_add_register(address_start + MX_LIMIT_POS_CW,
ANGULAR_RESOLUTION - 1,
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_CALIBRATION_REGION_MIN,
0,
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_CALIBRATION_REGION_MAX,
ANGULAR_RESOLUTION - 1,
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_CALIBRATION_OFFSET,
0,
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_SMOOTH_POSITION_UPDATE,
mdv_get_parameters(mdv)->smooth_interval,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
}
sbcp_error magnetic_encoder_gear_clbk(sbcp_reg_address addr, sbcp_reg_val * value){
magnetic_encoder * q = 0;
if(addr >= MDV_ME_Q1 && addr < MDV_ME_Q2){
q = me1;
addr -= MDV_ME_Q1;
} else if ( addr < MDV_ME_Q3) {
q = me2;
addr -= MDV_ME_Q2;
} else if ( addr < MDV_ME_Q3 + MEX_REGISTER_SIZE){
q = me3;
addr -= MDV_ME_Q3;
} else {
/** \todo replace this 42 */
return 42;
}
int gear_mult = addr == MEX_GEAR_MULT ? value->i : me_get_gear_mult(q);
int gear_div = addr == MEX_GEAR_DIV ? value->i : me_get_gear_div(q);
me_set_gear_ratio(q,gear_mult,gear_div);
return SBCP_NO_ERROR;
}
void init_sbcp_register_for_magnetic_encoder(magnetic_encoder * q, sbcp_reg_address start){
sbcp_add_register(start + MEX_POSITION,
me_get_value(q),
SBCP_REG_READABLE,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(start + MEX_GEAR_MULT,
me_get_gear_mult(q),
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
&magnetic_encoder_gear_clbk);
sbcp_add_register(start + MEX_GEAR_DIV,
me_get_gear_div(q),
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
& magnetic_encoder_gear_clbk);
}
void init_sbcp_register_for_sine(sbcp_reg_address start, int defaultAmplitude,
int defaultOffset,
int defaultFrequency){
sbcp_add_register_no_callback(start + SINE_FREQUENCY, defaultFrequency, SBCP_REG_READABLE | SBCP_REG_WRITABLE);
sbcp_add_register_no_callback(start + SINE_OFFSET, defaultOffset, SBCP_REG_READABLE | SBCP_REG_WRITABLE);
sbcp_add_register_no_callback(start + SINE_AMPLITUDE, defaultAmplitude, SBCP_REG_READABLE | SBCP_REG_WRITABLE);
}
void init_sbcp_register_for_load_cell(load_cell * q, sbcp_reg_address start){
sbcp_add_register(start + LCX_TORQUE,
lc_get_torque(q),
SBCP_REG_READABLE,
SBCP_REG_NO_CALLBACK);
}
void init_sbcp_mdv_registers(){
init_sbcp_register_for_magnetic_encoder(me1,MDV_ME_Q1);
init_sbcp_register_for_magnetic_encoder(me2,MDV_ME_Q2);
init_sbcp_register_for_magnetic_encoder(me3,MDV_ME_Q3);
init_sbcp_register_for_load_cell(lc1,MDV_LC_AXIS_1);
init_sbcp_register_for_load_cell(lc2,MDV_LC_AXIS_2);
init_sbcp_register_for_load_cell(lc3,MDV_LC_AXIS_3);
init_sbcp_register_for_mdv(mdv1,MDV_M1);
init_sbcp_register_for_mdv(mdv2,MDV_M2);
sbcp_append_low_latency_in_register(MDV_M1 + MX_GOAL_POSITION);
sbcp_append_low_latency_in_register(MDV_M2 + MX_GOAL_POSITION);
sbcp_append_low_latency_out_register(MDV_M1 + MX_GOAL_SPEED);
sbcp_append_low_latency_out_register(MDV_M2 + MX_GOAL_SPEED);
sbcp_append_low_latency_out_register(MDV_M1 + MX_PRESENT_POSITION);
sbcp_append_low_latency_out_register(MDV_M2 + MX_PRESENT_POSITION);
sbcp_append_low_latency_out_register(MDV_M1 + MX_PRESENT_SPEED);
sbcp_append_low_latency_out_register(MDV_M2 + MX_PRESENT_SPEED);
sbcp_append_low_latency_out_register(MDV_M1 + MX_TARGETED_TORQUE);
sbcp_append_low_latency_out_register(MDV_M2 + MX_TARGETED_TORQUE);
sbcp_append_low_latency_out_register(MDV_ME_Q1 + MEX_POSITION);
sbcp_append_low_latency_out_register(MDV_ME_Q2 + MEX_POSITION);
sbcp_append_low_latency_out_register(MDV_ME_Q3 + MEX_POSITION);
sbcp_append_low_latency_out_register(MDV_LC_AXIS_1 + LCX_TORQUE);
sbcp_append_low_latency_out_register(MDV_LC_AXIS_2 + LCX_TORQUE);
sbcp_append_low_latency_out_register(MDV_LC_AXIS_3 + LCX_TORQUE);
}
int get_me1(){
return me_get_value(me1);
}
int get_me2(){
return me_get_value(me2);
}
void sbcp_calibrate_motors(unsigned int * b){
sbcp_error err ;
err = mdv_calibrate_motor(mdv1,
sbcp_reg_table[MDV_M1 + MX_LIMIT_POS_CW].u & 0x7fff,
sbcp_reg_table[MDV_M1 + MX_CALIBRATION_REGION_MIN].u,
sbcp_reg_table[MDV_M1 + MX_CALIBRATION_REGION_MAX].u,
sbcp_reg_table[MDV_M1 + MX_CALIBRATION_OFFSET].i,
&get_me1,
abs((long)me_get_gear_mult(me1)* ((long)ANGULAR_RESOLUTION ) / (long)me_get_gear_div(me1)),
(sbcp_reg_table[MDV_M1 + MX_LIMIT_POS_CW].u & 0x8000) >> 15);
if(err != MDV_ERR_NO_ERROR){
sbcp_prepare_r_packet(0,err);
sbcp_mark_r_packet_ready();
return;
}
err = mdv_calibrate_motor(mdv2,
sbcp_reg_table[MDV_M2 + MX_LIMIT_POS_CW].u & 0x0fff,
sbcp_reg_table[MDV_M2 + MX_CALIBRATION_REGION_MIN].u,
sbcp_reg_table[MDV_M2 + MX_CALIBRATION_REGION_MAX].u,
sbcp_reg_table[MDV_M2 + MX_CALIBRATION_OFFSET].i,
&get_me2,
abs((long)me_get_gear_mult(me2)* ((long)ANGULAR_RESOLUTION) / (long)me_get_gear_div(me2)),
(sbcp_reg_table[MDV_M2 + MX_LIMIT_POS_CW].u & 0x8000) >> 15);
sbcp_prepare_r_packet(0,err);
sbcp_mark_r_packet_ready();
}
void sbcp_uncalibrate_motors(){
mdv_uncalibrate_motor(mdv1);
mdv_uncalibrate_motor(mdv2);
sbcp_prepare_r_packet(0,SBCP_NO_ERROR);
sbcp_mark_r_packet_ready();
}
void init_sbcp_mdv_instructions(){
sbcp_add_instruction(SBCP_MDV_CALIBRATE_MOTORS,
&sbcp_calibrate_motors);
sbcp_add_instruction(SBCP_MDV_UNCALIBRATE_MOTORS,
&sbcp_uncalibrate_motors);
}
c4science · Help