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

oncilla-motordriver-fw/src/mdv_speed_pid.c
d387f8f54972load-bad-commit

Last Change
Raw File

mdv_speed_pid.c
View Options

#include "mdv_speed_pid.h"
#include "mdv_internal.h"
#include "config.h"
#include "misc_math.h"
#define SMALLEST_HIGH_RES_SPEED mdv_lo2hi_res_speed(1L)
void mdv_update_position(motordriver * mdv){
int max_accel;
// The desired position moves according to the speed and acceleration settings...
//if not moving, we skip :P
if (!mdv_flag(mdv,MDV_F_MOVING_ENABLE)) {
return;
}
// if no acceleration curve is used, then moving_speed is just the set_speed
// M->moving_speed = M->set_speed;
// => Acceleration curve computation
// compute when to switch off the set-speed in order to start descelerating such that zero speed is reached at the goal position
// maximum acceleration must not exceed the current moving speed (for correctly computing the speed auto-switch-off position)
max_accel = min(abs(mdv->moving_speed), (int)mdv->max_accel);
// auto-switching off the set_speed should only be done if the current speed has the same direction as the set_speed
if( (mdv->moving_speed > 0 && mdv->goal_speed > 0)
|| (mdv->moving_speed < 0 && mdv->goal_speed < 0)){
///\todo : find what these two means
long first_operand = abs(mdv->moving_pos - mdv->goal_pos) * (long) max_accel;
long second_operand = mdv_hi2lo_res_speed( (long) abs(mdv->moving_speed)
* ( (long) abs(mdv->moving_speed)
+ (long) max_accel -1 )) / 2 ;
if (first_operand < second_operand ) {
// we don't set the desired speed to zero, but to the minimum acceleration setting in order to guarantee convergence
if (mdv->goal_speed > 0){
mdv->goal_speed = min((int)mdv->max_accel, mdv->goal_speed);
} else {
mdv->goal_speed = - min((int)mdv->max_accel, - mdv->goal_speed);
}
}
}
// compute new speed according to the acceleration parameter
if (mdv->moving_speed != mdv->goal_speed) {
if (mdv->moving_speed < mdv->goal_speed) {
mdv->moving_speed += (int)mdv->max_accel;
if (mdv->moving_speed > mdv->goal_speed) {
mdv->moving_speed = mdv->goal_speed;
}
} else {
mdv->moving_speed -= (int)mdv->max_accel;
if (mdv->moving_speed < mdv->goal_speed) {
mdv->moving_speed = mdv->goal_speed;
}
}
}
// compute new desired position for the current step
mdv->moving_pos += mdv_hi2lo_res_speed(mdv->moving_speed);
// compensate for the bit errors due to the shifting action
mdv->resolution_error += mdv->moving_speed - mdv_lo2hi_res_speed(mdv_hi2lo_res_speed(mdv->moving_speed)) ;
if (mdv->resolution_error > SMALLEST_HIGH_RES_SPEED ) {
mdv->resolution_error -= SMALLEST_HIGH_RES_SPEED;
mdv->moving_pos += 1;
} else if( mdv->resolution_error < - SMALLEST_HIGH_RES_SPEED) {
mdv->resolution_error += SMALLEST_HIGH_RES_SPEED;
mdv->moving_pos -= 1;
}
// check if the goal position is reached
if (((mdv->goal_speed > 0) && (mdv->moving_pos >= mdv->goal_pos)) || ((mdv->goal_speed < 0) && (mdv->moving_pos <= mdv->goal_pos)) ) {
mdv->moving_pos = mdv->goal_pos;
mdv->moving_speed = 0;
mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
}
}
void mdv_compute_and_apply_output(motordriver * mdv){
long desired_speed;
long p_error, d_error;
long pid_output;
unsigned int speed_output;
unsigned int scaled_speed_output;
long i_max;
long required_trq;
// ***************** Speed control section: velocity error *****************
desired_speed = mdv->moving_pos - mdv->pos_cnt;
// compute new PID output
p_error = (long)mdv->current_speed - desired_speed;
// only integrate action when not in complinat mode
if (mdv_flag(mdv,MDV_F_UPDATE_I_ERROR)){
mdv->i_error += p_error;
}
d_error = p_error - mdv->prev_p_error;
// if (M1.update_i_error) d_error = p_error - M1.prev_p_error; else d_error = 0;
pid_output = ((p_error*(long) (mdv->params.p_gain)) + (mdv->i_error*(long)mdv->params.i_gain) + (d_error*(long)mdv->params.d_gain))/(long)1024;
mdv->prev_p_error = p_error;
// anti-windup
i_max = ((long)(MAX_PWM-1)*(long)1024)/(mdv->params.i_gain*2);
mdv->i_error = clamp_absolute(mdv->i_error,i_max);
// clamp output
speed_output = min(abs(pid_output),(long)(MAX_PWM-1));
// set pwm output
//(MODE1==FAST_DECAY) ? (_PWM) + MAX_DUTY/2 : (_PWM)*2);
pwm_set_value(mdv->pins.speed,
mdv_in_fast_decay_mode(mdv) ? speed_output + MAX_DUTY / 2
: speed_output * 2 );
// ***************** Output torque control section *****************
// torque as feeled by the leg = spring force - spring damping + rotor inertia compensation
long output_spring = (((desired_speed)*(long)mdv->params.stiffness)/512) ;
long output_damping = - (((long)(mdv->current_speed - mdv_hi2lo_res_speed(mdv->moving_speed)) * (long)mdv->params.damping)/64);
mdv->output_trq = output_spring + output_damping;
// Add the spring preload term
if (desired_speed > 0){
mdv->output_trq += mdv->params.preload;
} else {
mdv->output_trq -= mdv->params.preload;
}
// required torque on the motor is the requested torque + a compensation term for the rotor inertia
///\todo remove hardcoded 64.
required_trq = mdv->output_trq + (long)mdv->current_accel* (long) MDV_INERTIA_COMP/64;
// scale speed to torque dimensions
scaled_speed_output = (long)((long)speed_output*MAX_DUTY)/(long)MAX_PWM;
// Set direction of torque, depending on which factor is smallest
// if ((long)_ABS(required_trq) < (long)(speed_output*MAX_DUTY)/MAX_PWM) {
if ((unsigned int)abs(mdv->params.preload) < scaled_speed_output) {
mdv_clear_flag(mdv, MDV_F_UPDATE_I_ERROR);
if (required_trq>0) {
gpio_clear(mdv->pins.dir);
} else {
gpio_set(mdv->pins.dir);
}
///\todo remove this hardcoded value
if (mdv->params.stiffness > 500) { // problematic switching between fast and slow decay for very stiff spring settings
pwm_set_value(mdv->pins.torque, mdv_trq_to_pwm(mdv,abs(required_trq), 1));
} else {
pwm_set_value(mdv->pins.torque, mdv_trq_to_pwm(mdv,abs(required_trq), 0));
// LED_TX_ENABLE;
}
}
else {
mdv_set_flag(mdv,MDV_F_UPDATE_I_ERROR);
// if (speed_output > 2)
if (pid_output<0) {
gpio_clear(mdv->pins.dir);
} else {
gpio_set(mdv->pins.dir);
}
// force fast decay mode
// if (scaled_speed_output*4 < (unsigned int)_ABS(M1.compliance_preload))
// PWM_TRQ1 = M1_trq_to_pwm((long)scaled_speed_output*4, TRUE);// (_ABS(required_trq)*(long)speed_output)/50, TRUE);
// else
// PWM_TRQ1 = M1_trq_to_pwm((long)scaled_speed_output, TRUE);
pwm_set_value(mdv->pins.torque, mdv_trq_to_pwm(mdv,abs(required_trq), 1));
// LED_TX_DISABLE;
}
}
unsigned int mdv_trq_to_pwm(motordriver * mdv, long desired_trq, uint8_t force_fast_decay) {
unsigned int pwm_out;
desired_trq = (desired_trq*4096)/MAX_DUTY;
// clamp desired_trq within safe range
if (desired_trq > 8192) {
desired_trq = 8192;
}
// desired torque is a 12-bit value
// translate to 12-bit pwm-output using a piece-wise linear function
// this function compensates for the non-linear pwm to torque relation
if (desired_trq < 2) pwm_out = ((unsigned int)desired_trq)*215;
else if (desired_trq < 40) pwm_out = ((unsigned int)desired_trq - 2)*7/2 + 430;
else if (desired_trq < 86) pwm_out = ((unsigned int)desired_trq -40)*3/2 + 563;
else pwm_out = ((unsigned int)desired_trq -86)*7/8 + 632;
// full pwm scale is now 12-bit, convert to pwm-range
pwm_out = (unsigned int)(((unsigned long)pwm_out*(unsigned long)MAX_DUTY)/TORQUE_RESOLUTION);
// saturate at maximum torque
if ((mdv->max_trq > ((MAX_DUTY * MDV_SLOWFAST_THRES)/256)) && (!force_fast_decay)) {
if (pwm_out > mdv->max_trq + MDV_SLOWFAST_COMP) pwm_out = mdv->max_trq + MDV_SLOWFAST_COMP;
}
else { // motor remains in the fast decay regime
if (pwm_out > mdv->max_trq) pwm_out = mdv->max_trq;
}
if (force_fast_decay) {
mdv_set_fast_decay(mdv);
// LED_TX_DISABLE;
}
else {
// choose appropriate decay mode
if (pwm_out > (unsigned int)((MAX_DUTY*MDV_SLOWFAST_THRES)/256)) {
mdv_set_slow_decay(mdv);
// LED_TX_ENABLE;
}
else if (pwm_out < (unsigned int)((MAX_DUTY*(MDV_SLOWFAST_THRES-10))/256)) { // small hysteresis
mdv_set_fast_decay(mdv);
// LED_TX_DISABLE;
}
}
// compensate for the slightly higher efficiency of the slow decay mode
if ( mdv_in_slow_decay_mode(mdv) ) {
pwm_out -= (unsigned int)MDV_SLOWFAST_COMP;
}
return pwm_out;
}
void mdv_speed_pid_with_compliance_process(motordriver * mdv){
mdv_update_position(mdv);
mdv_compute_and_apply_output(mdv);
}
c4science · Help