uint8_t motion_append(void)
// Append a new curve keypoint from data stored in the curve registers. The keypoint
// is offset from the previous curve by the specified delta. An error is returned if
// there is no more room to store the new keypoint in the buffer or if the delta is
// less than one (a zero delta is not allowed).
{
int16_t position;
int16_t in_velocity;
int16_t out_velocity;
uint8_t next;
uint16_t delta;
// Get the next index in the buffer.
next = (motion_head + 1) & MOTION_BUFFER_MASK;
// Return error if we have looped the head to the tail and the buffer is filled.
if (next == motion_tail) return 0;
// Get the position, velocity and time delta values from the registers.
position = (int16_t) registers_read_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO);
in_velocity = (int16_t) registers_read_word(REG_CURVE_IN_VELOCITY_HI, REG_CURVE_IN_VELOCITY_LO);
out_velocity = (int16_t) registers_read_word(REG_CURVE_OUT_VELOCITY_HI, REG_CURVE_OUT_VELOCITY_LO);
delta = (uint16_t) registers_read_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO);
// Keypoint delta must be greater than zero.
if (delta < 1) return 0;
// Fill in the next keypoint.
keys[next].delta = delta;
keys[next].position = int_to_float(position);
keys[next].in_velocity = fixed_to_float(in_velocity);
keys[next].out_velocity = fixed_to_float(out_velocity);
// Is this keypoint being added to an empty buffer?
if (motion_tail == motion_head)
{
// Initialize a new hermite curve that gets us from the current position to the new position.
// We use a velocity of zero at each end to smoothly transition from one to the other.
curve_init(0, delta, curve_get_p1(), keys[next].position, 0.0, 0.0);
}
// Increase the duration of the buffer.
motion_duration += delta;
// Set the new head index.
motion_head = next;
// Reset the motion registers and update the buffer status.
motion_registers_reset();
return 1;
}
real& real::operator=(const GLfixed& fxt)
{
if (_is_float)
{
_float = fixed_to_float(fxt);
}
else
{
_fixed = fxt;
}
return *this;
}
/**
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;
}
GLfloat const_real::as_float() const
{
return _is_float ? _float : fixed_to_float(_fixed);
}
GLfloat const_real_ptr::as_float(size_t n) const
{
return _is_float ? _float[n] : fixed_to_float(_fixed[n]);
}
int main(int ac, char** av)
{
#if 1
{
fixed_t fu = float_to_fixed(1.2);
fixed_t bar = float_to_fixed(2.4);
fixed_t baz = fixed_add(fu, bar);
printf("%f + ", fixed_to_float(fu));
printf("%f = ", fixed_to_float(bar));
printf("%f\n", fixed_to_float(baz));
}
{
fixed_t fu = float_to_fixed(1.5);
fixed_t bar = int_to_fixed(2);
fixed_t baz = fixed_mul(fu, bar);
printf("%f * ", fixed_to_float(fu));
printf("%f = ", fixed_to_float(bar));
printf("%f\n", fixed_to_float(baz));
}
{
fixed_t fu = int_to_fixed(1);
fixed_t bar = int_to_fixed(2);
fixed_t baz = fixed_div(fu, bar);
printf("%f / ", fixed_to_float(fu));
printf("%f = ", fixed_to_float(bar));
printf("%f\n", fixed_to_float(baz));
}
{
fixed_t fu = int_to_fixed(-1);
fixed_t bar = int_to_fixed(2);
fixed_t baz = fixed_div(fu, bar);
printf("%f / ", fixed_to_float(fu));
printf("%f = ", fixed_to_float(bar));
printf("%f\n", fixed_to_float(baz));
}
#endif
#if 0
{
fixed_t alpha = float_to_fixed(0.0);
fixed_t step = float_to_fixed(0.01);
for (; alpha < FIXED_TWO_PI; alpha = fixed_add(alpha, step))
{
printf("%f ", fixed_to_float(alpha));
printf("%f\n", fabsf(sinf(fixed_to_float(alpha)) - fixed_to_float(fixed_sin(alpha))));
}
}
#endif
return 0;
}
void state_fixed_to_float(known_state_ints& state_ints, known_state& state)
{
state.p = fixed_to_float(state_ints.p);
state.q = fixed_to_float(state_ints.q);
state.r = fixed_to_float(state_ints.r);
state.dq = fixed_to_float(state_ints.dq);
state.dr = fixed_to_float(state_ints.dr);
state.iq = fixed_to_float(state_ints.iq);
state.ir = fixed_to_float(state_ints.iq);
state.error_heading = fixed_to_float(state_ints.error_heading);
state.derror_heading = fixed_to_float(state_ints.derror_heading);
state.ierror_heading = fixed_to_float(state_ints.ierror_heading);
state.error_pitch = fixed_to_float(state_ints.error_pitch);
state.dpitch = fixed_to_float(state_ints.dpitch);
state.ipitch = fixed_to_float(state_ints.ipitch);
}
