Page MenuHomec4science
Files F73012854

mdv_control_modes.c
No OneTemporary
Actions

File Metadata

Created
Thu, Jul 18, 01:51

mdv_control_modes.c
View Options

#include "mdv_control_modes.h"
#include "mdv_speed_pid.h"
#include "misc_math.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)){
int abs_speed = abs(mdv->goal_speed > 0);
//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)){
// compute new desired speed, clamp within the range of integers
int 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_speed_pid_with_compliance_process(mdv);
}
void mdv_smooth_position_init(motordriver * mdv){
gpio_set(mdv->pins.coast);
gpio_set(mdv->pins.brake);
}
#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);
}

Event Timeline

c4science · Help