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

oncilla-motordriver-fw/src/motorcontrol.c
df700679f13cload-cell-good

Last Change
Raw File

motorcontrol.c
View Options

#include <p33FJ128MC804.h>
#include "motorcontrol.h"
#include "misc.h"
#include "pindefs.h"
#include "config.h"
#include "timer.h"
#include "error.h"
#include "control_table.h"
// smooth position control mode: allowed jitter between two speed update commands
// expressed in times the expected update period
// jitter of 0 means now jitter allowed, jitter of 2 means 3 times the normal update rate is allowed
#define MAX_JITTER 3
// create 2 motor structures
motor M1, M2;
///////////////////////////////////////////////////////////////////////////////
// timing critical part
// estimate current speed and acceleration of motor and update the position counter
/*void update_position_and_speed(motor *M, int pos_cnt) {
int tmp_speed;
//calculate speed without resetting POSxCNT registers -> eliminates incremental errors
tmp_speed = pos_cnt - M->previous_cnt;
M->current_accel = tmp_speed - M->current_speed;
M->current_speed = tmp_speed;
M->previous_cnt = pos_cnt;
M->pos_cnt += M->current_speed;
}*/
///////////////////////////////////////////////////////////////////////////////
void init_motor(motor *M) {
// qei position parameters
M->previous_cnt = 0;
M->pos_cnt = 0;
// move controlling parameters
M->goal_pos = 0;
M->set_speed = 0;
// internally controlled variables
M->moving_pos = 0;
M->moving_speed = 0;
M->current_speed = 0;
M->current_accel = 0;
M->resolution_error = 0;
M->moving_en = 0;
// pid control in the "stiff" region
M->speed_pgain = 0;
M->speed_igain = 0;
M->speed_dgain = 0;
M->i_error = 0;
M->prev_p_error = 0;
M->update_i_error = FALSE;
// compliant mode, "damped spring" dynamics
M->compliance_preload = 0;
M->compliance_gain = 0;
M->compliance_damping = 0;
M->max_trq = 0;
// control output parameters
M->output_trq = 0;
// motor limits
M->pos_limit_CW = 0;
M->pos_limit_CCW = 0;
M->max_speed = 0;
M->max_accel = 0;
}
void M1_load_control_table_settings(void) {
// move controlling parameters
M1.goal_pos = M1_EXT_POS_2_INT_POS((long)control_table[A_MDRV_HIP_GOAL_POSITION].i);
M1.set_speed = M1_EXT_SPEED_2_INT_SPEED((long)control_table[A_MDRV_HIP_GOAL_SPEED].i);
M1.moving_en = 0; // disable moving
// pid control in the "stiff" region
M1.speed_pgain = control_table[A_MDRV_HIP_PID_PGAIN].i;
M1.speed_igain = control_table[A_MDRV_HIP_PID_IGAIN].i;
M1.speed_dgain = control_table[A_MDRV_HIP_PID_DGAIN].i;
// reset internal PID variables
M1.i_error = 0;
M1.prev_p_error = 0;
M1.update_i_error = FALSE;
// compliant mode, "damped spring" dynamics
M1.compliance_preload = control_table[A_MDRV_HIP_SPRING_PRELOAD].i;
M1.compliance_gain = M1_EXT_TRQ_2_INT_TRQ(control_table[A_MDRV_HIP_SPRING_FORCE].i);
M1.compliance_damping = control_table[A_MDRV_HIP_SPRING_DAMPING].i;
M1.max_trq = (unsigned int)control_table[A_MDRV_HIP_MAX_TORQUE].i;
// motor limits
// M1.pos_limit_CW = M1_EXT_POS_2_INT_POS((long)control_table[A_MDRV_HIP_CW_ANGLE_LIMIT].i);
// M1.pos_limit_CCW = M1_EXT_POS_2_INT_POS((long)control_table[A_MDRV_HIP_CCW_ANGLE_LIMIT].i);
M1.max_speed = (unsigned int)M1_MAX_SPEED_CNT;
M1.max_accel = M1_EXT_ACCEL_2_INT_ACCEL(control_table[A_MDRV_HIP_MAX_ACCELERATION].i);
}
void M2_load_control_table_settings(void) {
// move controlling parameters
M2.goal_pos = M2_EXT_POS_2_INT_POS((long)control_table[A_MDRV_KNEE_GOAL_POSITION].i);
M2.set_speed = M2_EXT_SPEED_2_INT_SPEED((long)control_table[A_MDRV_KNEE_GOAL_SPEED].i);
M2.moving_en = 0; // disable moving
// pid control in the "stiff" region
M2.speed_pgain = control_table[A_MDRV_KNEE_PID_PGAIN].i;
M2.speed_igain = control_table[A_MDRV_KNEE_PID_IGAIN].i;
M2.speed_dgain = control_table[A_MDRV_KNEE_PID_DGAIN].i;
// reset internal PID variables
M2.i_error = 0;
M2.prev_p_error = 0;
M2.update_i_error = FALSE;
// compliant mode, "damped spring" dynamics
M2.compliance_preload = control_table[A_MDRV_KNEE_SPRING_PRELOAD].i;
M2.compliance_gain = M2_EXT_TRQ_2_INT_TRQ(control_table[A_MDRV_KNEE_SPRING_FORCE].i);
M2.compliance_damping = control_table[A_MDRV_KNEE_SPRING_DAMPING].i;
M2.max_trq = (unsigned int)control_table[A_MDRV_KNEE_MAX_TORQUE].i;
// motor limits
// M2.pos_limit_CW = M2_EXT_POS_2_INT_POS((long)control_table[A_MDRV_KNEE_CW_ANGLE_LIMIT].i);
// M2.pos_limit_CCW = M2_EXT_POS_2_INT_POS((long)control_table[A_MDRV_KNEE_CCW_ANGLE_LIMIT].i);
M2.max_speed = (unsigned int)M2_MAX_SPEED_CNT;
M2.max_accel = M2_EXT_ACCEL_2_INT_ACCEL(control_table[A_MDRV_KNEE_MAX_ACCELERATION].i);
}
uint8 set_motor_limits(motor *M, long pos_limit_CW, long pos_limit_CCW, unsigned int max_speed) {
if (pos_limit_CW < pos_limit_CCW)
return MOTOR_LIMIT_SETTING_ERROR;
M->pos_limit_CW = pos_limit_CW;
M->pos_limit_CCW = pos_limit_CCW;
M->max_speed = max_speed;
return NO_ERROR;
}
// This mode has the goal to reach a certain position at the desired speed
// position in number of qei pulses, speed in pulses per interrupt-cycle
uint8 motor_position_control(motor *M, long new_pos, int new_speed) {
uint8 return_value = NO_ERROR;
int abs_new_speed = _ABS(new_speed);
// limit speed to maximum allowed/possible speed
if (abs_new_speed > (int)M->max_speed) {
abs_new_speed = (int)M->max_speed;
return_value = MOTOR_SPEED_OUT_OF_RANGE;
}
// limit position
if (new_pos > M->pos_limit_CW) {
new_pos = M->pos_limit_CW;
return_value = MOTOR_POSITION_OUT_OF_RANGE;
}
else if (new_pos < M->pos_limit_CCW) {
new_pos = M->pos_limit_CCW;
return_value = MOTOR_POSITION_OUT_OF_RANGE;
}
// disable movement during setting the parameters
M->moving_en = 0;
M->goal_pos = new_pos;
// update goal position
if (new_pos != M->moving_pos) {
// update speed settings
if (new_pos > M->moving_pos) {
// positive direction of movement
M->set_speed = abs_new_speed;
}
else {
// negative direction of movement
M->set_speed = -abs_new_speed;
}
// enable new movement
M->moving_en = 1;
}
return return_value;
}
// This mode computes motor speed based on the position that should be reached in the desired period of time
// Note that the goal position in this mode is the position limit
// (if the goal position would be the next position, the controller would decelerate towards this position)
// position in number of qei pulses, period time in number of system timer cycles
uint8 motor_speed_control(motor *M, int speed) {
uint8 return_value = NO_ERROR;
// limit speed to maximum allowed/possible speed
if (speed > M->max_speed) {
speed = M->max_speed;
return_value = MOTOR_SPEED_OUT_OF_RANGE;
} else if (speed < -M->max_speed) {
speed = -M->max_speed;
return_value = MOTOR_SPEED_OUT_OF_RANGE;
}
// disable movement during setting the parameters
M->moving_en = 0;
M->set_speed = speed;
// check the direction of movement and set the desired position limit
if (M->set_speed > 0) {
M->goal_pos = M->pos_limit_CW;
if (M->moving_pos < M->goal_pos) M->moving_en = 1;
}
else {
M->goal_pos = M->pos_limit_CCW;
if (M->moving_pos > M->goal_pos) M->moving_en = 1;
}
// enable new movement
// M->moving_en = 1;
// Update control table
return return_value;
}
// This mode computes motor speed based on the position that should be reached in the desired period of time
// Note that the goal position in this mode is the position limit
// (if the goal position would be the next position, the controller would decelerate towards this position)
// position in number of qei pulses, period time in number of system timer cycles
uint8 motor_smooth_position_control(motor *M, long next_pos, long period) {
uint8 return_value = NO_ERROR;
int new_speed;
// compute new desired speed, clamp within the range of integers
new_speed = (int)_MAX(-32768, _MIN(32767, (next_pos - M->moving_pos)*(long)(1<<SPEED_SHIFT)/period));
// limit speed to maximum allowed/possible speed
if (new_speed > M->max_speed) {
new_speed = M->max_speed;
return_value = MOTOR_SPEED_OUT_OF_RANGE;
} else if (new_speed < -M->max_speed) {
new_speed = -M->max_speed;
return_value = MOTOR_SPEED_OUT_OF_RANGE;
}
// disable movement during setting the parameters
M->moving_en = 0;
M->set_speed = new_speed;
M->goal_pos = M->moving_pos + (next_pos - M->moving_pos)*(long)(MAX_JITTER+1);
// limit position
if (M->goal_pos > M->pos_limit_CW) {
M->goal_pos = M->pos_limit_CW;
// return_value = MOTOR_POSITION_OUT_OF_RANGE;
}
else if (M->goal_pos < M->pos_limit_CCW) {
M->goal_pos = M->pos_limit_CCW;
// return_value = MOTOR_POSITION_OUT_OF_RANGE;
}
// update goal position
if (M->goal_pos != M->moving_pos) {
// enable new movement
M->moving_en = 1;
}
// Update control table
return return_value;
}
///////////////////////////////////////////////////////////////////////////////
void motorcontrol_main() {
// LED_TX_ENABLE;
update_moving_position(&M1);
update_moving_position(&M2);
M1_compute_output();
M2_compute_output();
motorcontrol_main_update_control_table();
// LED_TX_DISABLE;
}
void motorcontrol_main_update_control_table(void) {
// update control table parameters that may have changed after executing the main control loop
if (!M1.moving_en)
// clear moving status flag
control_table_AND_mask(A_MDRV_HIP_MOTOR_CONTROL_MODE, ~(STATUS_MOTOR_MOVING));
if (!M2.moving_en)
// clear moving status flag
control_table_AND_mask(A_MDRV_KNEE_MOTOR_CONTROL_MODE, ~(STATUS_MOTOR_MOVING));
set_control_table_volatile(A_MDRV_HIP_PRESENT_DESIRED_POSITION, (int)M1_INT_POS_2_EXT_POS(M1.moving_pos));
set_control_table_volatile(A_MDRV_HIP_PRESENT_POSITION, (int)M1_INT_POS_2_EXT_POS(M1.pos_cnt));
set_control_table_volatile(A_MDRV_KNEE_PRESENT_DESIRED_POSITION, (int)M2_INT_POS_2_EXT_POS(M2.moving_pos));
set_control_table_volatile(A_MDRV_KNEE_PRESENT_POSITION, (int)M2_INT_POS_2_EXT_POS(M2.pos_cnt));
set_control_table_volatile(A_MDRV_HIP_PRESENT_DESIRED_SPEED, (int)M1_INT_SPEED_2_EXT_SPEED(M1.moving_speed));
set_control_table_volatile(A_MDRV_HIP_PRESENT_SPEED, (int)M1_INT_SPEED_2_EXT_SPEED(M1.current_speed));
set_control_table_volatile(A_MDRV_KNEE_PRESENT_DESIRED_SPEED, (int)M2_INT_SPEED_2_EXT_SPEED(M2.moving_speed));
set_control_table_volatile(A_MDRV_KNEE_PRESENT_SPEED, (int)M2_INT_SPEED_2_EXT_SPEED(M2.current_speed));
set_control_table_volatile(A_MDRV_HIP_PRESENT_TORQUE, M1.output_trq);
set_control_table_volatile(A_MDRV_KNEE_PRESENT_TORQUE, M2.output_trq);
}
///////////////////////////////////////////////////////////////////////////////
// Compute the reference position and speed of the motorshaft, based on the set_speed and acceleration settings
void update_moving_position( motor *M) {
int max_accel;
// The desired position moves according to the speed and acceleration settings...
if (M->moving_en) {
// 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(M->moving_speed), (int)M->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 ((M->moving_speed>0 && M->set_speed>0) || (M->moving_speed<0 && M->set_speed<0)) {
// if ((long)M->moving_speed*(long)M->set_speed > 0) {
// if (_ABS(M->moving_pos-M->goal_pos) < ((long)_ABS(M->moving_speed)*((_ABS(M->moving_speed-1)/max_accel)+1+1))/(2*(1<<SPEED_SHIFT))) {
// if ((_ABS(M->moving_pos-M->goal_pos)*(long)max_accel*(1<<SPEED_SHIFT)*2) < ((long)_ABS(M->moving_speed)*(_ABS(M->moving_speed) + max_accel-1))) {
if ((_ABS(M->moving_pos-M->goal_pos)*(long)max_accel) < ((long)_ABS(M->moving_speed)*((long)_ABS(M->moving_speed) + (long)max_accel-1))/((long)(1<<SPEED_SHIFT)*2)) {
// we don't set the desired speed to zero, but to the minimum acceleration setting in order to guarantee convergence
if (M->set_speed > 0) M->set_speed = _MIN((int)M->max_accel, M->set_speed);
else M->set_speed = -_MIN((int)M->max_accel, -M->set_speed);
}
}
// compute new speed according to the acceleration parameter
if (M->moving_speed != M->set_speed) {
// LED_TX_ENABLE;
if (M->moving_speed < M->set_speed) {
M->moving_speed += (int)M->max_accel;
if (M->moving_speed > M->set_speed) {
M->moving_speed = M->set_speed;
}
}
else {
M->moving_speed -= (int)M->max_accel;
if (M->moving_speed < M->set_speed) {
M->moving_speed = M->set_speed;
}
}
}
// else LED_TX_DISABLE;
// compute new desired position for the current step
M->moving_pos += M->moving_speed>>SPEED_SHIFT;
// compensate for the bit errors due to the shifting action
M->resolution_error += (M->moving_speed - ((M->moving_speed>>SPEED_SHIFT)<<SPEED_SHIFT));
if (M->resolution_error > (1<<SPEED_SHIFT)) {
M->resolution_error -= (1<<SPEED_SHIFT);
M->moving_pos += 1;
}
// check if the goal position is reached
if (((M->set_speed > 0) && (M->moving_pos >= M->goal_pos)) || ((M->set_speed < 0) && (M->moving_pos <= M->goal_pos)) ) {
M->moving_pos = M->goal_pos;
M->moving_speed = 0;
M->moving_en = 0; // disable stepping when goal position is reached
// LED_TX_DISABLE;
}
}
}
//long prev_p_error = 0;
// compute new speed and torque settings
void M1_compute_output(void) {
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 = M1.moving_pos - M1.pos_cnt;
// compute new PID output
p_error = (long)M1.current_speed - desired_speed;
if (M1.update_i_error) M1.i_error += p_error; // no integrate action in compliant mode
d_error = p_error - M1.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)M1.speed_pgain) + (M1.i_error*(long)M1.speed_igain) + (d_error*(long)M1.speed_dgain))/(long)1024;
M1.prev_p_error = p_error;
// anti-windup
i_max = ((long)(MAX_PWM-1)*(long)1024)/(M1.speed_igain*2);
if (M1.i_error > i_max)
M1.i_error = i_max;
else if (M1.i_error < -i_max)
M1.i_error = -i_max;
// clamp output
speed_output = (_ABS(pid_output) > (long)(MAX_PWM-1)) ? (unsigned int)(MAX_PWM-1) : (unsigned int)_ABS(pid_output);
// set pwm output
M1_SET_PWM(speed_output);
// ***************** Output torque control section *****************
// torque as feeled by the leg = spring force - spring damping + rotor inertia compensation
M1.output_trq = (((desired_speed)*(long)M1.compliance_gain)/512)
- (((long)(M1.current_speed-(long)(M1.moving_speed>>SPEED_SHIFT))*(long)M1.compliance_damping)/64);
// Add the spring preload term
if (desired_speed > 0)
M1.output_trq += M1.compliance_preload;
else
M1.output_trq -= M1.compliance_preload;
// required torque on the motor is the requested torque + a compensation term for the rotor inertia
required_trq = M1.output_trq + (long)M1.current_accel*(long)M1_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(M1.compliance_preload) < scaled_speed_output) {
M1.update_i_error = 0;
if (required_trq>0) DIR1=0; else DIR1=1;
if (M1.compliance_gain>500) // problematic switching between fast and slow decay for very stiff spring settings
PWM_TRQ1 = M1_trq_to_pwm(_ABS(required_trq), TRUE);
else
PWM_TRQ1 = M1_trq_to_pwm(_ABS(required_trq), FALSE);
// LED_TX_ENABLE;
}
else {
M1.update_i_error = 1;
// if (speed_output > 2)
if (pid_output<0) DIR1=0; else DIR1=1;
// 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_TRQ1 = M1_trq_to_pwm(_ABS(required_trq), TRUE);
// LED_TX_DISABLE;
}
}
// compute new speed and torque settings
void M2_compute_output(void) {
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 = M2.moving_pos - M2.pos_cnt;
// compute new PID output
p_error = (long)M2.current_speed - desired_speed;
if (M2.update_i_error) M2.i_error += p_error; // no integrate action in compliant mode
d_error = p_error - M2.prev_p_error;
// if (M2.update_i_error) d_error = p_error - M2.prev_p_error; else d_error = 0;
pid_output = ((p_error*(long)M2.speed_pgain) + (M2.i_error*(long)M2.speed_igain) + (d_error*(long)M2.speed_dgain))/(long)1024;
M2.prev_p_error = p_error;
// anti-windup
i_max = ((long)(MAX_PWM-1)*(long)1024)/(M2.speed_igain*2);
if (M2.i_error > i_max)
M2.i_error = i_max;
else if (M2.i_error < -i_max)
M2.i_error = -i_max;
// clamp output
speed_output = (_ABS(pid_output) > (long)(MAX_PWM-1)) ? (unsigned int)(MAX_PWM-1) : (unsigned int)_ABS(pid_output);
// set pwm output
M2_SET_PWM(speed_output);
// ***************** Output torque control section *****************
// torque as feeled by the leg = spring force - spring damping + rotor inertia compensation
M2.output_trq = (((desired_speed)*(long)M2.compliance_gain)/512)
- (((long)(M2.current_speed-(long)(M2.moving_speed>>SPEED_SHIFT))*(long)M2.compliance_damping)/64);
// add the spring preload term
if (desired_speed > 0)
M2.output_trq += M2.compliance_preload;
else
M2.output_trq -= M2.compliance_preload;
// required torque on the motor is the requested torque + a compensation term for the rotor inertia
required_trq = M2.output_trq + (long)M2.current_accel*(long)M2_INERTIA_COMP/64;
// scale speed to torque dimensions
scaled_speed_output = (long)((long)speed_output*TORQUE_RESOLUTION)/(long)MAX_PWM;
// Set direction of torque, depending on which factor is smallest
// if ((long)_ABS(M2.output_trq) < (long)(speed_output*MAX_DUTY)/MAX_PWM) {
if ((unsigned int)_ABS(M2.compliance_preload) < scaled_speed_output) {
M2.update_i_error = 0;
if (required_trq>0) DIR2=0; else DIR2=1;
if (M2.compliance_gain>500) // problematic switching between fast and slow decay for very stiff spring settings
PWM_TRQ2 = M2_trq_to_pwm(_ABS(required_trq), TRUE);
else
PWM_TRQ2 = M2_trq_to_pwm(_ABS(required_trq), FALSE);
}
else {
M2.update_i_error = 1;
// if (speed_output > 2)
if (pid_output<0) DIR2=0; else DIR2=1;
// force fast decay mode
// if (scaled_speed_output*4 < (unsigned int)_ABS(M2.compliance_preload))
// PWM_TRQ2 = M2_trq_to_pwm((long)scaled_speed_output*4, TRUE);// (_ABS(required_trq)*(long)speed_output)/50, TRUE);
// else
// PWM_TRQ2 = M2_trq_to_pwm((long)scaled_speed_output, TRUE);
PWM_TRQ2 = M2_trq_to_pwm(_ABS(required_trq), TRUE);
}
}
/*
// compute new torque settings based on a damped spring system
void M2_compute_output(void) {
long speed_error;
long desired_speed;
int tmp;
// ***************** Output speed control section *****************
desired_speed = M2.moving_pos - M2.pos_cnt;
speed_error = (((long)M2.current_speed - desired_speed)*(long)M2.speed_pgain)/256;
// Set motor speed
tmp = (_ABS(speed_error)>(long)MAX_PWM) ? (int)MAX_PWM : (int)_ABS(speed_error);
M2_SET_PWM(tmp);
// M2_SET_PWM(MAX_PWM); // relying solely on the torque-control does not work in high-stiffness settings and generally results in more vibrations
// ***************** Output torque control section *****************
// torque = spring force - spring damping + rotor inertia compensation
M2.output_trq = (((desired_speed)*(long)M2.compliance_gain)/512)
- (((long)(M2.current_speed-(long)(M2.moving_speed>>SPEED_SHIFT))*(long)M2.compliance_damping)/64)
+ (long)M2.current_accel*(long)M2_INERTIA_COMP/64;
// Add the spring preload term
// if (desired_speed > 0) {
// M2.output_trq += M2.compliance_preload;
// }
// else {
// M2.output_trq -= M2.compliance_preload;
// }
if (desired_speed > 0) {
M2.output_trq += (M2.compliance_preload*tmp)/MAX_PWM;
}
else {
M2.output_trq -= (M2.compliance_preload*tmp)/MAX_PWM;
}
// Set direction of torque
if (M2.output_trq>0) DIR2=0; else DIR2=1;
// Set motor torque
PWM_TRQ2 = M2_trq_to_pwm(_ABS(M2.output_trq), FALSE);
}
*/
// torque value pased to this function is assumed being a 12-bit value, with 4096 being the maximum available torque
unsigned int M1_trq_to_pwm(long desired_trq, uint8 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 ((M1.max_trq > ((MAX_DUTY*M1_SLOWFAST_THRES)/256)) && (!force_fast_decay)) {
if (pwm_out > M1.max_trq + M1_SLOWFAST_COMP) pwm_out = M1.max_trq + M1_SLOWFAST_COMP;
}
else { // motor remains in the fast decay regime
if (pwm_out > M1.max_trq) pwm_out = M1.max_trq;
}
if (force_fast_decay) {
MODE1 = FAST_DECAY;
// LED_TX_DISABLE;
}
else {
// choose appropriate decay mode
if (pwm_out > (unsigned int)((MAX_DUTY*M1_SLOWFAST_THRES)/256)) {
MODE1 = SLOW_DECAY;
// LED_TX_ENABLE;
}
else if (pwm_out < (unsigned int)((MAX_DUTY*(M1_SLOWFAST_THRES-10))/256)) { // small hysteresis
MODE1 = FAST_DECAY;
// LED_TX_DISABLE;
}
}
// compensate for the slightly higher efficiency of the slow decay mode
if (MODE1 == SLOW_DECAY) pwm_out -= (unsigned int)M1_SLOWFAST_COMP;
return pwm_out;
}
// torque value pased to this function is assumed being a 12-bit value, with 4096 being the maximum available torque
unsigned int M2_trq_to_pwm(long desired_trq, uint8 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 (M2.max_trq > ((MAX_DUTY*M2_SLOWFAST_THRES)/256)) {
if (pwm_out > M2.max_trq + M2_SLOWFAST_COMP) pwm_out = M2.max_trq + M2_SLOWFAST_COMP;
}
else { // motor remains in the fast decay regime
if (pwm_out > M2.max_trq) pwm_out = M2.max_trq;
}
if (force_fast_decay) {
MODE2 = FAST_DECAY;
}
else {
// choose appropriate decay mode
if (pwm_out > (unsigned int)((MAX_DUTY*M2_SLOWFAST_THRES)/256)) {
MODE2 = SLOW_DECAY;
}
else if (pwm_out < (unsigned int)((MAX_DUTY*(M2_SLOWFAST_THRES-10))/256)) { // small hysteresis
MODE2 = FAST_DECAY;
}
}
// compensate for the slightly higher efficiency of the slow decay mode
if (MODE2 == SLOW_DECAY) pwm_out -= (unsigned int)M2_SLOWFAST_COMP;
return pwm_out;
}
c4science · Help