Homec4science
Diffusion Oncilla Motordriver Firmware (load-cell) Browse

oncilla-motordriver-fw/src/mdv_control_modes.c
a947e291722eload-cell

Last Change
Raw File

mdv_control_modes.c
View Options

#include "mdv_control_modes.h"
#include "mdv_speed_pid.h"
#include "misc_math.h"
#include "cos_table.h"
#include "sbcp_mdv.h"
void mdv_coast_init(motordriver * mdv){
gpio_set(mdv->pins.brake);
gpio_clear(mdv->pins.coast);
}
void mdv_brake_init(motordriver * mdv){
gpio_set(mdv->pins.coast);
gpio_clear(mdv->pins.brake);
}
void mdv_position_process(motordriver * mdv){
if(mdv_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE)){
mdv->goal_pos = mdv_e2i_position(mdv,mdv->user_goal_pos);
mdv->goal_pos = clamp(mdv->goal_pos,0,mdv->pos_limit);
mdv->goal_speed = clamp_absolute(mdv_lo2hi_res_speed(mdv->goal_pos - mdv->moving_pos) / mdv->params.smooth_interval ,mdv->max_speed );
int abs_speed = abs(mdv->goal_speed);
//mdv->goal_pos = mdv->moving_pos + (mdv->goal_pos - mdv->moving_pos)*(long)(SMOOTH_POSITION_OVERSHOOT + 1 );
//disable moving during play with parameter
mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
//checks that the set speed is correct !!!!!! users could be morrons
// and move motor if needed
if(mdv->goal_pos != mdv-> moving_pos ) {
if (mdv->goal_pos > mdv-> moving_pos) {
mdv->goal_speed = abs_speed;
} else {
mdv->goal_speed = -abs_speed;
}
mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
}
mdv_clear_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
}
//now process normally
mdv_speed_pid_with_compliance_process(mdv);
}
void mdv_position_init(motordriver * mdv){
gpio_set(mdv->pins.coast);
gpio_set(mdv->pins.brake);
}
void mdv_velocity_process(motordriver * mdv){
if(mdv_flag(mdv,MDV_F_NEW_SPEED_AVAILABLE)){
mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
//set flag if needed, and set the right goal position if user
// modify it
if(mdv->goal_speed > 0){
mdv->goal_pos = mdv->pos_limit;
if(mdv->moving_pos < mdv->goal_pos ){
mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
}
} else if (mdv->goal_speed < 0){
mdv->goal_pos = 0;
if(mdv->moving_pos > 0 ){
mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
}
}
mdv_clear_flag(mdv, MDV_F_NEW_SPEED_AVAILABLE);
}
mdv_speed_pid_with_compliance_process(mdv);
}
void mdv_velocity_init(motordriver * mdv){
gpio_set(mdv->pins.coast);
gpio_set(mdv->pins.brake);
}
void mdv_smooth_position_process(motordriver * mdv){
/// \todo implement safety where if no data is available for an
/// amount of time, we switch to COAST mode
if(mdv_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE))
{
pushIntoBuffer(&mdv->commandsBuffer, mdv->user_goal_pos);
}
// starts the logging of commands whend half the capacity is reached.
// can be changed to 3/4 or any other fraction of the capacity.
// this fraction can also be precomputed for more efficiency
if(!mdv->startSendOrders && mdv->commandsBuffer.numVals >= BUFFER_CAPACITY/2)
{
mdv->startSendOrders = 1;
}
if(!mdv->startSendOrders)
{
mdv_speed_pid_with_compliance_process(mdv);
return;
}
if(mdv->timeCounter < mdv->params.smooth_interval)
{
++(mdv->timeCounter);
mdv_speed_pid_with_compliance_process(mdv);
return;
}
//actually sends the orders if the buffer is not empty.
if(mdv->commandsBuffer.numVals > 0)
{
//gets the goal position from the buffer.
mdv->goal_pos = mdv_e2i_position(mdv,popFromBuffer(&mdv->commandsBuffer));
// compute new desired speed, clamp within the range of integers
long new_speed = clamp_absolute(mdv_lo2hi_res_speed(mdv->goal_pos - mdv->moving_pos) / mdv->params.smooth_interval ,
mdv->max_speed );
mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
mdv->goal_speed = new_speed;
mdv->goal_pos = mdv->moving_pos + (mdv->goal_pos - mdv->moving_pos)*(long)(SMOOTH_POSITION_OVERSHOOT + 1 );
// limit position
mdv->goal_pos = clamp(mdv->goal_pos,0,mdv->pos_limit);
// update goal position
if (mdv->goal_pos != mdv->moving_pos) {
// enable new movement
mdv_set_flag(mdv, MDV_F_MOVING_ENABLE);
}
mdv_clear_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
}
mdv->timeCounter = 0;
mdv_speed_pid_with_compliance_process(mdv);
}
void mdv_smooth_position_init(motordriver * mdv){
gpio_set(mdv->pins.coast);
gpio_set(mdv->pins.brake);
initBuffer(&mdv->commandsBuffer);
mdv->timeCounter = 0;
mdv->startSendOrders = 0;
}
#define MDV_RUNTIME_CALIBRATION_SUCCESSFUL 0xffff
#define mdv_rc_in_calibration_region(min,max,pos) ( (min) < (max) ? ( ( (min) <= (pos) ) && ( (pos) <= (max) ) ) : ( ( (pos) <= (max) ) || ( (min) <= (pos) ) ) )
/**
* returns 0xffff if calibration is over, otherwise a 12 bits unsigned
* int being the real position.
*/
int mdv_rc_check_and_calibrate(motordriver * mdv){
/* readout the real position */
int pos = (*(mdv->calib.pos_evaluator))();
if(mdv_rc_in_calibration_region(mdv->calib.min_region,mdv->calib.max_region,pos)){
//we check how much far away we are from the min position, in increments, and we map it to
//the 0 position, and add the offset.
long actual_position = (long)pos - (long)mdv->calib.min_region + (long)mdv->calib.offset;
if(actual_position < 0 ){
actual_position += mdv->calib.pos_evaluator_max_value;
}
actual_position = mdv_e2i_position(mdv,actual_position);
mdv->pos_limit = mdv_e2i_position(mdv,mdv->calib.pos_limit);
mdv_set_pos_count(mdv,actual_position);
return MDV_RUNTIME_CALIBRATION_SUCCESSFUL;
}
return pos;
}
void mdv_runtime_calibration_init(motordriver * mdv){
gpio_set(mdv->pins.brake);
gpio_clear(mdv->pins.coast);
//this sets an update of 10 ms, and make sure that the next process will
//compute an update.
mdv->calib.update_period = 10;
mdv->calib.ellapsed_ts = 10;
}
void mdv_runtime_calibration_process(motordriver * mdv){
//first we check if we can calibrate
int pos = mdv_rc_check_and_calibrate(mdv);
if (pos == MDV_RUNTIME_CALIBRATION_SUCCESSFUL ){
//yes, and calibrated , so go back to COAST
mdv->sc.status |= MDV_RUNTIME_CALIBRATED;
mdv_set_control_mode_private(mdv,MDV_M_COAST);
return;
}
//now check if we need an update of the target (every mdv->calib.update_period)
if( (++(mdv->calib.ellapsed_ts)) <= mdv->calib.update_period ) {
//no update needed, just process PID and return
mdv_speed_pid_with_compliance_process(mdv);
return;
}
//we need to compute a new update update.
mdv->calib.ellapsed_ts = 0;
int mini = mdv->calib.min_region;
int maxi = mdv->calib.max_region;
long target_position;
/* first put the actual absolute error in target position */
// the boundary are clean
if (mini < maxi){
target_position = (mini + maxi) / 2 - pos ;
} else { //max has an overshoot, we should take it into account
target_position = (mini + maxi + mdv->calib.pos_evaluator_max_value + 1) / 2 - pos;
}
//check for the shortest path !!!
if(abs(target_position) > ANGULAR_RESOLUTION / 2){
//we should make more than half a turn, so we go in the other direction !!!
//case target > 0 remove ANGULAR_RESOLUTION
//case target < 0 add ANGULAR_RESOLUTION
target_position += ANGULAR_RESOLUTION * ((target_position > 0) ? -1 : 1);
}
//don't forget to put this in the right dimension
target_position = mdv_e2i_position(mdv,target_position);
//IGNORE POSITION_LIMIT !!!!!
//we now now our next target
mdv->goal_pos = target_position + mdv->pos_cnt ;
if(mdv->goal_pos != mdv->pos_cnt){
mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
} else {
mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
}
mdv->goal_speed = mdv_lo2hi_res_speed((mdv->goal_pos - mdv->pos_cnt) / mdv->calib.update_period);
//prevent the speed to exceed 2 turn per minute of the effector i.e. ANGULAR_RESOLUTION / 30.
long max_speed = mdv_e2i_speed(mdv, ANGULAR_RESOLUTION / 30);
mdv->goal_speed = clamp_absolute(mdv->goal_speed,max_speed);
//now process the speed pid
mdv_speed_pid_with_compliance_process(mdv);
//activate move now
gpio_set(mdv->pins.coast);
}
void mdv_sine_generator_init(motordriver * mdv){
mdv->sine.t = COS_TABLE_SIZE / 4 - 1;
mdv->sine.t_residue = 0;
mdv->sine.iter = 4;
mdv_set_smooth_interval(mdv,5);
mdv_smooth_position_init(mdv);
}
void mdv_sine_generator_process(motordriver * mdv){
sbcp_reg_address r = mdv == mdv1 ? MDV_SINE_1 : MDV_SINE_2;
int16_t frequency = sbcp_reg_table[r + SINE_FREQUENCY].i;
mdv->sine.t += frequency / 100;
mdv->sine.t_residue += frequency % 100;
if(mdv->sine.t_residue > 100){
mdv->sine.t += 1;
mdv->sine.t_residue -= 100;
}
if(mdv->sine.t > COS_TABLE_SIZE){
mdv->sine.t -= COS_TABLE_SIZE;
}
mdv->sine.iter += 1;
//seems strange but we clear the flag, only us could move the motor
//if we need.
mdv_clear_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
if(mdv->sine.iter % 5 == 0){
mdv->sine.iter = 0;
int16_t amp = sbcp_reg_table[r + SINE_AMPLITUDE].i;
int16_t offset = sbcp_reg_table[r + SINE_OFFSET].i;
long pos = (long)amp * (long)cos_table[mdv->sine.t] / (long)2048 + (long)offset ;
mdv_set_goal_position(mdv, pos & 0xffff);
}
mdv_smooth_position_process(mdv);
}
c4science · Help