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); }