Homec4science
Diffusion Oncilla Motordriver Firmware (load-cell-good) Browse

oncilla-motordriver-fw/src/motordriver.c
d387f8f54972load-cell-good

Last Change
Raw File

motordriver.c
View Options

#include "motordriver.h"
#include "mdv_internal.h"
#include "timer.h"
#include "config.h"
#include "sbcp_mdv.h"
motordriver MDV_[2];
motordriver * mdv1 = &(MDV_[0]);
motordriver * mdv2 = &(MDV_[1]);
gpio mdv_master_reset;
void init_motordrivers(mdv_pin* mdv1_pin, mdv_pin* mdv2_pin){
init_motordriver(mdv1,mdv1_pin, MDV_M1);
init_motordriver(mdv2,mdv2_pin, MDV_M2);
}
void init_motordriver(motordriver * mdv, mdv_pin * pins, sbcp_reg_address address_start){
mdv->pins = *pins;
/*set coast mode as default and init it*/
mdv->sc.mode = MDV_COAST;
mdv->address_start = address_start;
mdv_coast_init(mdv);
mdv->previous_cnt = 0;
mdv->pos_cnt = 0;
mdv->goal_pos = 0;
mdv->goal_speed = 0;
mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE | MDV_F_UPDATE_I_ERROR);
mdv->params.p_gain = 0;
mdv->params.i_gain = 0;
mdv->params.d_gain = 0;
mdv->i_error = 0;
mdv->prev_p_error = 0;
mdv->params.stiffness = 0;
mdv->params.damping = 0;
mdv->params.preload = 0;
mdv->max_trq = 0;
mdv->output_trq = 0;
mdv->pos_limit = 0;
mdv->max_speed = 0;
mdv->max_accel = 0;
}
void mdv_reset_count(motordriver * mdv){
mdv->previous_cnt = 0;
mdv->pos_cnt = 0;
}
void mdv_update_sbcp_register(motordriver * mdv){
if(mdv_flag(mdv,MDV_F_MOVING_ENABLE)){
mdv->sc.status |= MDV_MOVING;
} else {
mdv->sc.status &= ~(MDV_MOVING);
}
sbcp_mdv_reg(mdv,MX_STATUS_AND_CONTROL_MODE) = mdv->sc.reg_val;
sbcp_mdv_reg(mdv, MX_TARGETED_POSITION).i = mdv_i2e_position(mdv,mdv->moving_pos);
sbcp_mdv_reg(mdv , MX_PRESENT_POSITION).i = mdv_i2e_position(mdv, mdv->pos_cnt);
sbcp_mdv_reg(mdv , MX_TARGETED_SPEED).i = mdv_i2e_speed(mdv, mdv->moving_speed);
sbcp_mdv_reg(mdv , MX_PRESENT_SPEED).i = mdv_i2e_speed(mdv,mdv_lo2hi_res_speed(mdv->current_speed));
sbcp_mdv_reg(mdv , MX_TARGETED_TORQUE).i = mdv_i2e_torque(mdv, mdv->output_trq);
sbcp_mark_new_low_latency_data_available();
};
void mdv_configure_from_sbcp_register(motordriver* mdv){
mdv->goal_pos = mdv_e2i_position(mdv,sbcp_mdv_reg(mdv,MX_GOAL_POSITION).i);
mdv->goal_speed = mdv_e2i_speed(mdv,sbcp_mdv_reg(mdv,MX_GOAL_SPEED).i);
mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
mdv->params.p_gain = sbcp_mdv_reg(mdv,MX_PID_PGAIN).u;
mdv->params.i_gain = sbcp_mdv_reg(mdv,MX_PID_IGAIN).u;
mdv->params.d_gain = sbcp_mdv_reg(mdv,MX_PID_DGAIN).u;
mdv->i_error = 0;
mdv->prev_p_error = 0;
mdv_clear_flag(mdv,MDV_F_UPDATE_I_ERROR);
mdv->params.stiffness = sbcp_mdv_reg(mdv,MX_COMPLIANCE_STIFFNESS).u;
mdv->params.damping = sbcp_mdv_reg(mdv,MX_COMPLIANCE_DAMPING).u;
mdv->params.preload = sbcp_mdv_reg(mdv,MX_COMPLIANCE_PRELOAD).u;
mdv->max_trq = mdv_e2i_torque(mdv,sbcp_mdv_reg(mdv,MX_MAX_TORQUE).i);
mdv->max_speed = mdv_e2i_speed(mdv,sbcp_mdv_reg(mdv,MX_MAX_SPEED).i);
mdv->max_accel = mdv_e2i_accel(mdv,sbcp_mdv_reg(mdv,MX_MAX_ACCELERATION).i);
//NO POS SETTINGS, WILL BE DONE IN CALIBRATION
}
void mdv_process(motordriver * mdv){
(*(mdv_ctrl[mdv->sc.mode].process))(mdv);
mdv_update_sbcp_register(mdv);
}
mdv_error mdv_set_control_mode(motordriver * mdv, int c){
if(c >= MDV_NUMBER_OF_CONTROL_MODE){
return MDV_ERR_UNKNOWN_CONTROL_MODE;
}
if(c == mdv->sc.mode) {
return;
}
mdv_cleanup(mdv);
mdv->sc.mode = c;
(*(mdv_ctrl[c].init))(mdv);
}
#define mdv_update_position_and_speed(mdv,pos_cnt_val) do{\
int _MC_tmp_speed = pos_cnt_val - mdv->previous_cnt;\
mdv->current_accel = _MC_tmp_speed - mdv->current_speed;\
mdv->current_speed = _MC_tmp_speed;\
mdv->previous_cnt = pos_cnt_val;\
mdv->pos_cnt += mdv->current_speed;\
}while(0)
/* Timer 1 is used as system timer */
void __attribute__((__interrupt__, no_auto_psv)) _T1Interrupt(void) {
systime ++;
// timing critical part of motor control
mdv_update_position_and_speed(mdv1,POS1CNT);
mdv_update_position_and_speed(mdv2,POS2CNT);
IFS0bits.T1IF = 0; // Clear Timer 1 Interrupt Flag
}
int mdv_get_status_and_control_mode(motordriver * mdv) {
return mdv->sc.reg_val.i;
}
mdv_error mdv_set_goal_position(motordriver * mdv, int position){
mdv->goal_pos = mdv_e2i_position(mdv,position);
mdv_set_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
}
mdv_error mdv_set_goal_speed(motordriver * mdv , int speed){
mdv->goal_speed = mdv_e2i_speed(mdv,speed);
mdv_set_flag(mdv,MDV_F_NEW_SPEED_AVAILABLE);
}
//mdv_error mdv_set_goal_torque(motordriver * mdv, mdv_torque_t torque);
int mdv_get_goal_position(motordriver * mdv){
return mdv_i2e_position(mdv,mdv->goal_pos);
}
int mdv_get_goal_speed(motordriver * mdv){
return mdv_i2e_speed(mdv,mdv->goal_speed);
}
//mdv_torque_t mdv_get_goal_torque(motordriver * mdv);
int mdv_get_position(motordriver * mdv){
return mdv_i2e_position(mdv,mdv->pos_cnt);
}
int mdv_get_speed(motordriver * mdv){
return mdv_i2e_speed(mdv,mdv_lo2hi_res_speed(mdv->current_speed));
}
int mdv_get_targeted_torque(motordriver * mdv){
return mdv_i2e_torque(mdv,mdv->output_trq);
}
int mdv_get_current(motordriver * mdv){
return 0;
}
int mdv_get_virtual_goal_position(motordriver * mdv){
return mdv_i2e_position(mdv,mdv->moving_pos);
}
int mdv_get_virtual_goal_speed(motordriver * mdv){
return mdv_i2e_speed(mdv,mdv->moving_speed);
}
mdv_parameters * mdv_get_parameters(motordriver * mdv){
return &(mdv->params);
}
int mdv_get_maximal_acceleration(motordriver * mdv){
return mdv_i2e_accel(mdv,mdv->max_accel);
}
int mdv_get_maximal_speed(motordriver * mdv){
return mdv_i2e_speed(mdv,mdv->max_speed);
}
int mdv_get_maximal_torque(motordriver * mdv){
return mdv_i2e_torque(mdv,mdv->max_trq);
}
mdv_error mdv_set_maximal_acceleration(motordriver * mdv, int value){
mdv->max_accel = mdv_e2i_accel(mdv,value);
}
mdv_error mdv_set_maximal_speed(motordriver * mdv, int value){
mdv->max_speed = mdv_e2i_speed(mdv,value);
}
mdv_error mdv_set_maximal_torque(motordriver * mdv, int value){
mdv->max_trq = mdv_e2i_torque(mdv,value);
}
mdv_error mdv_set_gear_ratio(motordriver * mdv, int gear_mult, int gear_div){
mdv->gear_mult = GEAR_MULT_COEFF * gear_mult;
mdv->gear_div = GEAR_DIV_COEFF * gear_div;
}
int mdv_get_gear_mult(motordriver * mdv){
return mdv->gear_mult / GEAR_MULT_COEFF;
}
int mdv_get_gear_div(motordriver * mdv){
return mdv->gear_div / GEAR_DIV_COEFF;
}
mdv_error mdv_set_limit_position(motordriver * mdv, int limit_pos){
return mdv->pos_limit = mdv_e2i_position(mdv,abs(limit_pos));
}
int mdv_get_limit_position(motordriver * mdv){
return mdv_i2e_position(mdv,mdv->pos_limit);
}
c4science · Help