diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index aaac53b5..bd000916 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -62,8 +62,8 @@ void Axis::StateMachineLoop() { for (;;) { // Keep rotor estimation up to date while idling osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); - update_rotor(legacy_motor_ref_); - + loop_updates(legacy_motor_ref_); + if (do_calibration_) { do_calibration_ = false; diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index c49b95ea..3819fa43 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -72,6 +72,7 @@ Motor_t motors[] = { .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] .resistance_calib_max_voltage = 1.0f, // [V] + .dc_bus_brownout_trip_level = 8.0f, // [V] .phase_inductance = 0.0f, // to be set by measure_phase_inductance .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, @@ -177,6 +178,7 @@ Motor_t motors[] = { .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] .resistance_calib_max_voltage = 1.0f, // [V] + .dc_bus_brownout_trip_level = 8.0f, // [V] .phase_inductance = 0.0f, // to be set by measure_phase_inductance .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, @@ -553,6 +555,7 @@ void step_cb(uint16_t GPIO_Pin) { } // Triggered when an encoder passes over the "Index" pin +// TODO: only arm index edge interrupt when we know encoder has powered up void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { Motor_t* motor = &motors[motor_index]; if (!motor->encoder.index_found) { @@ -685,6 +688,9 @@ bool measure_phase_resistance(Motor_t* motor, float test_current, float max_volt motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; + float Ialpha = -(motor->current_meas.phB + motor->current_meas.phC); test_voltage += (kI * current_meas_period) * (test_current - Ialpha); if (test_voltage > max_voltage) test_voltage = max_voltage; @@ -724,6 +730,9 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h motor->error = ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; + Ialphas[i] += -motor->current_meas.phB - motor->current_meas.phC; // Test voltage along phase A @@ -774,6 +783,8 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; queue_voltage_timings(motor, voltage_magnitude, 0.0f); } // scan forwards @@ -783,6 +794,8 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; float v_alpha = voltage_magnitude * arm_cos_f32(ph); float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); @@ -808,6 +821,8 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; float v_alpha = voltage_magnitude * arm_cos_f32(ph); float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); @@ -911,6 +926,8 @@ bool scan_for_enc_idx(Motor_t* motor, float omega, float voltage_magnitude) { for (;;) { for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); + if (!do_checks(motor)) + return false; if (motor->encoder.index_found) return true; @@ -1101,8 +1118,13 @@ bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { motor->error = ERROR_SPIN_UP_TIMEOUT; return false; } + + if (!do_checks(motor)) + return false; // run estimator - update_rotor(motor); + if (!loop_updates(motor)) + return false; + // override the phase during spinup motor->sensorless.phase = phase; // run current control (with the phase override) @@ -1191,6 +1213,8 @@ void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { queue_modulation_timings(motor, mod_alpha, mod_beta); } +// TODO: This doesn't update brake current +// We should probably make FOC Current call FOC Voltage to avoid duplication. bool FOC_voltage(Motor_t* motor, float v_d, float v_q) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); @@ -1273,14 +1297,40 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { motor->error = ERROR_FOC_TIMING; return false; } + + update_brake_current(); return true; } -//Returns true if the fault line is asserted +//Returns true if everything is OK (no fault) bool check_DRV_fault(Motor_t* motor) { //TODO: make this pin configurable per motor ch GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(nFAULT_GPIO_Port, nFAULT_Pin); - return (nFAULT_state == GPIO_PIN_RESET) ? true : false; + return (nFAULT_state == GPIO_PIN_RESET) ? false : true; +} + +//Returns true if everything is OK (no fault) +bool check_PSU_brownout(Motor_t* motor) { + if(vbus_voltage < motor->dc_bus_brownout_trip_level) { + motor->error = ERROR_DC_BUS_BROWNOUT; + return false; + } + return true; +} + +// Returns true if everything is ok. Sets motor->error and returns false otherwise. +bool do_checks(Motor_t* motor) { + // Checks + if (!check_DRV_fault(motor)) { + motor->error = ERROR_DRV_FAULT; + return false; + } + return true; +} + +bool loop_updates(Motor_t* motor) { + update_rotor(motor); + return true; } void control_motor_loop(Motor_t* motor) { @@ -1289,12 +1339,14 @@ void control_motor_loop(Motor_t* motor) { motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; break; } - if (check_DRV_fault(motor)) { - motor->error = ERROR_DRV_FAULT; + + if (!do_checks(motor)) break; - } - update_rotor(motor); - anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking + if (!loop_updates(motor)) + break; + + // Only runs if anticogging.calib_anticogging is true; non-blocking + anti_cogging_calibration(motor); // Position control // TODO Decide if we want to use encoder or pll position here @@ -1377,7 +1429,6 @@ void control_motor_loop(Motor_t* motor) { break; } - update_brake_current(); ++(motor->loop_counter); } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 9045b1d9..e8f4bcc5 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -50,6 +50,7 @@ typedef enum { ERROR_SPIN_UP_TIMEOUT, ERROR_DRV_FAULT, ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, + ERROR_DC_BUS_BROWNOUT, } Error_t; // Note: these should be sorted from lowest level of control to @@ -147,6 +148,7 @@ typedef struct { float current_setpoint; float calibration_current; float resistance_calib_max_voltage; + float dc_bus_brownout_trip_level; float phase_inductance; float phase_resistance; osThreadId motor_thread; @@ -243,6 +245,8 @@ bool anti_cogging_calibration(Motor_t* motor); // Test functions void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude); // Main motor control +bool do_checks(Motor_t* motor); +bool loop_updates(Motor_t* motor); void update_rotor(Motor_t* motor); bool using_encoder(Motor_t* motor); bool using_sensorless(Motor_t* motor);