/**
Calculates a thermal limiting multiplier for use by @ref mc_pid_current.
Using the @c thermal_current_limit and @c thermal_time_constant passed to @ref mc_init,
this function will estimate the current temperature of the motor and find a multiplier
that when multiplied by the current will limit the temperature. If the motor is too hot,
this function will call @ref mc_turnoff to completely shutdown the motor until it cools down.
@warning This function uses the motor current and Newton's Law of Cooling to estimate the temperature,
it doesn't measure the motor temperature directly, so care should be taken to let the motors cool down when
the robot is turned off and on, as the residual heat of the motors will not be taken into account.
@param current The newest motor current to add to the thermal checking.
*/
static fixed mc_get_thermal_limiter(fixed current){
//Check max current for thermal shutoff
//Must create current_factor, thermal_safe, thermal_limit
static fixed multiplier = FIXED_ONE;
static fixed a = 0; // a is C/R*(temperature - environmental temperature), C is heat capacity of motor, R is electric resitance which causes the heating I^2*R
// DANGEREROUS TO INITIALIZE A to zero, eg after lettting the robo on for a while, then switching off and then turning on again
fixed a_dot_dt; //derivative of 'a' multiplied by time step dt
fixed motor_current = current;
a_dot_dt = fixed_mult((fixed_mult(motor_current, motor_current)-fixed_mult(mc_data.tau_inv, a)), mc_data.dt); // derivative is -(1/tau)*a + I^2 , this is newton's law of cooling
a = a + a_dot_dt; //
if (a < mc_data.thermal_safe){ //below limit, full current
multiplier = FIXED_ONE;
if (mc_therm_off){ //previously turned off by thermal limit, but safe again
mc_turnon(MC_THERM); //turn on controller
}
} else if (a >= mc_data.thermal_safe && a < mc_data.thermal_limit){ //above safety limit, use mult factor
multiplier = FIXED_ONE - (fixed_mult(a - mc_data.thermal_safe, mc_data.thermal_diff)); //mc.thermal_diff = float_to_fixed(1.0/(thermal_limit - thermal_safe))
error_occurred(ERROR_MC_TEMP_SAFE);
} else if (a >= mc_data.thermal_limit){ //above strict limit, shutdown
mc_turnoff(MC_THERM); //turn off controller due to thermal limit
multiplier = 0;
} else {
multiplier = FIXED_ONE; //should never reach here
}
return multiplier;
}
/**
A compliant controller for the motor.
Similar to a PD-position controller. To use, the user should call @ref mc_set_stiffness,
@ref mc_set_dampness, and @ref mc_set_command_current to set the parameters of the controller.
A call to @ref mc_set_command_current will latch the new stiffness and dampness values; without this
call, dampness and stiffness (and the command current) won't change. The formula used in the
control is @code Icontrol = Icommand - (stiffness * pos) - (dampness * vel) @endcode.
Icontrol is passed to the PID current controller, which will attempt to go to the desired current.
To request a specific position (in radians) with a given stiffness, use: @code Icommand = stiffness * desired_position @endcode.
*/
void mc_compliant_control(void){
float pos_rads = mc_data.get_position();
fixed pos = float_to_fixed(pos_rads);
fixed vel = float_to_fixed(mc_data.get_velocity());
fixed control = mc_command_current - fixed_mult(mc_c1, pos) - fixed_mult(mc_c2, vel);
mc_set_target_current_fixed(control);
mc_pid_current();
}
/**
Calculates the result of plugging in @c value into the linear equation
described by the @c offset and @c coeff as a fixed point value.
Used in converting between units.
@code retval = (coeff * value) + offset @endcode
@param offset The offset of the linear equation. Often called @c b.
@param coeff The coefficient of the linear equation. Often called @c m.
@param value The value to be plugged into the linear equation. Often called @c x.
@return The fixed point result of plugging @c value into the linear equation.
*/
fixed linear_to_fixed(int offset, float coeff, int value){
int a, b, c, d, e;
a = value;
b = a - offset; //subtract the offset
c = -1 * int_to_fixed(b); //convert to fixed point
d = float_to_fixed(coeff); //convert coefficient to fixed point
e = fixed_mult(c,d); //multiply gain to get amps in fixed point
return e;
}
/**
Initializes the motor controller. Must be called before the motor
controller module can be used properly.
@param max_volts The maximum voltage the motor can take.
@param max_current The maximum allowed current for the motor.
@param min_current The minimum allowed current for the motor, often just -1 * @c max_current.
@param max_target The maximum allowed current to be commanded using @ref mc_set_target_current.
@param min_target The minimum allowed current to be commanded using @ref mc_set_target_current.
@param thermal_current_limit The nominal or max continuous current for the motor.
@param thermal_time_constant The Winding Thermal Time Constant.
@param kp Proportional constant for the PID current controller.
@param ki Integral constant for the PID current controller.
@param kd Derivative constant for the PID current controller.
@param max_pwm Maximum allowed PWM for the motor, often to limit voltage.
@param error_limit If there are this many errors in a row, shutdown the motor. (DEPRECATED)
@param smart_error_limit If the integral of the error is greater than this value, shutdown the motor.
@param op The operation of the motor, either @c MC_NORMAL or @c MC_BACKWARDS. Used if positive PWM and positive position don't match.
@param current A function that returns the motor current as a fixed point value in amps.
@param position A function that returns the motor position as a floating point number in radians.
@param velocity A function that returns the motor velocity as a floating point number in radians.
*/
void mc_init(
float max_volts,
float max_current,
float min_current,
float max_target, // dont see this in soft_setup_init
float min_target, // dont see this in soft_setup_init
float thermal_current_limit, //from spec sheet
float thermal_time_constant, //tau, from spec sheet
float kp,
float ki,
float kd,
int max_pwm,
int error_limit, // set 4 now
float smart_error_limit, // set 10 now
MC_OPERATION op, // I dont see this in soft_setup_init
FIXED_VOID_F current,
FLOAT_VOID_F position,
FLOAT_VOID_F velocity) //initializes the motor controller
{
mc_data.operation = op;
mc_data.motor_voltage_max = max_volts; //Volts
mc_data.current_max = float_to_fixed(max_current); //Amps
mc_data.current_min = float_to_fixed(min_current); //Amps
mc_data.target_max = float_to_fixed(max_target);
mc_data.target_min = float_to_fixed(min_target);
mc_data.thermal_limit = float_to_fixed((thermal_current_limit*thermal_current_limit)*thermal_time_constant);
mc_data.tau_inv = float_to_fixed(1.0/thermal_time_constant);
mc_data.thermal_safe = fixed_mult(float_to_fixed(0.75), mc_data.thermal_limit);
mc_data.thermal_diff = float_to_fixed(1.0/(thermal_current_limit - fixed_to_float(mc_data.thermal_safe)));
mc_data.dt = float_to_fixed(1.0/(float)(1000*SCHED_SPEED));
mc_data.kp = float_to_fixed(kp);
mc_data.ki = float_to_fixed(ki);
mc_data.kd = float_to_fixed(kd);
mc_data.integral = 0;
mc_data.max_pwm = max_pwm;
mc_data.max_integral = float_to_fixed((((float)max_pwm)/ki));
mc_data.error_limit = error_limit;
mc_data.smart_error_limit = float_to_fixed(smart_error_limit);
mc_data.smart_error_limit_inverse = float_to_fixed(1.0*10.0/smart_error_limit);
mc_data.get_current = current;
mc_data.get_position = position;
mc_data.get_velocity = velocity;
}
/**
Checks the motor current for limits.
If the motor current is outside of the @c current_limits, this returns a multiplier which
will be multiplied by the target current to slowly decrease its value.
Also integrates the motor current error (the amount the motor current is above the @c current_limit),
and if this value is greater than @c smart_error_limit, calls @ref mc_turnoff to completely turn off the motor,
until it is safely back within bounds.
@param current The newest current to add to the smart current limiting.
@return A multiplier to limit the current if it is out of bounds.
*/
static fixed mc_smart_check_current(fixed current){
//Check max current for mechanical shutoff
//Must change error_limit to around 20.
static fixed current_error = 0;
fixed motor_current = current;
static fixed multiplier = FIXED_ONE;
// we'll set the limit to 4.5 amps.. if it is below these limits then the things are fine.
// as soon as we try to go above this limit a multiplier less than one will be generated.
// ****** Integrate Error ****** //
if (motor_current > 0){
current_error = current_error + (motor_current - mc_data.current_max);
} else if (motor_current < 0){
current_error = current_error - (motor_current - mc_data.current_min);
}
if (current_error <= 0){
current_error = 0;
if (mc_mech_off){ //previously turned off due to mechanical limit
mc_turnon(MC_MECH); //integrator <= 0 so safe to turn back on
}
}
// ****** Calculate multiplier ****** //
multiplier = FIXED_ONE - (fixed_mult(current_error , mc_data.smart_error_limit_inverse));
if (multiplier < 0){multiplier = 0;}
mc_mult = fixed_to_float(multiplier);
// smart_erroe_limit_inverse is not exactly 1/smart_error_limit, which would mean multiplier goes from one to zero when error goes from 0 to error limit
// instead I wanted the multiplier to decay faster becasue its taking time to respond/ or the error is growing very fast.
// so I am settign it to be like 1/.1/smart_error_limit so that it goes to zero quicker and then remains zero
// ****** Check absolute limit ****** //
if (current_error >= mc_data.smart_error_limit){ //set limit to ~10, can be changed.
mc_turnoff(MC_MECH);
}
//this is so that when curren go to dangerous level of 8
//then the error 8-4.5 = 3.5 will have about three tic tocs to reach to 10 and stops.
//3.3 ms is the mechanichal time constant.
return multiplier;
}
/* Advance the frame of pac man's current movement animation, given
the amount of time (in ms) that has passed. */
void agent_pman_frame_advance(GameAgent *ga, Uint32 time)
{
int anim_start_frame = -1;
int anim_end_frame;
anim_start_frame = map_fixed_vector_to_direction(&ga->curr_move);
if (anim_start_frame == -1) return;
anim_start_frame *= PMAN_FRAMES_PER_DIRECTION;
anim_end_frame = anim_start_frame + PMAN_FRAMES_PER_DIRECTION;
ga->frame_curr += fixed_mult(FIXED_SET_INT(time), ga->frame_speed);
if (FIXED_GET_INT(ga->frame_curr) >= anim_end_frame-2) {
ga->frame_curr = FIXED_SET_INT(anim_end_frame-1-2);
ga->frame_speed = -abs(ga->frame_speed);
} else if (FIXED_GET_INT(ga->frame_curr) < anim_start_frame) {
ga->frame_curr = FIXED_SET_INT(anim_start_frame);
ga->frame_speed = abs(ga->frame_speed);
}
}
/**
Uses a PID controller to try and reach the target current.
The target currect used by this function should be set using
@ref mc_set_target_current or @ref mc_set_target_current_fixed.
The constants for the current controller are given by the @c kp, @c ki, and @c kd
given to @ref mc_init.
Updates the watchdog, and uses both a thermal limiting multiplier and integrating
current limits.
*/
void mc_pid_current(void)
{
volatile fixed mc_error = 0;
static unsigned char start_count = 2;
signed long int curr_pwm = 0;
long long int pid_sum, p_component, i_component;
fixed thermal_multiplier, mechanical_multiplier;
mc_update_watchdog(); //feed watchdog if running or sleeping
if (!start_count){ //wait a few iterations to allow the adcx to get meaningful data
fixed current = mc_data.get_current(); //motor current on the robot
fixed target_current = mc_target_current; //the target current
thermal_multiplier = mc_get_thermal_limiter(current); //thermal multiplier, est. temp and limit current
mechanical_multiplier = mc_smart_check_current(current);
//During direction control, we limit the target_current to the correct direction.
//Used to do PWM, but ran into problem with Integral control
if (target_current < 0) { //trying to go in negative direction
target_current = target_current * mc_negative;
} else if (target_current > 0) { //trying to go in positive direction, but not allowed
target_current = target_current * mc_positive;
}
if (mc_is_running())
{
// ******************************* calc prop and integral **************************
//Calculate the error:
mc_error = (target_current - current);
//Integral saturation during direction control is fixed by limiting the
//target_current during direction_control, not the PWM
mc_data.integral += mc_error;
// ****************** check for saturation of integral constant ********************
if (mc_data.integral > mc_data.max_integral){
mc_data.integral = mc_data.max_integral;
error_occurred(ERROR_MC_INTEG_SATUR);
} else if (mc_data.integral < -1*mc_data.max_integral){
mc_data.integral = -1*mc_data.max_integral;
error_occurred(ERROR_MC_INTEG_SATUR);
}
// Sum the terms of the PID algorithm to give the new PWM duty cycle value
//Note: Need to bound each component such that their sum can't overflow
p_component = fixed_mult_to_long((mc_error),(mc_data.kp));
i_component = fixed_mult_to_long((mc_data.integral),(mc_data.ki));
pid_sum = p_component + i_component;
// check for fixed point overflow
if(pid_sum > FIXED_MAX) {
pid_sum = FIXED_MAX;
error_occurred(ERROR_MC_FIXED_LIMIT);
} else if (pid_sum < -1*FIXED_MAX) {
pid_sum = -1*FIXED_MAX;
error_occurred(ERROR_MC_FIXED_LIMIT);
}
if(thermal_multiplier < mechanical_multiplier){ //use whichever multiplier is smaller
pid_sum = fixed_mult(pid_sum, thermal_multiplier);
} else {
pid_sum = fixed_mult(pid_sum, mechanical_multiplier);
}
curr_pwm = fixed_to_int((pid_sum));
// ********************** check if PWM is within limits ****************************
if (curr_pwm > mc_data.max_pwm){
curr_pwm = mc_data.max_pwm;
error_occurred(ERROR_MC_PWM_LIMIT);
}
else if (curr_pwm < -1*mc_data.max_pwm){
curr_pwm = -1*mc_data.max_pwm;
error_occurred(ERROR_MC_PWM_LIMIT);
}
mc_set_pwm(curr_pwm);
} else {
mc_set_pwm(0);
}
} else {
start_count--;
}
}