From ea665833c4be313fa6eff28baa3731c0b2f00006 Mon Sep 17 00:00:00 2001 From: Tomaz Zlindra Date: Sun, 28 Jun 2026 21:39:14 -0700 Subject: [PATCH] done --- .../SessionController/FiniteStateMachine.hpp | 14 +- .../SessionController/SessionController.hpp | 24 +- .../SessionController/FiniteStateMachine.cpp | 183 ++-------- .../SessionController/SessionController.cpp | 341 ++++++++---------- .../input_manager_interrupts.c | 87 ++--- 5 files changed, 232 insertions(+), 417 deletions(-) diff --git a/Core/Inc/Tasks/SessionController/FiniteStateMachine.hpp b/Core/Inc/Tasks/SessionController/FiniteStateMachine.hpp index f520c03..a445006 100644 --- a/Core/Inc/Tasks/SessionController/FiniteStateMachine.hpp +++ b/Core/Inc/Tasks/SessionController/FiniteStateMachine.hpp @@ -32,11 +32,8 @@ struct State { INIT_STATE = 0, USB_LOGGING_OPTION_DISPLAYED = 0, - USB_LOGGING_OPTION_EDIT, SD_LOGGING_OPTION_DISPLAYED, - SD_LOGGING_OPTION_EDIT, PID_ENABLE_DISPLAYED, - PID_ENABLE_EDIT, PID_DESIRED_RPM_DISPLAYED, PID_DESIRED_RPM_EDIT }; @@ -95,15 +92,18 @@ class FSM private: // Methods which are called when a state change is done void IdleState(); - void USBLoggingOptionDisplayedSettingsState(); - void USBLoggingOptionEditSettingsState(); + void USBLoggingOptionDisplayedSettingsState(); void SDLoggingOptionDisplayedSettingsState(); - void SDLoggingOptionEditSettingsState(); void PIDOptionDisplayedSettingsState(); - void PIDOptionEditSettingsState(); void PIDDesiredRPMOptionDisplayedSettingsState(); void PIDDesiredRPMOptionEditSettingsState(bool clearDisplay); void InSessionState(); + + // Renders an on/off settings option: title on row 0, ENABLED/DISABLED on row 1. + void ShowToggleOption(State::SettingsState settingsState, const char* title, uint8_t titleColumn, bool enabled); + + // Writes a null-terminated string to the LCD (length derived from the string). + void WriteLine(uint8_t row, uint8_t column, const char* str); // user input handlers void HandleRotaryEncoderInput(bool positiveTick); diff --git a/Core/Inc/Tasks/SessionController/SessionController.hpp b/Core/Inc/Tasks/SessionController/SessionController.hpp index 0c29522..fdf4bac 100644 --- a/Core/Inc/Tasks/SessionController/SessionController.hpp +++ b/Core/Inc/Tasks/SessionController/SessionController.hpp @@ -8,7 +8,6 @@ #include "FiniteStateMachine.hpp" -#include "MessagePassing/messages_public.h" #include "MessagePassing/messages_public.h" #include "MessagePassing/osqueue_helpers.h" @@ -44,16 +43,33 @@ class SessionController session_controller_os_task_queues* _task_queues; osMutexId_t _usart1Mutex; + // Latest sensor readings (drained from the circular buffers each loop iteration). + forcesensor_output_data _force_data; + optical_encoder_output_data _optical_encoder_data; + + // Previous values, used for change-detection so we only act/transmit on a change. bool _prevUSBLoggingEnabled; bool _prevSDLoggingEnabled; bool _prevPIDEnabled; bool _prevInSession; + bool _pidAckReceived; + float _prevThrottleDutyCycle; + float _prevBpmDutyCycle; + float _prevForce; + float _prevAngularVelocity; bool CheckTaskQueuesValid(); - inline float CalculateTorque(float angularAcceleration, float force, float angularVelocity); - inline float CalculatePower(float torque, float angularVelocity); - inline float CalculateMechanicalLosses(float angularAcceleration, float angularVelocity); + // Run() loop steps (see Run() for ordering). + void SyncLoggingState(); // notify USB/SD controllers when logging enable changes + void HandleSessionEdge(); // enable/disable sensors + BPM on session start/stop + void SyncPidState(); // push PID enable/target changes and handle the ack + void HandleManualControl(); // push manual throttle/BPM duty cycle while in session + void UpdateMetrics(); // drain sensors, compute torque/power, refresh the display + + float CalculateTorque(float angularAcceleration, float force, float angularVelocity); + float CalculatePower(float torque, float angularVelocity); + float CalculateMechanicalLosses(float angularAcceleration, float angularVelocity); }; diff --git a/Core/Src/Tasks/SessionController/FiniteStateMachine.cpp b/Core/Src/Tasks/SessionController/FiniteStateMachine.cpp index e0cb22e..3c9234b 100644 --- a/Core/Src/Tasks/SessionController/FiniteStateMachine.cpp +++ b/Core/Src/Tasks/SessionController/FiniteStateMachine.cpp @@ -76,26 +76,14 @@ void FSM::HandleRotaryEncoderInput(bool positiveTick) if (positiveTick) SDLoggingOptionDisplayedSettingsState(); else PIDDesiredRPMOptionDisplayedSettingsState(); break; - case State::SettingsState::USB_LOGGING_OPTION_EDIT: - // _usbLoggingEnabled = positiveTick; - // USBLoggingOptionEditSettingsState(); - break; case State::SettingsState::SD_LOGGING_OPTION_DISPLAYED: if (positiveTick) PIDOptionDisplayedSettingsState(); else USBLoggingOptionDisplayedSettingsState(); break; - case State::SettingsState::SD_LOGGING_OPTION_EDIT: - // _sdLoggingEnabled = positiveTick; - // SDLoggingOptionEditSettingsState(); - break; case State::SettingsState::PID_ENABLE_DISPLAYED: if (positiveTick) PIDDesiredRPMOptionDisplayedSettingsState(); else SDLoggingOptionDisplayedSettingsState(); break; - case State::SettingsState::PID_ENABLE_EDIT: - // _pidOptionToggleableEnabled = positiveTick; - // PIDOptionEditSettingsState(); - break; case State::SettingsState::PID_DESIRED_RPM_DISPLAYED: if (positiveTick) USBLoggingOptionDisplayedSettingsState(); else PIDOptionDisplayedSettingsState(); @@ -129,30 +117,7 @@ void FSM::HandleRotaryEncoderInput(bool positiveTick) } void FSM::HandleRotaryEncoderSwInput(void) { -// Was not working, will debug later -// _inSession = true; -// InSessionState(); - - -// switch(_state.mainState) -// { -// case State::MainDynoState::IDLE: -// // InSessionState(); -// break; -// case State::MainDynoState::SETTINGS_MENU: -// // InSessionState(); -// switch(_state.settingsState) -// { -// case State::SettingsState::PID_DESIRED_RPM_EDIT: -// break; -// default: -// break; -// } -// break; -// case State::MainDynoState::IN_SESSION: -// break; -// } - + // Rotary-encoder switch press is currently unused. Brake button drives session start/stop. } void FSM::HandleButtonBackInput(void) { @@ -166,21 +131,12 @@ void FSM::HandleButtonBackInput(void) case State::SettingsState::USB_LOGGING_OPTION_DISPLAYED: IdleState(); break; - case State::SettingsState::USB_LOGGING_OPTION_EDIT: - USBLoggingOptionDisplayedSettingsState(); - break; case State::SettingsState::SD_LOGGING_OPTION_DISPLAYED: IdleState(); break; - case State::SettingsState::SD_LOGGING_OPTION_EDIT: - SDLoggingOptionDisplayedSettingsState(); - break; case State::SettingsState::PID_ENABLE_DISPLAYED: IdleState(); break; - case State::SettingsState::PID_ENABLE_EDIT: - PIDOptionDisplayedSettingsState(); - break; case State::SettingsState::PID_DESIRED_RPM_DISPLAYED: IdleState(); break; @@ -218,30 +174,15 @@ void FSM::HandleButtonSelectInput(void) { case State::SettingsState::USB_LOGGING_OPTION_DISPLAYED: _usbLoggingEnabled = !_usbLoggingEnabled; - USBLoggingOptionEditSettingsState(); - // We don't actually want to change state, should remove the USB_LOGGING_OPTION_EDIT state later - _state.settingsState = State::SettingsState::USB_LOGGING_OPTION_DISPLAYED; - break; - case State::SettingsState::USB_LOGGING_OPTION_EDIT: - // USBLoggingOptionEditSettingsState(); + USBLoggingOptionDisplayedSettingsState(); break; case State::SettingsState::SD_LOGGING_OPTION_DISPLAYED: _sdLoggingEnabled = !_sdLoggingEnabled; - SDLoggingOptionEditSettingsState(); - // We don't actually want to change state, should remove the SD_LOGGING_OPTION_EDIT state later - _state.settingsState = State::SettingsState::SD_LOGGING_OPTION_DISPLAYED; + SDLoggingOptionDisplayedSettingsState(); break; - case State::SettingsState::SD_LOGGING_OPTION_EDIT: - // SDLoggingOptionEditSettingsState(); - break; case State::SettingsState::PID_ENABLE_DISPLAYED: _pidOptionToggleableEnabled = !_pidOptionToggleableEnabled; - PIDOptionEditSettingsState(); - // We don't actually want to change state, should remove the PID_ENABLE_EDIT state later - _state.settingsState = State::SettingsState::PID_ENABLE_DISPLAYED; - break; - case State::SettingsState::PID_ENABLE_EDIT: - // PIDOptionEditSettingsState(); + PIDOptionDisplayedSettingsState(); break; case State::SettingsState::PID_DESIRED_RPM_DISPLAYED: PIDDesiredRPMOptionEditSettingsState(true); @@ -278,29 +219,16 @@ void FSM::HandleButtonSelectInput(void) void FSM::HandleButtonBrakeInput(bool isEnabled) { - if (isEnabled) - { - _inSession = true; - InSessionState(); - } - else - { - _inSession = false; - IdleState(); - } - - // switch(_state.mainState) - // { - // case State::MainDynoState::IDLE: - // break; - // case State::MainDynoState::SETTINGS_MENU: - // _inSession = true; - // DisplayInSessionScreen(); - // break; - // case State::MainDynoState::IN_SESSION: - // break; - // } - + if (isEnabled) + { + _inSession = true; + InSessionState(); + } + else + { + _inSession = false; + IdleState(); + } } int FSM::ConvertDesiredRpmUnitsStateToIncrement() @@ -352,94 +280,35 @@ void FSM::IdleState() AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 2, "PRESS SELECT", sizeof("PRESS SELECT") - 1); } -void FSM::USBLoggingOptionDisplayedSettingsState() -{ - _state.mainState = State::MainDynoState::SETTINGS_MENU; - _state.settingsState = State::SettingsState::USB_LOGGING_OPTION_DISPLAYED; - - ClearDisplay(); - - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 0, 2, "USB LOGGING", sizeof("USB LOGGING") - 1); - - if (_usbLoggingEnabled) - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "ENABLED", sizeof("ENABLED") - 1); - else - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "DISABLED", sizeof("DISABLED") - 1); -} - -void FSM::USBLoggingOptionEditSettingsState() +void FSM::WriteLine(uint8_t row, uint8_t column, const char* str) { - _state.mainState = State::MainDynoState::SETTINGS_MENU; - _state.settingsState = State::SettingsState::USB_LOGGING_OPTION_EDIT; - - ClearDisplay(); - - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 0, 2, "USB LOGGING", sizeof("USB LOGGING") - 1); - - if (_usbLoggingEnabled) - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "ENABLED", sizeof("ENABLED") - 1); - else - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "DISABLED", sizeof("DISABLED") - 1); + AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, row, column, str, strlen(str)); } -void FSM::SDLoggingOptionDisplayedSettingsState() +void FSM::ShowToggleOption(State::SettingsState settingsState, const char* title, uint8_t titleColumn, bool enabled) { _state.mainState = State::MainDynoState::SETTINGS_MENU; - _state.settingsState = State::SettingsState::SD_LOGGING_OPTION_DISPLAYED; + _state.settingsState = settingsState; ClearDisplay(); - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 0, 3, "SD LOGGING", sizeof("SD LOGGING") - 1); - - if (_sdLoggingEnabled) - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "ENABLED", sizeof("ENABLED") - 1); - else - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "DISABLED", sizeof("DISABLED") - 1); + WriteLine(0, titleColumn, title); + WriteLine(1, 4, enabled ? "ENABLED" : "DISABLED"); } -void FSM::SDLoggingOptionEditSettingsState() +void FSM::USBLoggingOptionDisplayedSettingsState() { - _state.mainState = State::MainDynoState::SETTINGS_MENU; - _state.settingsState = State::SettingsState::SD_LOGGING_OPTION_EDIT; - - ClearDisplay(); - - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 0, 3, "SD LOGGING", sizeof("SD LOGGING") - 1); - - if (_sdLoggingEnabled) - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "ENABLED", sizeof("ENABLED") - 1); - else - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "DISABLED", sizeof("DISABLED") - 1); + ShowToggleOption(State::SettingsState::USB_LOGGING_OPTION_DISPLAYED, "USB LOGGING", 2, _usbLoggingEnabled); } -void FSM::PIDOptionDisplayedSettingsState() +void FSM::SDLoggingOptionDisplayedSettingsState() { - _state.mainState = State::MainDynoState::SETTINGS_MENU; - _state.settingsState = State::SettingsState::PID_ENABLE_DISPLAYED; - - ClearDisplay(); - - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 0, 2, "PID LOGGING", sizeof("PID LOGGING") - 1); - - if (_pidOptionToggleableEnabled) - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "ENABLED", sizeof("ENABLED") - 1); - else - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "DISABLED", sizeof("DISABLED") - 1); + ShowToggleOption(State::SettingsState::SD_LOGGING_OPTION_DISPLAYED, "SD LOGGING", 3, _sdLoggingEnabled); } -void FSM::PIDOptionEditSettingsState() +void FSM::PIDOptionDisplayedSettingsState() { - _state.mainState = State::MainDynoState::SETTINGS_MENU; - _state.settingsState = State::SettingsState::PID_ENABLE_EDIT; - - ClearDisplay(); - - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 0, 2, "PID LOGGING", sizeof("PID LOGGING") - 1); - - if (_pidOptionToggleableEnabled) - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "ENABLED", sizeof("ENABLED") - 1); - else - AddToLumexLCDMessageQueue(WRITE_TO_DISPLAY, 1, 4, "DISABLED", sizeof("DISABLED") - 1); + ShowToggleOption(State::SettingsState::PID_ENABLE_DISPLAYED, "PID LOGGING", 2, _pidOptionToggleableEnabled); } void FSM::PIDDesiredRPMOptionDisplayedSettingsState() diff --git a/Core/Src/Tasks/SessionController/SessionController.cpp b/Core/Src/Tasks/SessionController/SessionController.cpp index 75cfab9..e0942a1 100644 --- a/Core/Src/Tasks/SessionController/SessionController.cpp +++ b/Core/Src/Tasks/SessionController/SessionController.cpp @@ -18,10 +18,17 @@ SessionController::SessionController(session_controller_os_task_queues* task_que _fsm(task_queues->lumex_lcd), _task_queues(task_queues), _usart1Mutex(usart1Mutex), + _force_data{}, + _optical_encoder_data{}, _prevUSBLoggingEnabled(false), _prevSDLoggingEnabled(false), _prevPIDEnabled(false), - _prevInSession(false) + _prevInSession(false), + _pidAckReceived(false), + _prevThrottleDutyCycle(0.0f), + _prevBpmDutyCycle(0.0f), + _prevForce(0.0f), + _prevAngularVelocity(0.0f) {} bool SessionController::CheckTaskQueuesValid() @@ -96,238 +103,192 @@ bool SessionController::Init(void) void SessionController::Run() { - forcesensor_output_data force_data; - optical_encoder_output_data optical_encoder_data; - - float prevThrottleDutyCycle = 0.0f; - float prevBpmDutyCycle = 0.0f; - float prevForce = 0.0f; - float prevAngularVelocity = 0.0f; - - memset(&force_data, 0, sizeof(force_data)); - memset(&optical_encoder_data, 0, sizeof(optical_encoder_data)); - - + // Send the PID controller its initial (disabled) target before entering the loop. session_controller_to_pid_controller pid_msg; pid_msg.enable_status = false; pid_msg.desired_angular_velocity = _fsm.GetDesiredAngularVelocity(); - - bool pidAckReceived = false; osMessageQueuePut(_task_queues->pid_controller, &pid_msg, 0, osWaitForever); - while(1) + while (1) { - - // First Handle Any User Inputs _fsm.HandleUserInputs(); - - // Get USB Enabled Status and enable USB Controller - bool usbLoggingEnabled = _fsm.GetUSBLoggingEnabledStatus(); - // Only if the status has changed - if (usbLoggingEnabled ^ _prevUSBLoggingEnabled) - { - #if USB_CONTROLLER_TASK_ENABLE - osMessageQueuePut(_task_queues->usb_controller, &usbLoggingEnabled, 0, osWaitForever); - #endif - _prevUSBLoggingEnabled = usbLoggingEnabled; - } - + SyncLoggingState(); + HandleSessionEdge(); - - // Get SD Card Enabled Status and enable SD Card Controller - bool SDLoggingEnabled = _fsm.GetSDLoggingEnabledStatus(); - // Only if the status has changed - if (SDLoggingEnabled ^ _prevSDLoggingEnabled) + // Everything below only runs during an active session. + if (!_fsm.GetInSessionStatus()) { - #if SD_CONTROLLER_TASK_ENABLE - osMessageQueuePut(_task_queues->sd_controller, &SDLoggingEnabled, 0, osWaitForever); - #endif - _prevSDLoggingEnabled = SDLoggingEnabled; + osDelay(SESSIONCONTROLLER_TASK_OSDELAY); + continue; } - - - bool InSessionStatus = _fsm.GetInSessionStatus(); - bool InSessionRisingEdge = InSessionStatus && !_prevInSession; - bool InSessionFallingEdge = !InSessionStatus && _prevInSession; - - // Get PID enabled status and enable PID Controller - bool PIDEnabled = _fsm.GetPIDEnabledModeStatus(); - bool PIDOptionToggleableEnabled = _fsm.GetPIDOptionToggleableEnabledStatus(); + SyncPidState(); + HandleManualControl(); + UpdateMetrics(); - // only run this code if the 'InSession' status has changed - if (InSessionRisingEdge || InSessionFallingEdge) - { - bool opticalEncoderEnable; - bool forceSensorEnable; + osDelay(SESSIONCONTROLLER_TASK_OSDELAY); + } +} - // enable things - if (InSessionRisingEdge) - { - opticalEncoderEnable = true; - forceSensorEnable = true; +void SessionController::SyncLoggingState() +{ + bool usbLoggingEnabled = _fsm.GetUSBLoggingEnabledStatus(); + if (usbLoggingEnabled != _prevUSBLoggingEnabled) + { + #if USB_CONTROLLER_TASK_ENABLE + osMessageQueuePut(_task_queues->usb_controller, &usbLoggingEnabled, 0, osWaitForever); + #endif + _prevUSBLoggingEnabled = usbLoggingEnabled; + } - _fsm.DisplayRpm(0); + bool sdLoggingEnabled = _fsm.GetSDLoggingEnabledStatus(); + if (sdLoggingEnabled != _prevSDLoggingEnabled) + { + #if SD_CONTROLLER_TASK_ENABLE + osMessageQueuePut(_task_queues->sd_controller, &sdLoggingEnabled, 0, osWaitForever); + #endif + _prevSDLoggingEnabled = sdLoggingEnabled; + } +} - _fsm.DisplayTorque(0); - _fsm.DisplayPower(0); +void SessionController::HandleSessionEdge() +{ + bool inSession = _fsm.GetInSessionStatus(); + bool risingEdge = inSession && !_prevInSession; + bool fallingEdge = !inSession && _prevInSession; - if (PIDOptionToggleableEnabled) _fsm.DisplayPIDEnabled(); - else if (_fsm.GetManualBpmModeStatus()) _fsm.DisplayManualBPMDutyCycle(); - else _fsm.DisplayManualThrottleDutyCycle(); + if (!risingEdge && !fallingEdge) + { + return; + } - } - - // disable things - else if (InSessionFallingEdge) - { - - opticalEncoderEnable = false; - forceSensorEnable = false; + bool sensorsEnable = risingEdge; - - session_controller_to_bpm bpmSettings; - bpmSettings.op = STOP_PWM; - bpmSettings.new_duty_cycle_percent = static_cast(0); + if (risingEdge) + { + _fsm.DisplayRpm(0); + _fsm.DisplayTorque(0); + _fsm.DisplayPower(0); - osMessageQueuePut(_task_queues->bpm_controller, &bpmSettings, 0, osWaitForever); - - } - - // Send enable or disable messages - osMessageQueuePut(_task_queues->optical_sensor, &opticalEncoderEnable, 0, osWaitForever); + if (_fsm.GetPIDOptionToggleableEnabledStatus()) _fsm.DisplayPIDEnabled(); + else if (_fsm.GetManualBpmModeStatus()) _fsm.DisplayManualBPMDutyCycle(); + else _fsm.DisplayManualThrottleDutyCycle(); + } + else // falling edge: stop the brake PWM + { + session_controller_to_bpm bpmSettings; + bpmSettings.op = STOP_PWM; + bpmSettings.new_duty_cycle_percent = 0.0f; + osMessageQueuePut(_task_queues->bpm_controller, &bpmSettings, 0, osWaitForever); + } - osMessageQueuePut(_task_queues->force_sensor, &forceSensorEnable, 0, osWaitForever); - + osMessageQueuePut(_task_queues->optical_sensor, &sensorsEnable, 0, osWaitForever); + osMessageQueuePut(_task_queues->force_sensor, &sensorsEnable, 0, osWaitForever); - _prevInSession = InSessionStatus; + _prevInSession = inSession; +} +void SessionController::SyncPidState() +{ + bool pidEnabled = _fsm.GetPIDEnabledModeStatus(); - } + if (pidEnabled != _prevPIDEnabled) + { + session_controller_to_pid_controller pid_msg; + pid_msg.enable_status = pidEnabled; + pid_msg.desired_angular_velocity = _fsm.GetDesiredAngularVelocity(); + _pidAckReceived = false; + osMessageQueuePut(_task_queues->pid_controller, &pid_msg, 0, osWaitForever); + _prevPIDEnabled = pidEnabled; + } - if (!InSessionStatus) - { - osDelay(SESSIONCONTROLLER_TASK_OSDELAY); - continue; - } + if (!_pidAckReceived) + { + GetLatestFromQueue(_task_queues->pid_controller_ack, &_pidAckReceived, sizeof(_pidAckReceived), 0); - // Only if the status has changed - if (PIDEnabled ^ _prevPIDEnabled) + // Runs once after PID is enabled and the controller has acknowledged. + if (_pidAckReceived) { - session_controller_to_pid_controller pid_msg; - pid_msg.enable_status = PIDEnabled; - pid_msg.desired_angular_velocity = _fsm.GetDesiredAngularVelocity(); - pidAckReceived = false; - osMessageQueuePut(_task_queues->pid_controller, &pid_msg, 0, osWaitForever); - _prevPIDEnabled = PIDEnabled; - - - } + if (pidEnabled) + { + session_controller_to_bpm bpmSettings{}; + bpmSettings.op = READ_FROM_PID; + osMessageQueuePut(_task_queues->bpm_controller, &bpmSettings, 0, osWaitForever); + } - if (!pidAckReceived) - { - GetLatestFromQueue(_task_queues->pid_controller_ack, &pidAckReceived, sizeof(pidAckReceived), 0); - // This should only run once PIDEnabled changes from false to true and once the ack has been received - if (pidAckReceived) + if (_fsm.GetPIDOptionToggleableEnabledStatus()) { - - if (PIDEnabled) - { - session_controller_to_bpm bpmSettings{}; - bpmSettings.op = READ_FROM_PID; - osMessageQueuePut(_task_queues->bpm_controller, &bpmSettings, 0, osWaitForever); - - } - - if (PIDOptionToggleableEnabled) - { - _fsm.DisplayPIDEnabled(); - } - - - } + _fsm.DisplayPIDEnabled(); + } } + } +} - - if (!PIDOptionToggleableEnabled) - { - - if (_fsm.GetManualThrottleModeStatus()) - { - // Always run since the PID controller could be turned off while in-session - float newThrottleDutyCycle = _fsm.GetDesiredThrottleDutyCycle(); - if (newThrottleDutyCycle != prevThrottleDutyCycle) - { - osMutexAcquire(_usart1Mutex, osWaitForever); +void SessionController::HandleManualControl() +{ + // Manual control only applies when the toggleable PID option is off. + if (_fsm.GetPIDOptionToggleableEnabledStatus()) + { + return; + } - uint8_t newDutyCycle255 = static_cast(newThrottleDutyCycle * 255.0f); - HAL_UART_Transmit(&huart1, &newDutyCycle255, sizeof(newDutyCycle255), HAL_MAX_DELAY); + if (_fsm.GetManualThrottleModeStatus()) + { + // Transmit the throttle duty cycle over UART only when it changes. + float newThrottleDutyCycle = _fsm.GetDesiredThrottleDutyCycle(); + if (newThrottleDutyCycle != _prevThrottleDutyCycle) + { + osMutexAcquire(_usart1Mutex, osWaitForever); - osMutexRelease(_usart1Mutex); + uint8_t newDutyCycle255 = static_cast(newThrottleDutyCycle * 255.0f); + HAL_UART_Transmit(&huart1, &newDutyCycle255, sizeof(newDutyCycle255), HAL_MAX_DELAY); - prevThrottleDutyCycle = newThrottleDutyCycle; - } - _fsm.DisplayManualThrottleDutyCycle(); - - } - else - { - - float newBpmDutyCycle = _fsm.GetDesiredBpmDutyCycle(); - - if (newBpmDutyCycle != prevBpmDutyCycle) - { - session_controller_to_bpm bpmSettings; - bpmSettings.op = START_PWM; - bpmSettings.new_duty_cycle_percent = newBpmDutyCycle; - osMessageQueuePut(_task_queues->bpm_controller, &bpmSettings, 0, osWaitForever); - prevBpmDutyCycle = newBpmDutyCycle; - } - _fsm.DisplayManualBPMDutyCycle(); - - } - + osMutexRelease(_usart1Mutex); + _prevThrottleDutyCycle = newThrottleDutyCycle; } - - // Get the most recent force sensor data - while(_forcesensor_buffer_reader.GetElementAndIncrementIndex(force_data)); - - // Get the most recent optical encoder data - while(_optical_encoder_buffer_reader.GetElementAndIncrementIndex(optical_encoder_data)); - - // TO UPDATE - float angularAcceleration = 0; - float angularVelocity = optical_encoder_data.angular_velocity; - float force = force_data.force; - - float torque = CalculateTorque(angularAcceleration, force, angularVelocity); - - - if (prevAngularVelocity != angularVelocity) + _fsm.DisplayManualThrottleDutyCycle(); + } + else + { + // Push the brake (BPM) duty cycle only when it changes. + float newBpmDutyCycle = _fsm.GetDesiredBpmDutyCycle(); + if (newBpmDutyCycle != _prevBpmDutyCycle) { - _fsm.DisplayRpm(optical_encoder_data.angular_velocity); - - if (prevForce != force) - { - _fsm.DisplayTorque(torque); - _fsm.DisplayPower(CalculatePower(torque, angularVelocity)); - prevForce = force; - } - prevAngularVelocity = angularVelocity; + session_controller_to_bpm bpmSettings; + bpmSettings.op = START_PWM; + bpmSettings.new_duty_cycle_percent = newBpmDutyCycle; + osMessageQueuePut(_task_queues->bpm_controller, &bpmSettings, 0, osWaitForever); + _prevBpmDutyCycle = newBpmDutyCycle; } + _fsm.DisplayManualBPMDutyCycle(); + } +} +void SessionController::UpdateMetrics() +{ + // Drain the circular buffers down to the most recent reading. + while (_forcesensor_buffer_reader.GetElementAndIncrementIndex(_force_data)); + while (_optical_encoder_buffer_reader.GetElementAndIncrementIndex(_optical_encoder_data)); - - - osDelay(SESSIONCONTROLLER_TASK_OSDELAY); + float angularAcceleration = 0; // TODO: derive angular acceleration from the optical encoder + float angularVelocity = _optical_encoder_data.angular_velocity; + float force = _force_data.force; - - + float torque = CalculateTorque(angularAcceleration, force, angularVelocity); - + if (_prevAngularVelocity != angularVelocity) + { + _fsm.DisplayRpm(_optical_encoder_data.angular_velocity); + if (_prevForce != force) + { + _fsm.DisplayTorque(torque); + _fsm.DisplayPower(CalculatePower(torque, angularVelocity)); + _prevForce = force; + } + _prevAngularVelocity = angularVelocity; } } diff --git a/Core/Src/Tasks/SessionController/input_manager_interrupts.c b/Core/Src/Tasks/SessionController/input_manager_interrupts.c index f25355a..ac8c691 100644 --- a/Core/Src/Tasks/SessionController/input_manager_interrupts.c +++ b/Core/Src/Tasks/SessionController/input_manager_interrupts.c @@ -9,27 +9,32 @@ volatile button_press_data button_press_circular_buffer[USER_INPUT_CIRCULAR_BUFF void register_rotary_encoder_input() { + // Called on a ROT_EN_A edge; the level of ROT_EN_B gives the rotation direction. + bool positive = (HAL_GPIO_ReadPin(ROT_EN_B_GPIO_Port, ROT_EN_B_Pin) == GPIO_PIN_SET); - // This function would be called on ROT_EN_A - bool positive; - // The state of ROT_EN_B would tell us the direction - if (HAL_GPIO_ReadPin(ROT_EN_B_GPIO_Port, ROT_EN_B_Pin)) + add_to_circular_buffer(ROT_EN_TICKS, positive); +} + +// Shared handler for momentary push-buttons (back, select): LED lights while held, +// and the press is queued on release. LEDs are active-low (RESET = on). +static void handle_momentary_button(GPIO_TypeDef* btn_port, uint16_t btn_pin, + GPIO_TypeDef* led_port, uint16_t led_pin, + button_opcode opcode) +{ + if (HAL_GPIO_ReadPin(btn_port, btn_pin) == GPIO_PIN_RESET) { - positive = true; + // Pressed: turn LED on. + HAL_GPIO_WritePin(led_port, led_pin, GPIO_PIN_RESET); } else { - positive = false; + // Released: turn LED off and queue the event for the session controller. + // POSITIVE is unused for these opcodes; see input_manager_interrupts.h. + HAL_GPIO_WritePin(led_port, led_pin, GPIO_PIN_SET); + add_to_circular_buffer(opcode, false); } - - // Add the number of changed ticks to the buffer - add_to_circular_buffer(ROT_EN_TICKS, positive); - - } - - void register_rotary_encoder_sw_input() { // if button is pressed @@ -52,45 +57,14 @@ void register_rotary_encoder_sw_input() void register_button_back_input() { - - // Get the current pin state of the back button - GPIO_PinState pin_state = HAL_GPIO_ReadPin(BTN_BACK_GPIO_Port, BTN_BACK_Pin); - if (pin_state == GPIO_PIN_RESET) - { - // Turn on LED - HAL_GPIO_WritePin(LED_BACK_GPIO_Port, LED_BACK_Pin, GPIO_PIN_RESET); - } - else - { - // Turn off LED - HAL_GPIO_WritePin(LED_BACK_GPIO_Port, LED_BACK_Pin, GPIO_PIN_SET); - - // Add to the circular buffer for the session controller to read later - // POSITIVE parameter does not matter here for the SELECT_INPUT. Read input_manager_interrupts.h file for explanation - add_to_circular_buffer(BTN_BACK, false); - - } + handle_momentary_button(BTN_BACK_GPIO_Port, BTN_BACK_Pin, + LED_BACK_GPIO_Port, LED_BACK_Pin, BTN_BACK); } void register_button_select_input() { - // Get the current pin state of the select button - GPIO_PinState pin_state = HAL_GPIO_ReadPin(BTN_SELECT_GPIO_Port, BTN_SELECT_Pin); - if (pin_state == GPIO_PIN_RESET) - { - // Turn on LED - HAL_GPIO_WritePin(LED_SELECT_GPIO_Port, LED_SELECT_Pin, GPIO_PIN_RESET); - } - else - { - // Turn off LED - HAL_GPIO_WritePin(LED_SELECT_GPIO_Port, LED_SELECT_Pin, GPIO_PIN_SET); - - // Add to the circular buffer for the session controller to read later - // POSITIVE parameter does not matter here for the SELECT_INPUT. Read input_manager_interrupts.h file for explanation - add_to_circular_buffer(BTN_SELECT, false); - - } + handle_momentary_button(BTN_SELECT_GPIO_Port, BTN_SELECT_Pin, + LED_SELECT_GPIO_Port, LED_SELECT_Pin, BTN_SELECT); } void register_button_brake_input() @@ -119,25 +93,20 @@ void register_button_brake_input() void add_to_circular_buffer(button_opcode opcode, bool positive) { - // Create the data struct to add button_press_data data_to_add; data_to_add.opcode = opcode; data_to_add.positive = positive; - // Disable all button interrupts. Due to the nature of interrupts, they can interrupt each other - // We cannot allow an interrupt to interrupt another while writing to the input_data buffer -// HAL_NVIC_DisableIRQ(EXTI3_IRQn); -// HAL_NVIC_DisableIRQ(EXTI4_IRQn); -// HAL_NVIC_DisableIRQ(EXTI9_5_IRQn); + // These handlers run in EXTI ISRs that can preempt one another, so the buffer write and + // index increment must be atomic. Mask all interrupts for the (very short) critical section, + // saving/restoring PRIMASK so we behave correctly even if called with interrupts already off. + uint32_t primask = __get_PRIMASK(); + __disable_irq(); - // Critical section: write to the circular buffer button_press_circular_buffer[interrupt_input_data_index] = data_to_add; interrupt_input_data_index = (interrupt_input_data_index + 1) % USER_INPUT_CIRCULAR_BUFFER_SIZE; - // renable the IRQs -// HAL_NVIC_EnableIRQ(EXTI3_IRQn); -// HAL_NVIC_EnableIRQ(EXTI4_IRQn); -// HAL_NVIC_EnableIRQ(EXTI9_5_IRQn); + __set_PRIMASK(primask); } volatile button_press_data* get_circular_buffer_data(uint32_t index)