From 02fadf39308c96900c8addc67ae00740f641e3d4 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sat, 2 Mar 2024 02:31:39 -0500 Subject: [PATCH 01/29] launch controller class implemented in state machine, utilizing mcu_status LC active flag --- include/dashboard.hpp | 13 +++- include/launch_controller.hpp | 33 +++++++++ include/pedal_handler.hpp | 1 + include/state_machine.hpp | 5 +- src/dashboard.cpp | 11 +++ src/launch_controller.cpp | 134 ++++++++++++++++++++++++++++++++++ src/main.cpp | 3 +- src/pedal_handler.cpp | 4 + src/state_machine.cpp | 71 ++++++++++++++++-- 9 files changed, 266 insertions(+), 9 deletions(-) create mode 100644 include/launch_controller.hpp create mode 100644 src/launch_controller.cpp diff --git a/include/dashboard.hpp b/include/dashboard.hpp index 56aaa6f..21bf15d 100644 --- a/include/dashboard.hpp +++ b/include/dashboard.hpp @@ -35,6 +35,17 @@ class Dashboard * @return false */ bool get_button(uint8_t button) {return (button_states & (0x1 << (button - 1)));} + /** + * @brief Get the button held duration object + * + * @param button + * @param duration_ms + * @return true + * @return false + */ + bool get_button_held_duration(uint8_t button, unsigned long duration_ms); + bool get_button_released_duration(uint8_t button, unsigned long duration_ms); + void updateDashCAN(); /** * @brief Get the button last pressed time object @@ -49,7 +60,7 @@ class Dashboard float last_received_timestamp = 0; private: uint8_t button_states; - elapsedMillis button_last_pressed_time[6] = {0,0,0,0,0,0}; + elapsedMillis button_last_pressed_time[6] = {0,0,0,0,0,0}; // Maybe this should be a map :skullemoji: void reset_all_button_timers(); }; diff --git a/include/launch_controller.hpp b/include/launch_controller.hpp new file mode 100644 index 0000000..0dfeda9 --- /dev/null +++ b/include/launch_controller.hpp @@ -0,0 +1,33 @@ +#ifndef LAUNCH_CONTROLLER_HPP +#define LAUNCH_CONTROLLER_HPP +#include + +enum class launchState +{ + IDLE = 0, + WAITING_TO_LAUNCH = 1, + LAUNCHING = 2, + FINISHED = 3 +}; + +class launchController +{ + +private: + const unsigned long launchControlMaxDuration = 1200; // milliseconds that the launching period will run for + unsigned long launchStartTime = 0; // ms the starting time of the launch + unsigned long launchCurrentTime = 0; // ms the current time of the launch + unsigned long launchElapsedTime = 0; // ms time elapsed from start of launch + int driverTorqueRequest = 0; // Driver torque command if using pedal travel + int lcTorqueRequest = 0; // launch control system torque command + int outputTorqueCommand = 0; // The ffinal torque command that will be output from the launch controller + bool disableTorqueCommanding = true; // to disable torque commanding in IDLE and WAIT states + launchState launchControlState; // state enum to control LC actions +public: + void initLaunchController(unsigned long sysTime); + launchState getState(); + launchState setState(launchState nextState, unsigned long sysTime); + int calculateTorque(unsigned long elapsedTime, int maxTorque); + void run(unsigned long sysTime, int &torqueRequest); +}; +#endif \ No newline at end of file diff --git a/include/pedal_handler.hpp b/include/pedal_handler.hpp index febf3cb..b4a603a 100644 --- a/include/pedal_handler.hpp +++ b/include/pedal_handler.hpp @@ -88,6 +88,7 @@ class PedalHandler void send_readings(); bool get_board_sensor_readings(); void read_pedal_values_debug(uint16_t value); + float getAppsTravel(); }; typedef struct pedal_thresholds_0_t diff --git a/include/state_machine.hpp b/include/state_machine.hpp index 585b071..0b49f4b 100644 --- a/include/state_machine.hpp +++ b/include/state_machine.hpp @@ -8,6 +8,7 @@ #include "dashboard.hpp" #include #include "parameters.hpp" +#include "launch_controller.hpp" class StateMachine { @@ -18,11 +19,13 @@ class StateMachine Dashboard *dash_; Metro *debug_; PedalHandler *pedals; + launchController *launchControl; Metro *pedal_check_; void set_state(MCU_status &mcu_status, MCU_STATE new_state); void send_state_msg(MCU_status &mcu_status); public: - StateMachine(Inverter *inv, Accumulator *acc, Metro *rs_tim, Dashboard *dash, Metro *debug, PedalHandler *pedals, Metro * ped_t) : pm100(inv), accumulator(acc), timer_ready_sound(rs_tim), dash_(dash), debug_(debug), pedals(pedals), pedal_check_(ped_t) {}; + StateMachine(Inverter *inv, Accumulator *acc, Metro *rs_tim, Dashboard *dash, Metro *debug, PedalHandler *pedals, launchController *lc,Metro *ped_t) + : pm100(inv), accumulator(acc), timer_ready_sound(rs_tim), dash_(dash), debug_(debug), pedals(pedals), launchControl(lc),pedal_check_(ped_t){}; void init_state_machine(MCU_status &mcu_status); void handle_state_machine(MCU_status &mcu_status); diff --git a/src/dashboard.cpp b/src/dashboard.cpp index ac9a9d1..6abed2b 100644 --- a/src/dashboard.cpp +++ b/src/dashboard.cpp @@ -39,6 +39,17 @@ void Dashboard::update_dash(uint8_t input) } this->set_buttons(input); } +// true if time since button pressed > duration_ms +bool Dashboard::get_button_held_duration(uint8_t button, unsigned long duration_ms) +{ + return this->get_button(button) && (this->get_button_last_pressed_time(button) >= duration_ms); +} +// true if time since a button was released > duration_ms (if you want to delay an action for example) +bool Dashboard::get_button_released_duration(uint8_t button, unsigned long duration_ms) +{ + return !(this->get_button(button)) && (this->get_button_last_pressed_time(button) >= duration_ms); +} + void Dashboard::updateDashCAN() { CAN_message_t rxMsg; diff --git a/src/launch_controller.cpp b/src/launch_controller.cpp new file mode 100644 index 0000000..4a51d97 --- /dev/null +++ b/src/launch_controller.cpp @@ -0,0 +1,134 @@ +#include "launch_controller.hpp" + +const float cal5 = -0.000000000000085; // Fifth +const float cal4 = 0.000000000114956; // Fourth +const float cal3 = 0.000000015913376; // Third +const float cal2 = 0.000011808754927; // Second +const float cal1 = 0.093415288604319; // First order of poly +const float calIntercept = 10.361085973494500; + +void launchController::initLaunchController(unsigned long sysTime) +{ + driverTorqueRequest = 0; + lcTorqueRequest = 0; + outputTorqueCommand = 0; + setState(launchState::IDLE,sysTime); +} +launchState launchController::getState() +{ + return this->launchControlState; +} +launchState launchController::setState(launchState nextState, unsigned long sysTime) +{ + launchState currentState = this->getState(); + if (currentState == nextState) + { + return currentState; + } + // State exit logic + switch (currentState) + { + case launchState::IDLE: + { + break; + } + case launchState::WAITING_TO_LAUNCH: + { + break; + } + case launchState::LAUNCHING: + { + break; + } + case launchState::FINISHED: + { + break; + } + } + + // State ENTRY logic + switch (nextState) + { + case launchState::IDLE: + { + this->disableTorqueCommanding = true; + break; + } + case launchState::WAITING_TO_LAUNCH: + { + this->disableTorqueCommanding = true; + break; + } + case launchState::LAUNCHING: + { + this->launchStartTime = sysTime; + this->disableTorqueCommanding = false; + break; + } + case launchState::FINISHED: + { + this->disableTorqueCommanding = true; + break; + } + } + return this->getState(); +} + +void launchController::run(unsigned long sysTime, int &torqueRequest) +{ + launchCurrentTime = sysTime; + launchElapsedTime = launchCurrentTime - launchStartTime; + + if (disableTorqueCommanding) + { + torqueRequest = 0; + } + + switch (this->getState()) + { + case launchState::IDLE: + { + // Do nothing + this->disableTorqueCommanding = true; + break; + } + case launchState::WAITING_TO_LAUNCH: + { + // Do nothing? + this->disableTorqueCommanding = true; + break; + } + case launchState::LAUNCHING: + { + if (launchElapsedTime > launchControlMaxDuration) + { + this->setState(launchState::FINISHED,sysTime); + } + outputTorqueCommand = this->calculateTorque(launchElapsedTime, torqueRequest); + + break; + } + case launchState::FINISHED: + { + this->disableTorqueCommanding = true; + break; + } + } +} + +int launchController::calculateTorque(unsigned long elapsedTime, int maxTorque) +{ + int torqueOut = 0; + // @jstri114 peep dis + // lcTorqueRequest = (cal5 * pow(elapsedTime, 5)) + (cal4 * pow(elapsedTime, 4)) + (cal3 * pow(elapsedTime, 3)) + (cal2 * pow(elapsedTime, 2)) + (cal1 * (elapsedTime)) + calIntercept; + lcTorqueRequest = (1 / 250) * (elapsedTime) + calIntercept; // Performs the calibration curve math, shrimple linear for now + if (lcTorqueRequest > maxTorque) + { + torqueOut = maxTorque; + } + else + { + torqueOut = lcTorqueRequest; + } + return torqueOut; +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 441369c..20e567d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -64,7 +64,8 @@ Dashboard dash; Inverter pm100(&timer_mc_kick_timer, &timer_inverter_enable, &timer_motor_controller_send, &timer_current_limit_send, &dash); Accumulator accum(&precharge_timeout,&ksu_can); PedalHandler pedals(&timer_debug_pedals_raw, &pedal_out_20hz, &pedal_out_1hz, &speedPID, ¤t_rpm, &set_rpm, &throttle_out, &wsfl, &wsfr); -StateMachine state_machine(&pm100, &accum, &timer_ready_sound, &dash, &debug_tim, &pedals, &pedal_check); +launchController launchControl; +StateMachine state_machine(&pm100, &accum, &timer_ready_sound, &dash, &debug_tim, &pedals, &launchControl, &pedal_check); MCU_status mcu_status = MCU_status(); static CAN_message_t mcu_status_msg; diff --git a/src/pedal_handler.cpp b/src/pedal_handler.cpp index 09cbc84..07c0ea4 100644 --- a/src/pedal_handler.cpp +++ b/src/pedal_handler.cpp @@ -22,6 +22,10 @@ void PedalHandler::run_pedals() this->bse1.sensor_run(); } +float PedalHandler::getAppsTravel() +{ + return (this->apps1.getTravelRatio() + this->apps2.getTravelRatio())/2; +} /** * @brief calculate torque to be commanded based on accel pedal position * diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 651e762..ef94361 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -65,7 +65,7 @@ void StateMachine::set_state(MCU_status &mcu_status, MCU_STATE new_state) // disable lowside outputs (pump, etc.) digitalWrite(LOWSIDE1, LOW); digitalWrite(LOWSIDE2, LOW); - + mcu_status.set_launch_ctrl_active(0); // disable launch control if it was active break; } } @@ -140,10 +140,10 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) pedals->send_readings(); - // If dash button is on and has been on for + // If dash button is on and has been on for if (dash_->get_button(6) && (dash_->get_button_last_pressed_time(6)) > 750) { - dash_->set_button_last_pressed_time(0,6); + dash_->set_button_last_pressed_time(0, 6); mcu_status.toggle_max_torque(mcu_status.get_torque_mode()); mcu_status.set_max_torque(torque_mode_list[mcu_status.get_torque_mode() - 1]); send_state_msg(mcu_status); @@ -242,14 +242,14 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) #if USE_INVERTER if (!pm100->check_TS_active()) { - set_state(mcu_status, MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); // uncomet stage 1 + set_state(mcu_status, MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } #else if (false) { } // dummy #endif - else if (accumulator->check_precharge_timeout()) // uncomet stage 1 + else if (accumulator->check_precharge_timeout()) { set_state(mcu_status, MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); @@ -360,10 +360,69 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) } // Torque calc always runs in the superloop - + // Toggle launch control if button 2 & 6 are held for 1 second, while brake is pressed + if (dash_->get_button_held_duration(2, 1000) && dash_->get_button_held_duration(6, 1000) && mcu_status.get_brake_pedal_active()) + { + // Toggle launch control (allows deactivating if sitting in it) + mcu_status.set_launch_ctrl_active(!(mcu_status.get_launch_ctrl_active())); + // Reset the launch controller each time we toggle it + launchControl->initLaunchController(millis()); +#if DEBUG + Serial.printf("DEBUG: Set launch control to %d", mcu_status.get_launch_ctrl_active()); +#endif + } if (mcu_status.get_launch_ctrl_active()) { + // does it make mroe sense to run the launch controller here? + launchControl->run(millis(), calculated_torque); // Do launch control things + switch (launchControl->getState()) + { + case launchState::IDLE: + { + // If button is held for 1 second, APPS is floored (90%), brake is not active, and calc torque is not 0 (0 would indicate there is a fault present) + // THEN: go to WAITING_TO_LAUNCH + if (dash_->get_button_held_duration(6, 1000) && (pedals->getAppsTravel() > 0.9) && !(mcu_status.get_brake_pedal_active()) && calculated_torque > 0) + { + launchControl->setState(launchState::WAITING_TO_LAUNCH,millis()); + break; + } + break; + } + case launchState::WAITING_TO_LAUNCH: + { + // If gas is released, return to IDLE + if ((pedals->getAppsTravel() > 0.9) || (calculated_torque <=(0.9*600))) + { + launchControl->setState(launchState::IDLE,millis()); + break; + } + // If gas is still pinned and button has been relased for 500ms, start LAUNCHING + if ((pedals->getAppsTravel()> 0.9) && dash_->get_button_released_duration(6,500)) + { + launchControl->setState(launchState::LAUNCHING,millis()); + } + break; + } + case launchState::LAUNCHING: + { + if ((mcu_status.get_brake_pedal_active())) + { + // Terminate launch control early if the brake is pressed + launchControl->setState(launchState::FINISHED,millis()); + break; + } + // TODO do i run the launch controller here? + // launchControl->run(millis(),calculated_torque); + break; + } + case launchState::FINISHED: + { + // Finished! exit launch control active! + mcu_status.set_launch_ctrl_active(0); + break; + } + } } #if USE_INVERTER pm100->calc_and_send_current_limit(pm100->getmcBusVoltage(), DISCHARGE_POWER_LIM, CHARGE_POWER_LIM); From 530f3d9ddd72e22412f5838c1e53eecd5d9e573a Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sat, 2 Mar 2024 04:41:52 -0500 Subject: [PATCH 02/29] button "held time" needs to be reset when checked otherwise it will be true nonstop --- src/dashboard.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/dashboard.cpp b/src/dashboard.cpp index 6abed2b..e8db50f 100644 --- a/src/dashboard.cpp +++ b/src/dashboard.cpp @@ -42,12 +42,16 @@ void Dashboard::update_dash(uint8_t input) // true if time since button pressed > duration_ms bool Dashboard::get_button_held_duration(uint8_t button, unsigned long duration_ms) { - return this->get_button(button) && (this->get_button_last_pressed_time(button) >= duration_ms); + bool wasHeld = this->get_button(button) && (this->get_button_last_pressed_time(button) >= duration_ms); + set_button_last_pressed_time(0,button); + return wasHeld; } // true if time since a button was released > duration_ms (if you want to delay an action for example) bool Dashboard::get_button_released_duration(uint8_t button, unsigned long duration_ms) { - return !(this->get_button(button)) && (this->get_button_last_pressed_time(button) >= duration_ms); + bool wasReleased = !(this->get_button(button)) && (this->get_button_last_pressed_time(button) >= duration_ms); + set_button_last_pressed_time(0,button); + return wasReleased; } void Dashboard::updateDashCAN() From 5d46ddc46987df74cd480e7bed7138d5b84845d3 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sat, 2 Mar 2024 04:51:40 -0500 Subject: [PATCH 03/29] add launch control mermaid chart to README.md --- README.md | 37 ++++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 79321a8..5d48d40 100644 --- a/README.md +++ b/README.md @@ -91,4 +91,39 @@ flowchart TD P --> Q(RETURN CALCULATED TORQUE) -``` \ No newline at end of file +``` + +## launch control state diagram + +```mermaid + +stateDiagram-v2 + + rtd : READY_TO_DRIVE + [*]-->rtd + rtd --> checkButtons + checkButtons-->toggle_lc:toggle lc if specific buttons are held + state toggle_lc{ + toggle_get_lc_active-->set_lc_active(1):set to 1 if it was 0 + toggle_get_lc_active-->set_lc_active(0):set to 0 if it was 1 + set_lc_active(1)-->[*] + set_lc_active(0)-->[*] + } + toggle_lc -->get_lc_active + checkButtons-->get_lc_active:if buttons are not held go to check lc state + get_lc_active-->launchState:mcu_status.get_launch_ctrl_active returns 1 + get_lc_active-->command_torque:mcu_status.get_launch_ctrl_active returns 0 + state launchState{ + idle-->waiting:gas pressed, button pressed, and no implaus + idle-->[*] + waiting-->idle: gas released or implaus + waiting-->[*] + waiting-->launching: gas still pressed and button released + launching-->finished: brake pressed (cancel early) + launching-->finished: time completed + launching-->[*]:launching in progress + finished-->[*]:launch control done, set launch_ctrl_active to 0 + } + launchState-->command_torque:deactivate LC when finished + command_torque-->rtd:return to start of RTD loop +``` From 0b9d192a04733268bc44e4decf4cc40c60bd3d00 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Fri, 15 Mar 2024 13:33:14 -0400 Subject: [PATCH 04/29] add LC tests, fix some state logic --- .github/workflows/main.yml | 7 ++- include/launch_controller.hpp | 3 +- platformio.ini | 3 ++ src/launch_controller.cpp | 24 +++++---- src/state_machine.cpp | 13 ++--- test/test_systems/test_launch_control.h | 65 +++++++++++++++++++++++++ test/test_systems/test_main.cpp | 14 ++++++ test/test_systems/test_mcu_state.h | 14 ++++++ 8 files changed, 125 insertions(+), 18 deletions(-) create mode 100644 test/test_systems/test_launch_control.h create mode 100644 test/test_systems/test_main.cpp create mode 100644 test/test_systems/test_mcu_state.h diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index af73378..c2cfade 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -3,7 +3,7 @@ name: PlatformIO CI on: [push] jobs: - build-and-upload: + test-build-upload: runs-on: ubuntu-latest steps: @@ -20,8 +20,11 @@ jobs: - name: Install PlatformIO Core run: pip install --upgrade platformio + - name: run platformio tests + run: pio test -e test_env -v + - name: Build PlatformIO Project - run: pio run + run: pio run -e teensy41 -v - name: Upload Build Artifact uses: actions/upload-artifact@v3 diff --git a/include/launch_controller.hpp b/include/launch_controller.hpp index 0dfeda9..994004c 100644 --- a/include/launch_controller.hpp +++ b/include/launch_controller.hpp @@ -26,8 +26,9 @@ class launchController public: void initLaunchController(unsigned long sysTime); launchState getState(); - launchState setState(launchState nextState, unsigned long sysTime); + launchState setState(const launchState nextState, unsigned long sysTime); int calculateTorque(unsigned long elapsedTime, int maxTorque); void run(unsigned long sysTime, int &torqueRequest); + int getTorqueOutput() const { return outputTorqueCommand;}; }; #endif \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 1707f92..a852a80 100644 --- a/platformio.ini +++ b/platformio.ini @@ -7,6 +7,9 @@ ; ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html +[env:test_env] +platform = native +test_framework = googletest [env:teensy41] platform = teensy diff --git a/src/launch_controller.cpp b/src/launch_controller.cpp index 4a51d97..50e3fd4 100644 --- a/src/launch_controller.cpp +++ b/src/launch_controller.cpp @@ -12,13 +12,13 @@ void launchController::initLaunchController(unsigned long sysTime) driverTorqueRequest = 0; lcTorqueRequest = 0; outputTorqueCommand = 0; - setState(launchState::IDLE,sysTime); + this->setState(launchState::IDLE,sysTime); } launchState launchController::getState() { return this->launchControlState; } -launchState launchController::setState(launchState nextState, unsigned long sysTime) +launchState launchController::setState(const launchState nextState, unsigned long sysTime) { launchState currentState = this->getState(); if (currentState == nextState) @@ -46,6 +46,7 @@ launchState launchController::setState(launchState nextState, unsigned long sysT } } + launchControlState = nextState; // State ENTRY logic switch (nextState) { @@ -67,7 +68,7 @@ launchState launchController::setState(launchState nextState, unsigned long sysT } case launchState::FINISHED: { - this->disableTorqueCommanding = true; + this->disableTorqueCommanding = false; break; } } @@ -78,10 +79,11 @@ void launchController::run(unsigned long sysTime, int &torqueRequest) { launchCurrentTime = sysTime; launchElapsedTime = launchCurrentTime - launchStartTime; + driverTorqueRequest = torqueRequest; if (disableTorqueCommanding) { - torqueRequest = 0; + outputTorqueCommand = 0; } switch (this->getState()) @@ -100,17 +102,19 @@ void launchController::run(unsigned long sysTime, int &torqueRequest) } case launchState::LAUNCHING: { - if (launchElapsedTime > launchControlMaxDuration) + if (launchElapsedTime >= launchControlMaxDuration) { this->setState(launchState::FINISHED,sysTime); + break; } - outputTorqueCommand = this->calculateTorque(launchElapsedTime, torqueRequest); + outputTorqueCommand = this->calculateTorque(launchElapsedTime, driverTorqueRequest); break; } case launchState::FINISHED: { - this->disableTorqueCommanding = true; + outputTorqueCommand = driverTorqueRequest; + this->disableTorqueCommanding = false; break; } } @@ -120,8 +124,10 @@ int launchController::calculateTorque(unsigned long elapsedTime, int maxTorque) { int torqueOut = 0; // @jstri114 peep dis - // lcTorqueRequest = (cal5 * pow(elapsedTime, 5)) + (cal4 * pow(elapsedTime, 4)) + (cal3 * pow(elapsedTime, 3)) + (cal2 * pow(elapsedTime, 2)) + (cal1 * (elapsedTime)) + calIntercept; - lcTorqueRequest = (1 / 250) * (elapsedTime) + calIntercept; // Performs the calibration curve math, shrimple linear for now + lcTorqueRequest = (cal5 * pow(elapsedTime, 5)) + (cal4 * pow(elapsedTime, 4)) + (cal3 * pow(elapsedTime, 3)) + (cal2 * pow(elapsedTime, 2)) + (cal1 * (elapsedTime)) + calIntercept; + lcTorqueRequest *= 10; // Scale for inverter + // this linear equation goes nowhere + // lcTorqueRequest = (1.0/25.0) * (elapsedTime) + calIntercept; // Performs the calibration curve math, shrimple linear for now if (lcTorqueRequest > maxTorque) { torqueOut = maxTorque; diff --git a/src/state_machine.cpp b/src/state_machine.cpp index ef94361..c4a86a7 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -375,14 +375,15 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) { // does it make mroe sense to run the launch controller here? launchControl->run(millis(), calculated_torque); + calculated_torque = launchControl->getTorqueOutput(); // Do launch control things switch (launchControl->getState()) { case launchState::IDLE: { - // If button is held for 1 second, APPS is floored (90%), brake is not active, and calc torque is not 0 (0 would indicate there is a fault present) + // If button is held for 1 second, APPS is floored (90%), brake is not active, and impl_occ is false // THEN: go to WAITING_TO_LAUNCH - if (dash_->get_button_held_duration(6, 1000) && (pedals->getAppsTravel() > 0.9) && !(mcu_status.get_brake_pedal_active()) && calculated_torque > 0) + if (dash_->get_button_held_duration(6, 1000) && (pedals->getAppsTravel() > 0.9) && !(mcu_status.get_brake_pedal_active()) && !impl_occ) { launchControl->setState(launchState::WAITING_TO_LAUNCH,millis()); break; @@ -392,12 +393,12 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) case launchState::WAITING_TO_LAUNCH: { // If gas is released, return to IDLE - if ((pedals->getAppsTravel() > 0.9) || (calculated_torque <=(0.9*600))) + if ((pedals->getAppsTravel() < 0.5) || (calculated_torque <=(0.5*600))) // TODO be less redundant? { launchControl->setState(launchState::IDLE,millis()); break; } - // If gas is still pinned and button has been relased for 500ms, start LAUNCHING + // If gas is still pinned and button has been released for 500ms, start LAUNCHING if ((pedals->getAppsTravel()> 0.9) && dash_->get_button_released_duration(6,500)) { launchControl->setState(launchState::LAUNCHING,millis()); @@ -406,9 +407,9 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) } case launchState::LAUNCHING: { - if ((mcu_status.get_brake_pedal_active())) + if ((mcu_status.get_brake_pedal_active()) || impl_occ) { - // Terminate launch control early if the brake is pressed + // Terminate launch control early if the brake is pressed or there was a pedal fault launchControl->setState(launchState::FINISHED,millis()); break; } diff --git a/test/test_systems/test_launch_control.h b/test/test_systems/test_launch_control.h new file mode 100644 index 0000000..1e37537 --- /dev/null +++ b/test/test_systems/test_launch_control.h @@ -0,0 +1,65 @@ +#ifndef LAUNCH_CONTROL_TEST +#define LAUNCH_CONTROL_TEST +#include +#include +#include +#include "launch_controller.hpp" +#include "launch_controller.cpp" + +TEST(lcTesting, test_launch_control_exists) +{ + launchController launchControl; + unsigned long systime = 0; + launchControl.initLaunchController(systime); + EXPECT_EQ(launchControl.getState(), launchState::IDLE); + launchControl.setState(launchState::WAITING_TO_LAUNCH,0); + EXPECT_EQ(launchControl.getState(), launchState::WAITING_TO_LAUNCH); + launchControl.setState(launchState::LAUNCHING,0); + EXPECT_EQ(launchControl.getState(), launchState::LAUNCHING); + launchControl.setState(launchState::FINISHED,0); + EXPECT_EQ(launchControl.getState(), launchState::FINISHED); +} + +TEST(lcTesting, test_lc_notorque) +{ + launchController lc; + unsigned long t = 0; + lc.initLaunchController(t); + int torque = 2400; + lc.run(t,torque); + lc.getTorqueOutput(); + EXPECT_EQ(lc.getTorqueOutput(),0); + + torque= 2400; + t++; + launchState nextState = lc.setState(launchState::WAITING_TO_LAUNCH,t); + lc.run(t,torque); + EXPECT_EQ(lc.getTorqueOutput(),0); + + torque= 2400; + t++; + nextState = lc.setState(launchState::LAUNCHING,t); + lc.run(t,torque); + EXPECT_NE(lc.getTorqueOutput(),0); + +} + +TEST(lcTesting, test_lc_ramp) +{ + launchController lc; + unsigned long t = 0; + int nm = 2400; + lc.initLaunchController(t); + lc.setState(launchState::LAUNCHING,t); + std::cout << "time" << "," << "torqueOut" << "\n"; + + for (int i = 0; i < 1400; i += 10) + { + lc.run(i,nm); + std::cout << i << "," << lc.getTorqueOutput() << "\n"; + + } + +} + +#endif \ No newline at end of file diff --git a/test/test_systems/test_main.cpp b/test/test_systems/test_main.cpp new file mode 100644 index 0000000..c7088b8 --- /dev/null +++ b/test/test_systems/test_main.cpp @@ -0,0 +1,14 @@ +#include +#include "test_launch_control.h" +#include "test_mcu_state.h" + +int main(int argc, char **argv) +{ + + + testing::InitGoogleTest(&argc, argv); + if (RUN_ALL_TESTS()) + ; + // Always return zero-code and allow PlatformIO to parse results + return 0; +} diff --git a/test/test_systems/test_mcu_state.h b/test/test_systems/test_mcu_state.h new file mode 100644 index 0000000..3acc989 --- /dev/null +++ b/test/test_systems/test_mcu_state.h @@ -0,0 +1,14 @@ +#ifndef MCU_STATUS_TEST +#define MCU_STATUS_TEST +#include +#include +#include "MCU_status.hpp" + +TEST(MCUstatusTesting,is_at_startup) +{ + MCU_status mcu_status; + MCU_STATE init_state = mcu_status.get_state(); + EXPECT_EQ(init_state, MCU_STATE::STARTUP); +} + +#endif \ No newline at end of file From 1be9fcdc0314b78a30f4a70da246c9bab76dfbfe Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Fri, 15 Mar 2024 13:49:33 -0400 Subject: [PATCH 05/29] set default initialization values for all member vars of MCU_status --- include/MCU_status.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/include/MCU_status.hpp b/include/MCU_status.hpp index 8b762f0..7ebe9b5 100644 --- a/include/MCU_status.hpp +++ b/include/MCU_status.hpp @@ -18,7 +18,13 @@ enum class MCU_STATE // @Parseclass @Custom(parse_mcu_enums) class MCU_status { public: - MCU_status() = default; + MCU_status() : + shutdown_states(0xff), + pedal_states(0), + ecu_states(0), + max_torque(0), + torque_mode(0), + distance_travelled(0) {} MCU_status(const uint8_t buf[8]) { load(buf); } inline void load(const uint8_t buf[]) { memcpy(this, buf, sizeof(*this)); } From 64f915e571a77944c9b58d42b8d86bad3c691d66 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Wed, 27 Mar 2024 16:48:54 -0400 Subject: [PATCH 06/29] create derived launch control classes and simplify lc state functions --- include/common_structs.h | 9 ++ include/launch_controller.hpp | 65 ++++++++--- include/pedal_handler.hpp | 5 +- platformio.ini | 13 ++- src/inverter.cpp | 4 +- src/launch_controller.cpp | 141 ++++++++---------------- src/pedal_handler.cpp | 4 +- src/state_machine.cpp | 14 +-- test/test_systems/test_launch_control.h | 17 +-- 9 files changed, 142 insertions(+), 130 deletions(-) create mode 100644 include/common_structs.h diff --git a/include/common_structs.h b/include/common_structs.h new file mode 100644 index 0000000..0da5801 --- /dev/null +++ b/include/common_structs.h @@ -0,0 +1,9 @@ +#ifndef COMMON_STRUCTS_H +#define COMMON_STRUCTS_H +typedef struct wheelSpeeds_s { + float fl; + float fr; + float rl; + float rr; +} wheelSpeeds_s; +#endif \ No newline at end of file diff --git a/include/launch_controller.hpp b/include/launch_controller.hpp index 994004c..3d7f777 100644 --- a/include/launch_controller.hpp +++ b/include/launch_controller.hpp @@ -1,6 +1,8 @@ #ifndef LAUNCH_CONTROLLER_HPP #define LAUNCH_CONTROLLER_HPP #include +#include +#include enum class launchState { @@ -15,20 +17,55 @@ class launchController private: const unsigned long launchControlMaxDuration = 1200; // milliseconds that the launching period will run for - unsigned long launchStartTime = 0; // ms the starting time of the launch - unsigned long launchCurrentTime = 0; // ms the current time of the launch - unsigned long launchElapsedTime = 0; // ms time elapsed from start of launch - int driverTorqueRequest = 0; // Driver torque command if using pedal travel - int lcTorqueRequest = 0; // launch control system torque command - int outputTorqueCommand = 0; // The ffinal torque command that will be output from the launch controller - bool disableTorqueCommanding = true; // to disable torque commanding in IDLE and WAIT states - launchState launchControlState; // state enum to control LC actions + unsigned long launchStartTime = 0; // ms the starting time of the launch + unsigned long launchCurrentTime = 0; // ms the current time of the launch + unsigned long launchElapsedTime = 0; // ms time elapsed from start of launch + int driverTorqueRequest = 0; // Driver torque command if using pedal travel + int lcTorqueRequest = 0; // launch control system torque command + int outputTorqueCommand = 0; // The final torque command that will be output from the launch controller + bool disableTorqueCommanding = true; // to disable torque commanding in IDLE and WAIT states + launchState launchControlState; // state enum to control LC actions public: - void initLaunchController(unsigned long sysTime); - launchState getState(); - launchState setState(const launchState nextState, unsigned long sysTime); - int calculateTorque(unsigned long elapsedTime, int maxTorque); - void run(unsigned long sysTime, int &torqueRequest); - int getTorqueOutput() const { return outputTorqueCommand;}; + void initLaunchController(unsigned long sysTime); // general to all launchControllers + launchState getState(); // general + launchState setState(const launchState nextState, unsigned long sysTime); // general + virtual int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); + virtual void run(unsigned long sysTime, int &torqueRequest, wheelSpeeds_s &wheelSpeedData); // instance specific + int getTorqueOutput() const { return outputTorqueCommand; }; // general +}; + +class lc_lut : public launchController +{ +private: + const float cal5 = -0.000000000000085; // Fifth + const float cal4 = 0.000000000114956; // Fourth + const float cal3 = 0.000000015913376; // Third + const float cal2 = 0.000011808754927; // Second + const float cal1 = 0.093415288604319; // First order of poly + const float calIntercept = 10.361085973494500; + +public: + int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); +}; +class lc_pid : public launchController +{ +private: + const double tireSlipLow = 0.05; + const double tireSlipHigh = 0.2; + double d_kp = 1.5; + double d_ki = 0; + double d_kd = 0.5; + const double output_min = 0.6; // Minimum output of the PID controller + const double output_max = 1.0; // Max output of the PID controller + double input, setpoint, output; + AutoPID pid; + +public: + lc_pid() : pid(&input, &setpoint, &output, output_min, output_max, d_kp, d_ki, d_kd) + { + pid.setBangBang(tireSlipLow); + pid.setTimeStep(1); + } + int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); }; #endif \ No newline at end of file diff --git a/include/pedal_handler.hpp b/include/pedal_handler.hpp index b4a603a..d2013d8 100644 --- a/include/pedal_handler.hpp +++ b/include/pedal_handler.hpp @@ -13,6 +13,7 @@ #include "parameters.hpp" #include "KS2eVCUgpios.hpp" #include "FlexCAN_util.hpp" +#include // check that the pedals are reading within 10% of each other // sum of the two readings should be within 10% of the average travel @@ -83,8 +84,8 @@ class PedalHandler void run_pedals(); void ws_run(); void update_wheelspeed(unsigned long current_time_millis, wheelspeeds_t *ws, FreqMeasureMulti *freq); - double get_wsfl(); - double get_wsfr(); + float get_wsfl(); + float get_wsfr(); void send_readings(); bool get_board_sensor_readings(); void read_pedal_values_debug(uint16_t value); diff --git a/platformio.ini b/platformio.ini index a852a80..0598bd7 100644 --- a/platformio.ini +++ b/platformio.ini @@ -10,15 +10,20 @@ [env:test_env] platform = native test_framework = googletest +lib_compat_mode = off +; Add libs on a case by case basis - +lib_deps = + https://github.com/KSU-MS/ksu-autoPID [env:teensy41] platform = teensy board = teensy41 framework = arduino -; teensy-cli gives some computers problems when uploading -; upload_protocol = teensy-cli lib_deps = - r-downing/AutoPID @ ^1.0.3 + https://github.com/KSU-MS/ksu-autoPID https://github.com/PaulStoffregen/FreqMeasureMulti https://github.com/KSU-MS/pio-git-hash-gen - https://github.com/KSU-MS/ksu-ms-dbc/releases/download/14/can_lib.tar.gz \ No newline at end of file + https://github.com/KSU-MS/ksu-ms-dbc/releases/download/14/can_lib.tar.gz + +; teensy-cli gives some computers problems when uploading +; upload_protocol = teensy-cli diff --git a/src/inverter.cpp b/src/inverter.cpp index 9c776b2..7fa56b9 100644 --- a/src/inverter.cpp +++ b/src/inverter.cpp @@ -116,8 +116,8 @@ void Inverter::writeEnableNoTorque() */ bool Inverter::command_torque(int torque) { - uint8_t torquePart1 = torque % 256; - uint8_t torquePart2 = torque / 256; + uint8_t torquePart1 = torque & 0xFF; + uint8_t torquePart2 = torque >> 8; uint8_t angularVelocity1 = 0, angularVelocity2 = 0; bool emraxDirection = true; // true for forward, false for reverse bool inverterEnable = true; // go brrr diff --git a/src/launch_controller.cpp b/src/launch_controller.cpp index 50e3fd4..86118f3 100644 --- a/src/launch_controller.cpp +++ b/src/launch_controller.cpp @@ -1,11 +1,5 @@ #include "launch_controller.hpp" -const float cal5 = -0.000000000000085; // Fifth -const float cal4 = 0.000000000114956; // Fourth -const float cal3 = 0.000000015913376; // Third -const float cal2 = 0.000011808754927; // Second -const float cal1 = 0.093415288604319; // First order of poly -const float calIntercept = 10.361085973494500; void launchController::initLaunchController(unsigned long sysTime) { @@ -25,116 +19,79 @@ launchState launchController::setState(const launchState nextState, unsigned lon { return currentState; } - // State exit logic - switch (currentState) - { - case launchState::IDLE: - { - break; - } - case launchState::WAITING_TO_LAUNCH: - { - break; - } - case launchState::LAUNCHING: - { - break; - } - case launchState::FINISHED: - { - break; - } - } - - launchControlState = nextState; - // State ENTRY logic - switch (nextState) - { - case launchState::IDLE: - { - this->disableTorqueCommanding = true; - break; - } - case launchState::WAITING_TO_LAUNCH: - { - this->disableTorqueCommanding = true; - break; - } - case launchState::LAUNCHING: - { - this->launchStartTime = sysTime; - this->disableTorqueCommanding = false; - break; - } - case launchState::FINISHED: - { - this->disableTorqueCommanding = false; - break; - } - } + this->launchControlState = nextState; + this->launchStartTime = sysTime; return this->getState(); } -void launchController::run(unsigned long sysTime, int &torqueRequest) +void launchController::run(unsigned long sysTime, int &torqueRequest, wheelSpeeds_s &wheelSpeedData) { launchCurrentTime = sysTime; launchElapsedTime = launchCurrentTime - launchStartTime; driverTorqueRequest = torqueRequest; - if (disableTorqueCommanding) - { - outputTorqueCommand = 0; - } - - switch (this->getState()) - { - case launchState::IDLE: - { - // Do nothing - this->disableTorqueCommanding = true; - break; - } - case launchState::WAITING_TO_LAUNCH: - { - // Do nothing? - this->disableTorqueCommanding = true; - break; - } - case launchState::LAUNCHING: + if (this->getState() == launchState::LAUNCHING) { if (launchElapsedTime >= launchControlMaxDuration) { this->setState(launchState::FINISHED,sysTime); - break; } - outputTorqueCommand = this->calculateTorque(launchElapsedTime, driverTorqueRequest); - - break; + outputTorqueCommand = this->calculateTorque(launchElapsedTime, driverTorqueRequest, wheelSpeedData); + // Clamp output torque + if (outputTorqueCommand > driverTorqueRequest) + { + outputTorqueCommand = driverTorqueRequest; + } } - case launchState::FINISHED: + else if (this->getState() == launchState::FINISHED) { outputTorqueCommand = driverTorqueRequest; - this->disableTorqueCommanding = false; - break; - } } + else { outputTorqueCommand = 0; } // Set output to zero if not in LAUNCHING or FINISHED } -int launchController::calculateTorque(unsigned long elapsedTime, int maxTorque) +// Torque in = torque out in base LC class +int launchController::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) { - int torqueOut = 0; + float torqueOut = 0; + torqueOut = maxTorque; + return static_cast(torqueOut); +} + +// Calculate the launch control system's ideal torque output +int lc_lut::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) +{ + float torqueOut = 0; // @jstri114 peep dis - lcTorqueRequest = (cal5 * pow(elapsedTime, 5)) + (cal4 * pow(elapsedTime, 4)) + (cal3 * pow(elapsedTime, 3)) + (cal2 * pow(elapsedTime, 2)) + (cal1 * (elapsedTime)) + calIntercept; - lcTorqueRequest *= 10; // Scale for inverter + torqueOut = (cal5 * pow(elapsedTime, 5)) + (cal4 * pow(elapsedTime, 4)) + (cal3 * pow(elapsedTime, 3)) + (cal2 * pow(elapsedTime, 2)) + (cal1 * (elapsedTime)) + calIntercept; + torqueOut *= 10; // Scale for inverter // this linear equation goes nowhere - // lcTorqueRequest = (1.0/25.0) * (elapsedTime) + calIntercept; // Performs the calibration curve math, shrimple linear for now - if (lcTorqueRequest > maxTorque) + // torqueOut = (1.0/25.0) * (elapsedTime) + calIntercept; // Performs the calibration curve math, shrimple linear for now + return static_cast(torqueOut); +} + +// PID to get optimal slip +int lc_pid::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) +{ + float torqueOut = 0; + // Calculate front and rear wheel speeds - take average of left and right + float frontRpmAvg = ((wheelSpeedData.fl+wheelSpeedData.fr)/2); + float rearRpmAvg = ((wheelSpeedData.rl+wheelSpeedData.rr)/2); + // avoid zero division + if (frontRpmAvg || rearRpmAvg <= 0.001) { - torqueOut = maxTorque; + this->input = 0; // treat it like 0 slip (maybe this is bad) } else { - torqueOut = lcTorqueRequest; + // Slip = (rear / front) - 1 + // ie. 1000rpm/900rpm = 1.111.. + // 1.111 - 1 = 0.111 slip ratio + this->input = (rearRpmAvg / frontRpmAvg) - 1; } - return torqueOut; -} \ No newline at end of file + + pid.run(elapsedTime); + torqueOut = output * maxTorque; + + return static_cast(torqueOut); +} diff --git a/src/pedal_handler.cpp b/src/pedal_handler.cpp index 07c0ea4..3908c9a 100644 --- a/src/pedal_handler.cpp +++ b/src/pedal_handler.cpp @@ -274,8 +274,8 @@ void PedalHandler::verify_pedals( } // idgaf anything below (all wheel speed) -double PedalHandler::get_wsfr() { return wsfr_t.current_rpm; } -double PedalHandler::get_wsfl() { return wsfl_t.current_rpm; } +float PedalHandler::get_wsfr() { return wsfr_t.current_rpm; } +float PedalHandler::get_wsfl() { return wsfl_t.current_rpm; } /** * @brief update wheel speed readings * diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 07af43a..4bf18a7 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -373,14 +373,12 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) } if (mcu_status.get_launch_ctrl_active()) { - // does it make mroe sense to run the launch controller here? - launchControl->run(millis(), calculated_torque); - calculated_torque = launchControl->getTorqueOutput(); // Do launch control things switch (launchControl->getState()) { case launchState::IDLE: { + calculated_torque=0; // Set torque to zero in IDLE // If button is held for 1 second, APPS is floored (90%), brake is not active, and impl_occ is false // THEN: go to WAITING_TO_LAUNCH if (dash_->get_button_held_duration(6, 1000) && (pedals->getAppsTravel() > 0.9) && !(mcu_status.get_brake_pedal_active()) && !impl_occ) @@ -392,14 +390,15 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) } case launchState::WAITING_TO_LAUNCH: { + calculated_torque=0; // Set torque to zero in WAITING_TO_LAUNCH // If gas is released, return to IDLE if ((pedals->getAppsTravel() < 0.5) || (calculated_torque <=(0.5*600))) // TODO be less redundant? { launchControl->setState(launchState::IDLE,millis()); break; } - // If gas is still pinned and button has been released for 500ms, start LAUNCHING - if ((pedals->getAppsTravel()> 0.9) && dash_->get_button_released_duration(6,500)) + // If gas is still pinned and button has been released for 1000ms, start LAUNCHING + if ((pedals->getAppsTravel()> 0.9) && !dash_->get_button6() && dash_->get_button_released_duration(6,1000)) { launchControl->setState(launchState::LAUNCHING,millis()); } @@ -413,8 +412,9 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) launchControl->setState(launchState::FINISHED,millis()); break; } - // TODO do i run the launch controller here? - // launchControl->run(millis(),calculated_torque); + wheelSpeeds_s wheelSpeedData = {pedals->get_wsfl(),pedals->get_wsfr(),pm100->getmcMotorRPM(),pm100->getmcMotorRPM()}; + launchControl->run(millis(), calculated_torque, wheelSpeedData); + calculated_torque = launchControl->getTorqueOutput(); break; } case launchState::FINISHED: diff --git a/test/test_systems/test_launch_control.h b/test/test_systems/test_launch_control.h index 1e37537..494630b 100644 --- a/test/test_systems/test_launch_control.h +++ b/test/test_systems/test_launch_control.h @@ -5,10 +5,11 @@ #include #include "launch_controller.hpp" #include "launch_controller.cpp" +#include "AutoPID.h" TEST(lcTesting, test_launch_control_exists) { - launchController launchControl; + lc_lut launchControl; unsigned long systime = 0; launchControl.initLaunchController(systime); EXPECT_EQ(launchControl.getState(), launchState::IDLE); @@ -22,31 +23,33 @@ TEST(lcTesting, test_launch_control_exists) TEST(lcTesting, test_lc_notorque) { - launchController lc; + wheelSpeeds_s wheelSpeedData = {0,0,0,0}; + lc_lut lc; unsigned long t = 0; lc.initLaunchController(t); int torque = 2400; - lc.run(t,torque); + lc.run(t,torque,wheelSpeedData); lc.getTorqueOutput(); EXPECT_EQ(lc.getTorqueOutput(),0); torque= 2400; t++; launchState nextState = lc.setState(launchState::WAITING_TO_LAUNCH,t); - lc.run(t,torque); + lc.run(t,torque,wheelSpeedData); EXPECT_EQ(lc.getTorqueOutput(),0); torque= 2400; t++; nextState = lc.setState(launchState::LAUNCHING,t); - lc.run(t,torque); + lc.run(t,torque,wheelSpeedData); EXPECT_NE(lc.getTorqueOutput(),0); } TEST(lcTesting, test_lc_ramp) { - launchController lc; + wheelSpeeds_s wheelSpeedData = {0,0,0,0}; + lc_lut lc; unsigned long t = 0; int nm = 2400; lc.initLaunchController(t); @@ -55,7 +58,7 @@ TEST(lcTesting, test_lc_ramp) for (int i = 0; i < 1400; i += 10) { - lc.run(i,nm); + lc.run(i,nm,wheelSpeedData); std::cout << i << "," << lc.getTorqueOutput() << "\n"; } From f0beca314bd281a9b62f14babae0ef56beb8503c Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Wed, 27 Mar 2024 21:34:32 -0400 Subject: [PATCH 07/29] add launchControlSystem and button to toggle launchControl modes when not in LC active state, add launchControl diagData_s struct --- .gitignore | 1 + include/FlexCAN_util.hpp | 13 +++++++ include/KS2eCAN.hpp | 1 + include/launch_controller.hpp | 29 +++++++++++++-- include/launch_system.h | 32 +++++++++++++++++ include/pedal_handler.hpp | 2 +- include/state_machine.hpp | 10 +++--- src/FlexCAN_handle.cpp | 2 +- src/dashboard.cpp | 4 +-- src/launch_controller.cpp | 10 ++++-- src/launch_system.cpp | 48 +++++++++++++++++++++++++ src/main.cpp | 4 +-- src/pedal_handler.cpp | 8 +++-- src/state_machine.cpp | 34 ++++++++++++------ test/test_systems/test_launch_control.h | 6 ++-- 15 files changed, 172 insertions(+), 32 deletions(-) create mode 100644 include/launch_system.h create mode 100644 src/launch_system.cpp diff --git a/.gitignore b/.gitignore index 89cc49c..fb3ca12 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ .vscode/c_cpp_properties.json .vscode/launch.json .vscode/ipch +.vscode/settings.json diff --git a/include/FlexCAN_util.hpp b/include/FlexCAN_util.hpp index 4c2132e..0c3ec72 100644 --- a/include/FlexCAN_util.hpp +++ b/include/FlexCAN_util.hpp @@ -3,6 +3,7 @@ #include "FlexCAN_T4.h" #include "ksu_ev_can.h" #include "KS2eCAN.hpp" +#include "launch_system.h" int unpack_flexcan_message(can_obj_ksu_ev_can_h_t *o,CAN_message_t &msg); // global wrapper around flexcan_t4 because it is a shit driver that should feel bad int WriteToDaqCAN(CAN_message_t &msg); @@ -13,6 +14,18 @@ int ReadDaqCAN(CAN_message_t &msg); int ReadInverterCAN(CAN_message_t &msg); int ReadAccumulatorCAN(CAN_message_t &msg); +template +bool sendStructOnCan(T data, uint32_t id) +{ + CAN_message_t msg; + msg.id = id; + if (sizeof(data) > sizeof(msg.buf)) + { + return 0; + } + memcpy(msg.buf, &data,sizeof(data)); + return WriteToDaqCAN(msg); +} void InitCAN(); #endif \ No newline at end of file diff --git a/include/KS2eCAN.hpp b/include/KS2eCAN.hpp index 20a36e2..962f9a8 100644 --- a/include/KS2eCAN.hpp +++ b/include/KS2eCAN.hpp @@ -30,6 +30,7 @@ #define ID_VCU_FW_VERSION 0xC8 #define ID_VCU_BOARD_ANALOG_READS_ONE 0xC9 #define ID_VCU_BOARD_ANALOG_READS_TWO 0xCA +#define ID_VCU_BASE_LAUNCH_CONTROLLER_INFO 0xCB #define ID_DASH_BUTTONS 0xEB #define ID_DASH_FW_VERSION 0xEC #define ID_MC_CURRENT_LIMIT_COMMAND 0x202 diff --git a/include/launch_controller.hpp b/include/launch_controller.hpp index 3d7f777..e233c40 100644 --- a/include/launch_controller.hpp +++ b/include/launch_controller.hpp @@ -1,6 +1,7 @@ #ifndef LAUNCH_CONTROLLER_HPP #define LAUNCH_CONTROLLER_HPP #include +#include #include #include @@ -12,6 +13,22 @@ enum class launchState FINISHED = 3 }; +enum launchControlTypes_e +{ + LC_DRIVERCONTROL = 0, // No firmware limiting, driver throttle directly + LC_LOOKUP = 1, // Polynomial/ lookup table + LC_PID = 2, // PID slip control + LC_NUM_CONTROLLERS +}; + +struct diagData_s { + unsigned long launchElapsedTime; + int16_t outputTorqueCommand; + uint8_t launchState; + uint8_t launchType; + diagData_s(unsigned long time, int16_t outputTorque, uint8_t state, uint8_t type) + : launchElapsedTime(time), outputTorqueCommand(outputTorque), launchState(state), launchType(type) {} +}; class launchController { @@ -31,10 +48,12 @@ class launchController launchState setState(const launchState nextState, unsigned long sysTime); // general virtual int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); virtual void run(unsigned long sysTime, int &torqueRequest, wheelSpeeds_s &wheelSpeedData); // instance specific + virtual launchControlTypes_e getType() {return launchControlTypes_e::LC_DRIVERCONTROL;} int getTorqueOutput() const { return outputTorqueCommand; }; // general + diagData_s getDiagData(); // 8 byte info on torque controller }; -class lc_lut : public launchController +class launchControllerLookup : public launchController { private: const float cal5 = -0.000000000000085; // Fifth @@ -46,8 +65,9 @@ class lc_lut : public launchController public: int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); + launchControlTypes_e getType() {return launchControlTypes_e::LC_LOOKUP;} }; -class lc_pid : public launchController +class launchControllerPID : public launchController { private: const double tireSlipLow = 0.05; @@ -61,11 +81,14 @@ class lc_pid : public launchController AutoPID pid; public: - lc_pid() : pid(&input, &setpoint, &output, output_min, output_max, d_kp, d_ki, d_kd) + launchControllerPID() : pid(&input, &setpoint, &output, output_min, output_max, d_kp, d_ki, d_kd) { pid.setBangBang(tireSlipLow); pid.setTimeStep(1); } int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); + launchControlTypes_e getType() {return launchControlTypes_e::LC_PID;} + + }; #endif \ No newline at end of file diff --git a/include/launch_system.h b/include/launch_system.h new file mode 100644 index 0000000..da7764b --- /dev/null +++ b/include/launch_system.h @@ -0,0 +1,32 @@ +#ifndef LAUNCH_SYSTEM_H +#define LAUNCH_SYSTEM_H +#include +#include "launch_controller.hpp" + +class launchControlSystem +{ + public: + void setActiveSystem(launchControlTypes_e systype) {lcType = systype;} + launchControlTypes_e getActiveSystem() {return lcType;} + launchController* getController() + { + return &lc_map[lcType]; + } + void toggleController(unsigned long systime); + void printControllerType(char buf[60]); + + private: + launchControlTypes_e lcType = launchControlTypes_e::LC_DRIVERCONTROL; + launchController lc_base; + launchControllerLookup lc_lookup; + launchControllerPID lc_pid; + std::unordered_map lc_map = { + {launchControlTypes_e::LC_DRIVERCONTROL, lc_base}, + {launchControlTypes_e::LC_LOOKUP, lc_lookup}, + {launchControlTypes_e::LC_PID, lc_pid} + }; + + + +}; +#endif \ No newline at end of file diff --git a/include/pedal_handler.hpp b/include/pedal_handler.hpp index d2013d8..7da576f 100644 --- a/include/pedal_handler.hpp +++ b/include/pedal_handler.hpp @@ -86,7 +86,7 @@ class PedalHandler void update_wheelspeed(unsigned long current_time_millis, wheelspeeds_t *ws, FreqMeasureMulti *freq); float get_wsfl(); float get_wsfr(); - void send_readings(); + bool send_readings(); bool get_board_sensor_readings(); void read_pedal_values_debug(uint16_t value); float getAppsTravel(); diff --git a/include/state_machine.hpp b/include/state_machine.hpp index 0b49f4b..ac07b58 100644 --- a/include/state_machine.hpp +++ b/include/state_machine.hpp @@ -1,14 +1,14 @@ #ifndef STATE_MACHINE_HPP #define STATE_MACHINE_HPP #include +#include #include "MCU_status.hpp" #include "inverter.hpp" #include "accumulator.hpp" #include "pedal_handler.hpp" #include "dashboard.hpp" -#include #include "parameters.hpp" -#include "launch_controller.hpp" +#include "launch_system.h" class StateMachine { @@ -19,13 +19,13 @@ class StateMachine Dashboard *dash_; Metro *debug_; PedalHandler *pedals; - launchController *launchControl; + launchControlSystem *lcSystem; Metro *pedal_check_; void set_state(MCU_status &mcu_status, MCU_STATE new_state); void send_state_msg(MCU_status &mcu_status); public: - StateMachine(Inverter *inv, Accumulator *acc, Metro *rs_tim, Dashboard *dash, Metro *debug, PedalHandler *pedals, launchController *lc,Metro *ped_t) - : pm100(inv), accumulator(acc), timer_ready_sound(rs_tim), dash_(dash), debug_(debug), pedals(pedals), launchControl(lc),pedal_check_(ped_t){}; + StateMachine(Inverter *inv, Accumulator *acc, Metro *rs_tim, Dashboard *dash, Metro *debug, PedalHandler *pedals, launchControlSystem *lcSys,Metro *ped_t) + : pm100(inv), accumulator(acc), timer_ready_sound(rs_tim), dash_(dash), debug_(debug), pedals(pedals), lcSystem(lcSys),pedal_check_(ped_t){}; void init_state_machine(MCU_status &mcu_status); void handle_state_machine(MCU_status &mcu_status); diff --git a/src/FlexCAN_handle.cpp b/src/FlexCAN_handle.cpp index d749b0d..2e6c692 100644 --- a/src/FlexCAN_handle.cpp +++ b/src/FlexCAN_handle.cpp @@ -1,5 +1,5 @@ #include "FlexCAN_util.hpp" - +#include "state_machine.hpp" FlexCAN_T4 DaqCAN_; FlexCAN_T4 Inverter_CAN_; FlexCAN_T4 AccumulatorCAN_; diff --git a/src/dashboard.cpp b/src/dashboard.cpp index e8db50f..7d8c78e 100644 --- a/src/dashboard.cpp +++ b/src/dashboard.cpp @@ -43,14 +43,14 @@ void Dashboard::update_dash(uint8_t input) bool Dashboard::get_button_held_duration(uint8_t button, unsigned long duration_ms) { bool wasHeld = this->get_button(button) && (this->get_button_last_pressed_time(button) >= duration_ms); - set_button_last_pressed_time(0,button); + if (wasHeld) {set_button_last_pressed_time(0,button);} return wasHeld; } // true if time since a button was released > duration_ms (if you want to delay an action for example) bool Dashboard::get_button_released_duration(uint8_t button, unsigned long duration_ms) { bool wasReleased = !(this->get_button(button)) && (this->get_button_last_pressed_time(button) >= duration_ms); - set_button_last_pressed_time(0,button); + if (wasReleased) {set_button_last_pressed_time(0,button);} return wasReleased; } diff --git a/src/launch_controller.cpp b/src/launch_controller.cpp index 86118f3..42bf053 100644 --- a/src/launch_controller.cpp +++ b/src/launch_controller.cpp @@ -50,6 +50,12 @@ void launchController::run(unsigned long sysTime, int &torqueRequest, wheelSpeed else { outputTorqueCommand = 0; } // Set output to zero if not in LAUNCHING or FINISHED } +diagData_s launchController::getDiagData() +{ + diagData_s diagData = diagData_s{this->launchElapsedTime,this->outputTorqueCommand,static_cast(this->getState()),static_cast(this->getType())}; + return diagData; +} + // Torque in = torque out in base LC class int launchController::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) { @@ -59,7 +65,7 @@ int launchController::calculateTorque(unsigned long elapsedTime, int maxTorque, } // Calculate the launch control system's ideal torque output -int lc_lut::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) +int launchControllerLookup::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) { float torqueOut = 0; // @jstri114 peep dis @@ -71,7 +77,7 @@ int lc_lut::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeed } // PID to get optimal slip -int lc_pid::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) +int launchControllerPID::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) { float torqueOut = 0; // Calculate front and rear wheel speeds - take average of left and right diff --git a/src/launch_system.cpp b/src/launch_system.cpp new file mode 100644 index 0000000..c610144 --- /dev/null +++ b/src/launch_system.cpp @@ -0,0 +1,48 @@ +#include "launch_system.h" +// #include +void launchControlSystem::toggleController(unsigned long systime){ + int current_controller = static_cast(lcType); + int next_controller = current_controller+1; + if (next_controller >= static_cast(launchControlTypes_e::LC_NUM_CONTROLLERS)) + { + next_controller = 0; + } + this->setActiveSystem(static_cast(next_controller)); + // init new system + this->getController()->initLaunchController(systime); +} +void launchControlSystem::printControllerType(char buf[60]) +{ + launchController* controller = getController(); + if (controller) + { + switch (controller->getType()){ + case (launchControlTypes_e::LC_DRIVERCONTROL): + { + char string[] = "Controller type is launchControllerBase"; + memcpy(buf,string,sizeof(string)); + break; + } + case (launchControlTypes_e::LC_LOOKUP): + { + char string[] = "Controller type is launchControllerLookup"; + memcpy(buf,string,sizeof(string)); + break; + } + case (launchControlTypes_e::LC_PID): + { + char string[] = "Controller type is launchControllerPID"; + memcpy(buf,string,sizeof(string)); + break; + } + case (launchControlTypes_e::LC_NUM_CONTROLLERS): + { + char string[] = "? LOL"; + memcpy(buf,string,sizeof(string)); + break; + } + default: + break; + } + } +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 20e567d..2fe78d2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -64,8 +64,8 @@ Dashboard dash; Inverter pm100(&timer_mc_kick_timer, &timer_inverter_enable, &timer_motor_controller_send, &timer_current_limit_send, &dash); Accumulator accum(&precharge_timeout,&ksu_can); PedalHandler pedals(&timer_debug_pedals_raw, &pedal_out_20hz, &pedal_out_1hz, &speedPID, ¤t_rpm, &set_rpm, &throttle_out, &wsfl, &wsfr); -launchController launchControl; -StateMachine state_machine(&pm100, &accum, &timer_ready_sound, &dash, &debug_tim, &pedals, &launchControl, &pedal_check); +launchControlSystem launchSystem; +StateMachine state_machine(&pm100, &accum, &timer_ready_sound, &dash, &debug_tim, &pedals, &launchSystem, &pedal_check); MCU_status mcu_status = MCU_status(); static CAN_message_t mcu_status_msg; diff --git a/src/pedal_handler.cpp b/src/pedal_handler.cpp index 3908c9a..07371ed 100644 --- a/src/pedal_handler.cpp +++ b/src/pedal_handler.cpp @@ -138,8 +138,9 @@ bool PedalHandler::read_pedal_values() * @brief send pedal readings over CAN, with timers built in to function * */ -void PedalHandler::send_readings() +bool PedalHandler::send_readings() { + bool sent = false; if (pedal_out_20hz->check()) { // Send Main Control Unit pedal reading message @ 20hz @@ -156,8 +157,8 @@ void PedalHandler::send_readings() int16_t rpm_wsfr = (int16_t)(wsfr_t.current_rpm); int16_t rpm_buf[]={rpm_wsfl,rpm_wsfr}; memcpy(&tx_msg2.buf[0], &rpm_buf, sizeof(rpm_buf)); - WriteCANToInverter(tx_msg); - WriteCANToInverter(tx_msg2); + sent = WriteCANToInverter(tx_msg); + sent = WriteCANToInverter(tx_msg2); } if (pedal_out_1hz->check()) { @@ -178,6 +179,7 @@ void PedalHandler::send_readings() WriteCANToInverter(tx_msg3); WriteCANToInverter(tx_msg4); } + return sent; } /** diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 4bf18a7..382e20c 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -138,9 +138,9 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) mcu_status.set_bspd_ok_high(pedals->get_board_sensor_readings()); mcu_status.set_bspd_current_high((accumulator->get_acc_current() > (bspd_current_high_threshold * 10))); - pedals->send_readings(); + if (pedals->send_readings() && mcu_status.get_launch_ctrl_active()) { sendStructOnCan(lcSystem->getController()->getDiagData(),ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); } - // If dash button is on and has been on for + // If dash button is on and has been on for 750ms if (dash_->get_button(6) && (dash_->get_button_last_pressed_time(6)) > 750) { dash_->set_button_last_pressed_time(0, 6); @@ -148,6 +148,19 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) mcu_status.set_max_torque(torque_mode_list[mcu_status.get_torque_mode() - 1]); send_state_msg(mcu_status); } + // If dash button held and LC not active + if (dash_->get_button_held_duration(2,500) && !mcu_status.get_launch_ctrl_active()) + { + int current_controller = static_cast(lcSystem->getActiveSystem()); + int next_controller = current_controller+1; + if (next_controller >= static_cast(launchControlTypes_e::LC_NUM_CONTROLLERS)) + { + next_controller = 0; + } + lcSystem->setActiveSystem(static_cast(next_controller)); + // init new system + lcSystem->getController()->initLaunchController(millis()); + } // Do Torque Calcs here int calculated_torque = 0; bool accel_is_plausible = false; @@ -366,7 +379,8 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) // Toggle launch control (allows deactivating if sitting in it) mcu_status.set_launch_ctrl_active(!(mcu_status.get_launch_ctrl_active())); // Reset the launch controller each time we toggle it - launchControl->initLaunchController(millis()); + lcSystem->getController()->initLaunchController(millis()); + #if DEBUG Serial.printf("DEBUG: Set launch control to %d", mcu_status.get_launch_ctrl_active()); #endif @@ -374,7 +388,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) if (mcu_status.get_launch_ctrl_active()) { // Do launch control things - switch (launchControl->getState()) + switch (lcSystem->getController()->getState()) { case launchState::IDLE: { @@ -383,7 +397,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) // THEN: go to WAITING_TO_LAUNCH if (dash_->get_button_held_duration(6, 1000) && (pedals->getAppsTravel() > 0.9) && !(mcu_status.get_brake_pedal_active()) && !impl_occ) { - launchControl->setState(launchState::WAITING_TO_LAUNCH,millis()); + lcSystem->getController()->setState(launchState::WAITING_TO_LAUNCH,millis()); break; } break; @@ -394,13 +408,13 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) // If gas is released, return to IDLE if ((pedals->getAppsTravel() < 0.5) || (calculated_torque <=(0.5*600))) // TODO be less redundant? { - launchControl->setState(launchState::IDLE,millis()); + lcSystem->getController()->setState(launchState::IDLE,millis()); break; } // If gas is still pinned and button has been released for 1000ms, start LAUNCHING if ((pedals->getAppsTravel()> 0.9) && !dash_->get_button6() && dash_->get_button_released_duration(6,1000)) { - launchControl->setState(launchState::LAUNCHING,millis()); + lcSystem->getController()->setState(launchState::LAUNCHING,millis()); } break; } @@ -409,12 +423,12 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) if ((mcu_status.get_brake_pedal_active()) || impl_occ) { // Terminate launch control early if the brake is pressed or there was a pedal fault - launchControl->setState(launchState::FINISHED,millis()); + lcSystem->getController()->setState(launchState::FINISHED,millis()); break; } wheelSpeeds_s wheelSpeedData = {pedals->get_wsfl(),pedals->get_wsfr(),pm100->getmcMotorRPM(),pm100->getmcMotorRPM()}; - launchControl->run(millis(), calculated_torque, wheelSpeedData); - calculated_torque = launchControl->getTorqueOutput(); + lcSystem->getController()->run(millis(), calculated_torque, wheelSpeedData); + calculated_torque = lcSystem->getController()->getTorqueOutput(); break; } case launchState::FINISHED: diff --git a/test/test_systems/test_launch_control.h b/test/test_systems/test_launch_control.h index 494630b..a8c6a18 100644 --- a/test/test_systems/test_launch_control.h +++ b/test/test_systems/test_launch_control.h @@ -9,7 +9,7 @@ TEST(lcTesting, test_launch_control_exists) { - lc_lut launchControl; + launchControllerLookup launchControl; unsigned long systime = 0; launchControl.initLaunchController(systime); EXPECT_EQ(launchControl.getState(), launchState::IDLE); @@ -24,7 +24,7 @@ TEST(lcTesting, test_launch_control_exists) TEST(lcTesting, test_lc_notorque) { wheelSpeeds_s wheelSpeedData = {0,0,0,0}; - lc_lut lc; + launchControllerLookup lc; unsigned long t = 0; lc.initLaunchController(t); int torque = 2400; @@ -49,7 +49,7 @@ TEST(lcTesting, test_lc_notorque) TEST(lcTesting, test_lc_ramp) { wheelSpeeds_s wheelSpeedData = {0,0,0,0}; - lc_lut lc; + launchControllerLookup lc; unsigned long t = 0; int nm = 2400; lc.initLaunchController(t); From e11b2d4f4d7ae7da12d79cf04294e719f8e487a8 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Fri, 29 Mar 2024 09:18:46 -0400 Subject: [PATCH 08/29] updated tests --- include/FlexCAN_util.hpp | 5 +- include/common_structs.h | 2 + include/parameters.hpp | 8 ++- platformio.ini | 8 +++ src/pedal_handler.cpp | 7 +- test/test_systems/test_launch_control.h | 94 ++++++++++++++++++------- 6 files changed, 88 insertions(+), 36 deletions(-) diff --git a/include/FlexCAN_util.hpp b/include/FlexCAN_util.hpp index 0c3ec72..f2dfba0 100644 --- a/include/FlexCAN_util.hpp +++ b/include/FlexCAN_util.hpp @@ -19,10 +19,7 @@ bool sendStructOnCan(T data, uint32_t id) { CAN_message_t msg; msg.id = id; - if (sizeof(data) > sizeof(msg.buf)) - { - return 0; - } + static_assert(sizeof(data) <= sizeof(msg.buf), "Data size exceeds message buffer size"); memcpy(msg.buf, &data,sizeof(data)); return WriteToDaqCAN(msg); } diff --git a/include/common_structs.h b/include/common_structs.h index 0da5801..d596e6e 100644 --- a/include/common_structs.h +++ b/include/common_structs.h @@ -5,5 +5,7 @@ typedef struct wheelSpeeds_s { float fr; float rl; float rr; + wheelSpeeds_s(float fl, float fr, float rl, float rr) + : fl(fl), fr(fr), rl(rl), rr(rr){} } wheelSpeeds_s; #endif \ No newline at end of file diff --git a/include/parameters.hpp b/include/parameters.hpp index 2510af9..489a3a1 100644 --- a/include/parameters.hpp +++ b/include/parameters.hpp @@ -51,16 +51,18 @@ const float accumulator_cell_minimum_voltage = 2.5; const float accumulator_cell_nominal_voltage = 3.6; const float accumulator_cell_maximum_voltage = 4.2; const float bspd_current_high_threshold = 5000/(accumulator_cell_count * accumulator_cell_nominal_voltage); // Current value where BSPD current detection should be high (5kw at nominal voltage) -#define MIN_HV_VOLTAGE 600 // apparently this is divided by ten? yes maybe, bc getmcbusvoltage returns a can packet which is the bus voltage*10? idk +#define MIN_HV_VOLTAGE 60 * 10 // 60V is HV threshold, multiplied by ten for inverter scaling factor #define DISCHARGE_POWER_LIM 75000 #define CHARGE_POWER_LIM 9000 -// Torque Calculation Defines -#define ALPHA 0.99 // This is the coefficient for exponential smoothing const float cutoff_10hz = 10; // Hz // Calculate filtering alpha value for the cutoff frequency const double FILTERING_ALPHA_10HZ = 2 * 3.14 * cutoff_10hz / (1 + 2 * 3.14 * cutoff_10hz); +const float cutoff_1hz = 1; // Hz +// Calculate filtering alpha value for the cutoff frequency +const double FILTERING_ALPHA_1HZ = 2 * 3.14 * cutoff_1hz / (1 + 2 * 3.14 * cutoff_1hz); + // Note that the variable max_torque is uin8_t // So it will overflow past a value of 255 const uint8_t TORQUE_1 = 10; // 1st Torque setting diff --git a/platformio.ini b/platformio.ini index 0598bd7..1dd44c7 100644 --- a/platformio.ini +++ b/platformio.ini @@ -7,6 +7,9 @@ ; ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html +[env] +build_unflags = -std=gnu++11 +build_flags = -std=c++17 [env:test_env] platform = native test_framework = googletest @@ -14,6 +17,9 @@ lib_compat_mode = off ; Add libs on a case by case basis - lib_deps = https://github.com/KSU-MS/ksu-autoPID +debug_test = test_systems +build_unflags = -std=gnu++11 +build_flags = -std=c++17 [env:teensy41] platform = teensy @@ -24,6 +30,8 @@ lib_deps = https://github.com/PaulStoffregen/FreqMeasureMulti https://github.com/KSU-MS/pio-git-hash-gen https://github.com/KSU-MS/ksu-ms-dbc/releases/download/14/can_lib.tar.gz +build_unflags = -std=gnu++11 +build_flags = -std=c++17 ; teensy-cli gives some computers problems when uploading ; upload_protocol = teensy-cli diff --git a/src/pedal_handler.cpp b/src/pedal_handler.cpp index 07371ed..cd7645e 100644 --- a/src/pedal_handler.cpp +++ b/src/pedal_handler.cpp @@ -48,9 +48,9 @@ int16_t PedalHandler::calculate_torque(int16_t &motor_speed, int &max_torque) { torque2 = max_torque; } - // Use average of the two APPS torque results to calculate the torque request - calculated_torque = (torque1 + torque2) / 2; // TODO un-cheese this + // Use average of the two APPS torque results to calculate the torque request + calculated_torque = (torque1 + torque2) / 2; if (calculated_torque > max_torque) { @@ -78,8 +78,7 @@ int16_t PedalHandler::calculate_regen(int16_t &motor_speed, int16_t max_regen_to const int16_t regen_torque_maximum = REGEN_NM * -10; calculated_regen_torque = this->bse1.getTravelRatio() * regen_torque_maximum; // Smooth regen torque so it doesnt yeet driveline - // TODO find out what this limits the rate of change to - smoothed_regen_torque = 0.8 * smoothed_regen_torque + (1-0.8) * calculated_regen_torque; + smoothed_regen_torque = FILTERING_ALPHA_1HZ * smoothed_regen_torque + (1-FILTERING_ALPHA_1HZ) * calculated_regen_torque; #if DEBUG Serial.printf("Calculated regen: %d smoothed regen: %d",calculated_regen_torque,smoothed_regen_torque); #endif diff --git a/test/test_systems/test_launch_control.h b/test/test_systems/test_launch_control.h index a8c6a18..7f86d6d 100644 --- a/test/test_systems/test_launch_control.h +++ b/test/test_systems/test_launch_control.h @@ -5,64 +5,108 @@ #include #include "launch_controller.hpp" #include "launch_controller.cpp" +#include "launch_system.h" +#include "launch_system.cpp" #include "AutoPID.h" TEST(lcTesting, test_launch_control_exists) { - launchControllerLookup launchControl; + launchControlSystem lcSystem; + launchController* launchControl = lcSystem.getController(); unsigned long systime = 0; - launchControl.initLaunchController(systime); - EXPECT_EQ(launchControl.getState(), launchState::IDLE); - launchControl.setState(launchState::WAITING_TO_LAUNCH,0); - EXPECT_EQ(launchControl.getState(), launchState::WAITING_TO_LAUNCH); - launchControl.setState(launchState::LAUNCHING,0); - EXPECT_EQ(launchControl.getState(), launchState::LAUNCHING); - launchControl.setState(launchState::FINISHED,0); - EXPECT_EQ(launchControl.getState(), launchState::FINISHED); + launchControl->initLaunchController(systime); + EXPECT_EQ(launchControl->getState(), launchState::IDLE); + launchControl->setState(launchState::WAITING_TO_LAUNCH,0); + EXPECT_EQ(launchControl->getState(), launchState::WAITING_TO_LAUNCH); + launchControl->setState(launchState::LAUNCHING,0); + EXPECT_EQ(launchControl->getState(), launchState::LAUNCHING); + launchControl->setState(launchState::FINISHED,0); + EXPECT_EQ(launchControl->getState(), launchState::FINISHED); } TEST(lcTesting, test_lc_notorque) { wheelSpeeds_s wheelSpeedData = {0,0,0,0}; - launchControllerLookup lc; + launchControlSystem lcSystem; + launchController* launchControl = lcSystem.getController(); unsigned long t = 0; - lc.initLaunchController(t); + launchControl->initLaunchController(t); int torque = 2400; - lc.run(t,torque,wheelSpeedData); - lc.getTorqueOutput(); - EXPECT_EQ(lc.getTorqueOutput(),0); + launchControl->run(t,torque,wheelSpeedData); + launchControl->getTorqueOutput(); + EXPECT_EQ(launchControl->getTorqueOutput(),0); torque= 2400; t++; - launchState nextState = lc.setState(launchState::WAITING_TO_LAUNCH,t); - lc.run(t,torque,wheelSpeedData); - EXPECT_EQ(lc.getTorqueOutput(),0); + launchState nextState = launchControl->setState(launchState::WAITING_TO_LAUNCH,t); + launchControl->run(t,torque,wheelSpeedData); + EXPECT_EQ(launchControl->getTorqueOutput(),0); torque= 2400; t++; - nextState = lc.setState(launchState::LAUNCHING,t); - lc.run(t,torque,wheelSpeedData); - EXPECT_NE(lc.getTorqueOutput(),0); + nextState = launchControl->setState(launchState::LAUNCHING,t); + launchControl->run(t,torque,wheelSpeedData); + EXPECT_NE(launchControl->getTorqueOutput(),0); } TEST(lcTesting, test_lc_ramp) { wheelSpeeds_s wheelSpeedData = {0,0,0,0}; - launchControllerLookup lc; + launchControlSystem lcSystem; + launchControlTypes_e typeToSet = launchControlTypes_e::LC_LOOKUP; + lcSystem.setActiveSystem(typeToSet); + launchControlTypes_e c = lcSystem.getController()->getType(); + ASSERT_EQ(c,typeToSet); + launchController* launchControl = lcSystem.getController(); unsigned long t = 0; int nm = 2400; - lc.initLaunchController(t); - lc.setState(launchState::LAUNCHING,t); + launchControl->initLaunchController(t); + launchControl->setState(launchState::LAUNCHING,t); std::cout << "time" << "," << "torqueOut" << "\n"; for (int i = 0; i < 1400; i += 10) { - lc.run(i,nm,wheelSpeedData); - std::cout << i << "," << lc.getTorqueOutput() << "\n"; + launchControl->run(i,nm,wheelSpeedData); + std::cout << i << "," << launchControl->getTorqueOutput() << "\n"; } } +TEST(lcTesting, test_lc_system_toggle) +{ + launchControlSystem lcSystem; + launchController* launchControl = lcSystem.getController(); + for (int i = 0; i < static_cast(launchControlTypes_e::LC_NUM_CONTROLLERS); i++) + { + launchControlTypes_e c = lcSystem.getController()->getType(); + launchControlTypes_e y = static_cast(i); + EXPECT_EQ(c,y); + lcSystem.toggleController(0); + } +} + +TEST(lcTesting, test_lc_pid) +{ + + launchControlSystem lcSystem; + launchControlTypes_e typeToSet = launchControlTypes_e::LC_PID; + lcSystem.setActiveSystem(typeToSet); + launchControlTypes_e c = lcSystem.getController()->getType(); + ASSERT_EQ(c,typeToSet); + launchController* launchControl = lcSystem.getController(); + + unsigned long t = 0; + launchControl->initLaunchController(t); + launchControl->setState(launchState::LAUNCHING,t); + for (float i = 10; i < 6000; i += 10) + { + wheelSpeeds_s wsData = wheelSpeeds_s(i,i,i*1.01,i*1.01); + int driver_torque = 240; + launchControl->run(t+1,driver_torque,wsData); + printf("time: %dms, slip: %f, output torque: %d\n",t,wsData.rl/wsData.fl,launchControl->getTorqueOutput()); + t++; + } +} #endif \ No newline at end of file From 35842763a61e1fae81c4ae3f34cd42ec047b97a1 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Fri, 29 Mar 2024 09:22:08 -0400 Subject: [PATCH 09/29] fix launchController map --- include/launch_controller.hpp | 17 ++++++++++++----- include/launch_system.h | 10 +++++----- src/launch_controller.cpp | 7 ------- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/include/launch_controller.hpp b/include/launch_controller.hpp index e233c40..67afc83 100644 --- a/include/launch_controller.hpp +++ b/include/launch_controller.hpp @@ -46,7 +46,12 @@ class launchController void initLaunchController(unsigned long sysTime); // general to all launchControllers launchState getState(); // general launchState setState(const launchState nextState, unsigned long sysTime); // general - virtual int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); + virtual int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) + { + float torqueOut = 0; + torqueOut = maxTorque; + return static_cast(torqueOut); + }; virtual void run(unsigned long sysTime, int &torqueRequest, wheelSpeeds_s &wheelSpeedData); // instance specific virtual launchControlTypes_e getType() {return launchControlTypes_e::LC_DRIVERCONTROL;} int getTorqueOutput() const { return outputTorqueCommand; }; // general @@ -72,9 +77,9 @@ class launchControllerPID : public launchController private: const double tireSlipLow = 0.05; const double tireSlipHigh = 0.2; - double d_kp = 1.5; - double d_ki = 0; - double d_kd = 0.5; + double d_kp = 4.0; + double d_ki = 2.0; + double d_kd = 1.0; const double output_min = 0.6; // Minimum output of the PID controller const double output_max = 1.0; // Max output of the PID controller double input, setpoint, output; @@ -83,7 +88,9 @@ class launchControllerPID : public launchController public: launchControllerPID() : pid(&input, &setpoint, &output, output_min, output_max, d_kp, d_ki, d_kd) { - pid.setBangBang(tireSlipLow); + output=0; + setpoint = tireSlipHigh; + pid.setBangBang(0); pid.setTimeStep(1); } int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); diff --git a/include/launch_system.h b/include/launch_system.h index da7764b..57e193a 100644 --- a/include/launch_system.h +++ b/include/launch_system.h @@ -10,7 +10,7 @@ class launchControlSystem launchControlTypes_e getActiveSystem() {return lcType;} launchController* getController() { - return &lc_map[lcType]; + return static_cast(lc_map[lcType]); } void toggleController(unsigned long systime); void printControllerType(char buf[60]); @@ -20,10 +20,10 @@ class launchControlSystem launchController lc_base; launchControllerLookup lc_lookup; launchControllerPID lc_pid; - std::unordered_map lc_map = { - {launchControlTypes_e::LC_DRIVERCONTROL, lc_base}, - {launchControlTypes_e::LC_LOOKUP, lc_lookup}, - {launchControlTypes_e::LC_PID, lc_pid} + std::unordered_map lc_map = { + {launchControlTypes_e::LC_DRIVERCONTROL, &lc_base}, + {launchControlTypes_e::LC_LOOKUP, &lc_lookup}, + {launchControlTypes_e::LC_PID, &lc_pid} }; diff --git a/src/launch_controller.cpp b/src/launch_controller.cpp index 42bf053..a62f613 100644 --- a/src/launch_controller.cpp +++ b/src/launch_controller.cpp @@ -56,13 +56,6 @@ diagData_s launchController::getDiagData() return diagData; } -// Torque in = torque out in base LC class -int launchController::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) -{ - float torqueOut = 0; - torqueOut = maxTorque; - return static_cast(torqueOut); -} // Calculate the launch control system's ideal torque output int launchControllerLookup::calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) From fd9caef0287266231cfb8afdc9da83d4b03eb5d1 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sat, 6 Apr 2024 02:56:46 -0400 Subject: [PATCH 10/29] use header file in library --- src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.cpp b/src/main.cpp index 2fe78d2..98f78af 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -15,7 +15,7 @@ #include "inverter.hpp" #include "accumulator.hpp" #include "state_machine.hpp" -#include "device_status.h" +#include "ksu_device_status.h" #define NEBUG static can_obj_ksu_ev_can_h_t ksu_can; From 832034a0f6138c7d972ea29fed1b2298b91f0412 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sun, 7 Apr 2024 01:23:33 -0400 Subject: [PATCH 11/29] add linear ramp controller --- include/launch_controller.hpp | 45 ++++++++++++++++++++++--- include/launch_system.h | 6 ++-- src/launch_controller.cpp | 8 +++-- src/launch_system.cpp | 6 ++++ src/main.cpp | 2 +- src/state_machine.cpp | 5 ++- test/test_systems/test_launch_control.h | 23 +++++++++++++ 7 files changed, 84 insertions(+), 11 deletions(-) diff --git a/include/launch_controller.hpp b/include/launch_controller.hpp index 67afc83..399a888 100644 --- a/include/launch_controller.hpp +++ b/include/launch_controller.hpp @@ -18,6 +18,7 @@ enum launchControlTypes_e LC_DRIVERCONTROL = 0, // No firmware limiting, driver throttle directly LC_LOOKUP = 1, // Polynomial/ lookup table LC_PID = 2, // PID slip control + LC_LINEAR = 3, // Linear equation based LC_NUM_CONTROLLERS }; @@ -29,20 +30,19 @@ struct diagData_s { diagData_s(unsigned long time, int16_t outputTorque, uint8_t state, uint8_t type) : launchElapsedTime(time), outputTorqueCommand(outputTorque), launchState(state), launchType(type) {} }; + class launchController { - private: - const unsigned long launchControlMaxDuration = 1200; // milliseconds that the launching period will run for unsigned long launchStartTime = 0; // ms the starting time of the launch unsigned long launchCurrentTime = 0; // ms the current time of the launch unsigned long launchElapsedTime = 0; // ms time elapsed from start of launch int driverTorqueRequest = 0; // Driver torque command if using pedal travel int lcTorqueRequest = 0; // launch control system torque command int outputTorqueCommand = 0; // The final torque command that will be output from the launch controller - bool disableTorqueCommanding = true; // to disable torque commanding in IDLE and WAIT states launchState launchControlState; // state enum to control LC actions public: + unsigned long launchControlMaxDuration = 1200; // milliseconds that the launching period will run for void initLaunchController(unsigned long sysTime); // general to all launchControllers launchState getState(); // general launchState setState(const launchState nextState, unsigned long sysTime); // general @@ -72,6 +72,7 @@ class launchControllerLookup : public launchController int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); launchControlTypes_e getType() {return launchControlTypes_e::LC_LOOKUP;} }; + class launchControllerPID : public launchController { private: @@ -80,8 +81,8 @@ class launchControllerPID : public launchController double d_kp = 4.0; double d_ki = 2.0; double d_kd = 1.0; - const double output_min = 0.6; // Minimum output of the PID controller - const double output_max = 1.0; // Max output of the PID controller + const double output_min = -1.0; // Minimum output of the PID controller + const double output_max = 0; // Max output of the PID controller double input, setpoint, output; AutoPID pid; @@ -95,7 +96,41 @@ class launchControllerPID : public launchController } int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); launchControlTypes_e getType() {return launchControlTypes_e::LC_PID;} +}; +class launchControllerLinear : public launchController +{ +private: + double m; // Slope of the linear equation + double b; // Intercept of the linear equation +public: + /** + * @brief Construct a new launch Controller Linear object + * + * @param startTime the time to start ramping at (assume 0) + * @param startTorque the torque to start the ramp at + * @param endTime the time to end at + * @param endTorque + */ + launchControllerLinear(double startTime, double startTorque, double endTime, double endTorque) + { + m = (endTorque - startTorque) / (endTime - startTime); + b = startTorque - m * startTime; + this->launchControlMaxDuration = endTime; + } + void updateRamp(double startTime, double startTorque, double endTime, double endTorque) + { + m = (endTorque - startTorque) / (endTime - startTime); + b = startTorque - m * startTime; + + } + int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData) + { + double x = elapsedTime; + double y = m * x + b; // Calculate y value using linear equation y = mx + b + return static_cast(y); // Return the calculated torque as an integer + } + launchControlTypes_e getType() {return launchControlTypes_e::LC_LINEAR;} }; #endif \ No newline at end of file diff --git a/include/launch_system.h b/include/launch_system.h index 57e193a..93165d9 100644 --- a/include/launch_system.h +++ b/include/launch_system.h @@ -2,7 +2,7 @@ #define LAUNCH_SYSTEM_H #include #include "launch_controller.hpp" - +#include class launchControlSystem { public: @@ -20,10 +20,12 @@ class launchControlSystem launchController lc_base; launchControllerLookup lc_lookup; launchControllerPID lc_pid; + launchControllerLinear lc_linear = launchControllerLinear(0,TORQUE_2,200,TORQUE_4); std::unordered_map lc_map = { {launchControlTypes_e::LC_DRIVERCONTROL, &lc_base}, {launchControlTypes_e::LC_LOOKUP, &lc_lookup}, - {launchControlTypes_e::LC_PID, &lc_pid} + {launchControlTypes_e::LC_PID, &lc_pid}, + {launchControlTypes_e::LC_LINEAR, &lc_linear} }; diff --git a/src/launch_controller.cpp b/src/launch_controller.cpp index a62f613..3ac2632 100644 --- a/src/launch_controller.cpp +++ b/src/launch_controller.cpp @@ -32,7 +32,7 @@ void launchController::run(unsigned long sysTime, int &torqueRequest, wheelSpeed if (this->getState() == launchState::LAUNCHING) { - if (launchElapsedTime >= launchControlMaxDuration) + if (launchElapsedTime > launchControlMaxDuration) { this->setState(launchState::FINISHED,sysTime); } @@ -42,6 +42,10 @@ void launchController::run(unsigned long sysTime, int &torqueRequest, wheelSpeed { outputTorqueCommand = driverTorqueRequest; } + else if (outputTorqueCommand < 0) + { + outputTorqueCommand = 0; + } } else if (this->getState() == launchState::FINISHED) { @@ -90,7 +94,7 @@ int launchControllerPID::calculateTorque(unsigned long elapsedTime, int maxTorqu } pid.run(elapsedTime); - torqueOut = output * maxTorque; + torqueOut = maxTorque + (output * maxTorque); return static_cast(torqueOut); } diff --git a/src/launch_system.cpp b/src/launch_system.cpp index c610144..8d4b596 100644 --- a/src/launch_system.cpp +++ b/src/launch_system.cpp @@ -35,6 +35,12 @@ void launchControlSystem::printControllerType(char buf[60]) memcpy(buf,string,sizeof(string)); break; } + case (launchControlTypes_e::LC_LINEAR): + { + char string[] = "Controller type is launchControllerLinear"; + memcpy(buf,string,sizeof(string)); + break; + } case (launchControlTypes_e::LC_NUM_CONTROLLERS): { char string[] = "? LOL"; diff --git a/src/main.cpp b/src/main.cpp index 98f78af..d1e3c21 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,7 +22,7 @@ static can_obj_ksu_ev_can_h_t ksu_can; // Metro timers for inverter: Metro timer_mc_kick_timer = Metro(50, 1); // Motor controller heartbeat timer Metro timer_inverter_enable = Metro(2000, 1); // Timeout for inverter enabling -Metro timer_motor_controller_send = Metro(10, 1); // Motor controller torque command timer +Metro timer_motor_controller_send = Metro(5, 1); // Motor controller torque command timer Metro timer_current_limit_send = Metro(10, 1); // Motor controller power limiting timer // timers for the accumulator: diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 42c4519..1cf0de3 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -138,7 +138,10 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) mcu_status.set_bspd_ok_high(pedals->get_board_sensor_readings()); mcu_status.set_bspd_current_high((accumulator->get_acc_current() > (bspd_current_high_threshold * 10))); - if (pedals->send_readings() && mcu_status.get_launch_ctrl_active()) { sendStructOnCan(lcSystem->getController()->getDiagData(),ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); } + if (pedals->send_readings()) + { + sendStructOnCan(lcSystem->getController()->getDiagData(),ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); + } // If dash button is on and has been on for 750ms if (dash_->get_button(6) && (dash_->get_button_last_pressed_time(6)) > 750) diff --git a/test/test_systems/test_launch_control.h b/test/test_systems/test_launch_control.h index 7f86d6d..0a3b03d 100644 --- a/test/test_systems/test_launch_control.h +++ b/test/test_systems/test_launch_control.h @@ -109,4 +109,27 @@ TEST(lcTesting, test_lc_pid) t++; } } + +TEST(lcTesting, test_lc_linear) +{ + + launchControlSystem lcSystem; + launchControlTypes_e typeToSet = launchControlTypes_e::LC_LINEAR; + lcSystem.setActiveSystem(typeToSet); + launchControlTypes_e c = lcSystem.getController()->getType(); + ASSERT_EQ(c,typeToSet); + launchController* launchControl = lcSystem.getController(); + + unsigned long t = 0; + launchControl->initLaunchController(t); + launchControl->setState(launchState::LAUNCHING,t); + for (float i = 10; i < 6000; i += 10) + { + wheelSpeeds_s wsData = wheelSpeeds_s(i,i,i*1.01,i*1.01); + int driver_torque = 240; + launchControl->run(t+1,driver_torque,wsData); + printf("time: %dms, slip: %f, output torque: %d\n",t,wsData.rl/wsData.fl,launchControl->getTorqueOutput()); + t++; + } +} #endif \ No newline at end of file From 29c2374896ccf342d7ff445694a7730520a06b96 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sun, 7 Apr 2024 02:10:07 -0400 Subject: [PATCH 12/29] add configuration of enabled launch modes --- include/launch_controller.hpp | 11 +++++++ include/launch_system.h | 56 ++++++++++++++++++++++------------- src/launch_system.cpp | 19 +++++++++--- src/main.cpp | 2 +- 4 files changed, 62 insertions(+), 26 deletions(-) diff --git a/include/launch_controller.hpp b/include/launch_controller.hpp index 399a888..fda3b89 100644 --- a/include/launch_controller.hpp +++ b/include/launch_controller.hpp @@ -94,6 +94,17 @@ class launchControllerPID : public launchController pid.setBangBang(0); pid.setTimeStep(1); } + launchControllerPID(double kp, double ki, double kd) : pid(&input, &setpoint, &output, output_min, output_max, kp, ki, kd) + { + this->d_kd=kd; + this->d_kp = kp; + this->d_ki = ki; + pid.setGains(d_kp,d_ki,d_kd); + output=0; + setpoint = tireSlipHigh; + pid.setBangBang(0); + pid.setTimeStep(1); + } int calculateTorque(unsigned long elapsedTime, int maxTorque, wheelSpeeds_s &wheelSpeedData); launchControlTypes_e getType() {return launchControlTypes_e::LC_PID;} }; diff --git a/include/launch_system.h b/include/launch_system.h index 93165d9..8b9fe19 100644 --- a/include/launch_system.h +++ b/include/launch_system.h @@ -3,32 +3,46 @@ #include #include "launch_controller.hpp" #include + class launchControlSystem { - public: - void setActiveSystem(launchControlTypes_e systype) {lcType = systype;} - launchControlTypes_e getActiveSystem() {return lcType;} - launchController* getController() +public: + launchControlSystem(std::initializer_list enabledTypes = {launchControlTypes_e::LC_DRIVERCONTROL, launchControlTypes_e::LC_LOOKUP, launchControlTypes_e::LC_PID, launchControlTypes_e::LC_LINEAR}) + { + for (auto type : enabledTypes) { - return static_cast(lc_map[lcType]); + // Add only the enabled types to the map + if (lc_map.find(type) != lc_map.end()) + { + enabled_lc_map[type] = lc_map[type]; + } } - void toggleController(unsigned long systime); - void printControllerType(char buf[60]); - - private: - launchControlTypes_e lcType = launchControlTypes_e::LC_DRIVERCONTROL; - launchController lc_base; - launchControllerLookup lc_lookup; - launchControllerPID lc_pid; - launchControllerLinear lc_linear = launchControllerLinear(0,TORQUE_2,200,TORQUE_4); - std::unordered_map lc_map = { - {launchControlTypes_e::LC_DRIVERCONTROL, &lc_base}, - {launchControlTypes_e::LC_LOOKUP, &lc_lookup}, - {launchControlTypes_e::LC_PID, &lc_pid}, - {launchControlTypes_e::LC_LINEAR, &lc_linear} - }; - + } + void setActiveSystem(launchControlTypes_e systype) { lcType = systype; } + launchControlTypes_e getActiveSystem() { return lcType; } + launchController *getController() + { + return static_cast(lc_map[lcType]); + } + void toggleController(unsigned long systime); + void printControllerType(char buf[60]); +private: + launchControlTypes_e lcType = launchControlTypes_e::LC_DRIVERCONTROL; + launchController lc_base; + launchControllerLookup lc_lookup; + launchControllerPID lc_pid; + launchControllerLinear lc_linear = launchControllerLinear(0, TORQUE_2, 200, TORQUE_4); + // This map should contain ALL types + std::unordered_map lc_map = { + {launchControlTypes_e::LC_DRIVERCONTROL, &lc_base}, + {launchControlTypes_e::LC_LOOKUP, &lc_lookup}, + {launchControlTypes_e::LC_PID, &lc_pid}, + {launchControlTypes_e::LC_LINEAR, &lc_linear}}; + + // This map will only have enabled types + std::unordered_map enabled_lc_map; }; + #endif \ No newline at end of file diff --git a/src/launch_system.cpp b/src/launch_system.cpp index 8d4b596..bd8b7b8 100644 --- a/src/launch_system.cpp +++ b/src/launch_system.cpp @@ -1,16 +1,27 @@ #include "launch_system.h" // #include -void launchControlSystem::toggleController(unsigned long systime){ +void launchControlSystem::toggleController(unsigned long systime) +{ int current_controller = static_cast(lcType); - int next_controller = current_controller+1; - if (next_controller >= static_cast(launchControlTypes_e::LC_NUM_CONTROLLERS)) + int next_controller = current_controller + 1; + while (true) { - next_controller = 0; + if (next_controller >= static_cast(launchControlTypes_e::LC_NUM_CONTROLLERS)) + { + next_controller = 0; + } + if (enabled_lc_map.find(static_cast(next_controller)) != enabled_lc_map.end()) + { + break; // Found an enabled controller + } + next_controller++; } + this->setActiveSystem(static_cast(next_controller)); // init new system this->getController()->initLaunchController(systime); } + void launchControlSystem::printControllerType(char buf[60]) { launchController* controller = getController(); diff --git a/src/main.cpp b/src/main.cpp index d1e3c21..12367db 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -64,7 +64,7 @@ Dashboard dash; Inverter pm100(&timer_mc_kick_timer, &timer_inverter_enable, &timer_motor_controller_send, &timer_current_limit_send, &dash); Accumulator accum(&precharge_timeout,&ksu_can); PedalHandler pedals(&timer_debug_pedals_raw, &pedal_out_20hz, &pedal_out_1hz, &speedPID, ¤t_rpm, &set_rpm, &throttle_out, &wsfl, &wsfr); -launchControlSystem launchSystem; +launchControlSystem launchSystem({launchControlTypes_e::LC_DRIVERCONTROL, launchControlTypes_e::LC_LINEAR}); StateMachine state_machine(&pm100, &accum, &timer_ready_sound, &dash, &debug_tim, &pedals, &launchSystem, &pedal_check); MCU_status mcu_status = MCU_status(); From ff1f513c00435112318ad92ff3667f318d30c767 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Mon, 8 Apr 2024 14:05:48 -0400 Subject: [PATCH 13/29] yerp --- include/launch_system.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/launch_system.h b/include/launch_system.h index 8b9fe19..2a80b6f 100644 --- a/include/launch_system.h +++ b/include/launch_system.h @@ -23,7 +23,7 @@ class launchControlSystem launchControlTypes_e getActiveSystem() { return lcType; } launchController *getController() { - return static_cast(lc_map[lcType]); + return static_cast(enabled_lc_map[lcType]); } void toggleController(unsigned long systime); void printControllerType(char buf[60]); @@ -32,7 +32,7 @@ class launchControlSystem launchControlTypes_e lcType = launchControlTypes_e::LC_DRIVERCONTROL; launchController lc_base; launchControllerLookup lc_lookup; - launchControllerPID lc_pid; + launchControllerPID lc_pid = launchControllerPID(4.0,2.0,1.0); launchControllerLinear lc_linear = launchControllerLinear(0, TORQUE_2, 200, TORQUE_4); // This map should contain ALL types std::unordered_map lc_map = { @@ -40,7 +40,7 @@ class launchControlSystem {launchControlTypes_e::LC_LOOKUP, &lc_lookup}, {launchControlTypes_e::LC_PID, &lc_pid}, {launchControlTypes_e::LC_LINEAR, &lc_linear}}; - + // This map will only have enabled types std::unordered_map enabled_lc_map; }; From 72ff2889a8ca9afa78e8e0f800af51306f4685aa Mon Sep 17 00:00:00 2001 From: mathbrook <32876429+mathbrook@users.noreply.github.com> Date: Sat, 13 Apr 2024 00:18:06 -0400 Subject: [PATCH 14/29] update button handling --- README.md | 7 +++++++ include/dashboard.hpp | 6 +++--- src/dashboard.cpp | 22 ++++++++++++++++------ src/inverter.cpp | 18 +----------------- 4 files changed, 27 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index 5d48d40..ec05914 100644 --- a/README.md +++ b/README.md @@ -5,6 +5,13 @@ the vcu CAN out is still TODO mostly, currently it spits out all RX / TX traffic below is a loose flow of the data and what handles what +## building +to build, run ```pio run -e teensy41``` +## Running tests + +to build and run native tests on windows, follow these instructions: https://code.visualstudio.com/docs/cpp/config-mingw +then, run tests by calling ```pio test -e test_env``` + ```mermaid graph LR; diff --git a/include/dashboard.hpp b/include/dashboard.hpp index 21bf15d..bd62cda 100644 --- a/include/dashboard.hpp +++ b/include/dashboard.hpp @@ -51,10 +51,10 @@ class Dashboard * @brief Get the button last pressed time object * * @param buttonNumber - * @return elapsedMillis + * @return unsigned long */ - elapsedMillis get_button_last_pressed_time(uint8_t buttonNumber); - void set_button_last_pressed_time(uint8_t setpoint,uint8_t buttonNumber); + unsigned long get_button_last_pressed_time(uint8_t buttonNumber); + void set_button_last_pressed_time(unsigned long setpoint,uint8_t buttonNumber); void update_dash(uint8_t input); bool new_dash_msg_received; float last_received_timestamp = 0; diff --git a/src/dashboard.cpp b/src/dashboard.cpp index 7d8c78e..5a1fccf 100644 --- a/src/dashboard.cpp +++ b/src/dashboard.cpp @@ -2,11 +2,11 @@ #include "FlexCAN_util.hpp" #include "dashboard.hpp" -elapsedMillis Dashboard::get_button_last_pressed_time(uint8_t buttonNumber) +unsigned long Dashboard::get_button_last_pressed_time(uint8_t buttonNumber) { return button_last_pressed_time[buttonNumber - 1]; } -void Dashboard::set_button_last_pressed_time(uint8_t setpoint, uint8_t buttonNumber) +void Dashboard::set_button_last_pressed_time(unsigned long setpoint, uint8_t buttonNumber) { button_last_pressed_time[buttonNumber - 1] = setpoint; } @@ -16,7 +16,7 @@ void Dashboard::reset_all_button_timers() { for (uint8_t i = 0; i < sizeof(button_last_pressed_time) / sizeof(button_last_pressed_time[0]); i++) { - set_button_last_pressed_time(0, i); + set_button_last_pressed_time(0, i+1); } } @@ -24,7 +24,9 @@ void Dashboard::reset_all_button_timers() void Dashboard::update_dash(uint8_t input) { float timestamp = millis() / float(1000); +#if DEBUG Serial.printf("Dash last received interval: %f\n", (timestamp - (this->last_received_timestamp))); +#endif this->last_received_timestamp = timestamp; for (int i = 0; i < 6; i++) { @@ -33,8 +35,11 @@ void Dashboard::update_dash(uint8_t input) bool old_val = (this->get_buttons() & bit); if (new_val != old_val) { - Serial.printf("Button number %d changed from %d to %d", i + 1, old_val, new_val); - this->set_button_last_pressed_time(0, i); +#if DEBUG + Serial.printf("Button number %d changed from %d to %d\n", i + 1, old_val, new_val); + Serial.printf("%d\n",static_cast(this->get_button_last_pressed_time(i+1))); +#endif + this->set_button_last_pressed_time(0, i+1); } } this->set_buttons(input); @@ -43,7 +48,12 @@ void Dashboard::update_dash(uint8_t input) bool Dashboard::get_button_held_duration(uint8_t button, unsigned long duration_ms) { bool wasHeld = this->get_button(button) && (this->get_button_last_pressed_time(button) >= duration_ms); - if (wasHeld) {set_button_last_pressed_time(0,button);} + if (wasHeld) { + #if DEBUG + Serial.printf("last pressed time: %d\n",static_cast(button_last_pressed_time[button-1])); + #endif + set_button_last_pressed_time(0,button); + } return wasHeld; } // true if time since a button was released > duration_ms (if you want to delay an action for example) diff --git a/src/inverter.cpp b/src/inverter.cpp index 7fa56b9..ca34c8a 100644 --- a/src/inverter.cpp +++ b/src/inverter.cpp @@ -61,23 +61,7 @@ void Inverter::updateInverterCAN() case (ID_DASH_BUTTONS): { uint8_t new_inputs = rxMsg.buf[0]; - float timestamp = millis() / float(1000); - #if DEBUG - Serial.printf("Dash last received interval: %f\n", (timestamp - (dash->last_received_timestamp))); - #endif - dash->last_received_timestamp = timestamp; - for (int i = 0; i < 6; i++) - { - uint8_t bit = (0x1 << i); - bool new_val = new_inputs & bit; - bool old_val = (dash->get_buttons() & bit); - if (new_val != old_val) - { - Serial.printf("Button number %d changed from %d to %d",i+1,old_val,new_val); - dash->set_button_last_pressed_time(0,i); - } - } - dash->set_buttons(new_inputs); + dash->update_dash(new_inputs); } default: break; From f3bd2739077e11349655b8944ecb9c4f09f0e3c0 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sat, 13 Apr 2024 01:21:40 -0400 Subject: [PATCH 15/29] make launch controllers return Nm*10. start linear ramp at 100nm --- include/launch_controller.hpp | 2 +- include/launch_system.h | 2 +- src/state_machine.cpp | 6 ++++-- test/test_systems/test_launch_control.h | 4 ++-- 4 files changed, 8 insertions(+), 6 deletions(-) diff --git a/include/launch_controller.hpp b/include/launch_controller.hpp index fda3b89..5b9efd7 100644 --- a/include/launch_controller.hpp +++ b/include/launch_controller.hpp @@ -139,7 +139,7 @@ class launchControllerLinear : public launchController { double x = elapsedTime; double y = m * x + b; // Calculate y value using linear equation y = mx + b - return static_cast(y); // Return the calculated torque as an integer + return static_cast(y * 10); // Return the calculated torque as an integer } launchControlTypes_e getType() {return launchControlTypes_e::LC_LINEAR;} diff --git a/include/launch_system.h b/include/launch_system.h index 2a80b6f..d06b238 100644 --- a/include/launch_system.h +++ b/include/launch_system.h @@ -33,7 +33,7 @@ class launchControlSystem launchController lc_base; launchControllerLookup lc_lookup; launchControllerPID lc_pid = launchControllerPID(4.0,2.0,1.0); - launchControllerLinear lc_linear = launchControllerLinear(0, TORQUE_2, 200, TORQUE_4); + launchControllerLinear lc_linear = launchControllerLinear(0, 100, 200, TORQUE_4); // This map should contain ALL types std::unordered_map lc_map = { {launchControlTypes_e::LC_DRIVERCONTROL, &lc_base}, diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 1cf0de3..0ffd7f5 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -144,9 +144,9 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) } // If dash button is on and has been on for 750ms - if (dash_->get_button(6) && (dash_->get_button_last_pressed_time(6)) > 750) + // AND the motor is not spinning!! + if (dash_->get_button_held_duration(6,750) && (pm100->getmcMotorRPM() <= 300)) { - dash_->set_button_last_pressed_time(0, 6); mcu_status.toggle_max_torque(mcu_status.get_torque_mode()); mcu_status.set_max_torque(torque_mode_list[mcu_status.get_torque_mode() - 1]); send_state_msg(mcu_status); @@ -163,6 +163,8 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) lcSystem->setActiveSystem(static_cast(next_controller)); // init new system lcSystem->getController()->initLaunchController(millis()); + sendStructOnCan(lcSystem->getController()->getDiagData(),ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); + } // Do Torque Calcs here int calculated_torque = 0; diff --git a/test/test_systems/test_launch_control.h b/test/test_systems/test_launch_control.h index 0a3b03d..0517fdc 100644 --- a/test/test_systems/test_launch_control.h +++ b/test/test_systems/test_launch_control.h @@ -103,7 +103,7 @@ TEST(lcTesting, test_lc_pid) for (float i = 10; i < 6000; i += 10) { wheelSpeeds_s wsData = wheelSpeeds_s(i,i,i*1.01,i*1.01); - int driver_torque = 240; + int driver_torque = 2400; launchControl->run(t+1,driver_torque,wsData); printf("time: %dms, slip: %f, output torque: %d\n",t,wsData.rl/wsData.fl,launchControl->getTorqueOutput()); t++; @@ -126,7 +126,7 @@ TEST(lcTesting, test_lc_linear) for (float i = 10; i < 6000; i += 10) { wheelSpeeds_s wsData = wheelSpeeds_s(i,i,i*1.01,i*1.01); - int driver_torque = 240; + int driver_torque = 2400; launchControl->run(t+1,driver_torque,wsData); printf("time: %dms, slip: %f, output torque: %d\n",t,wsData.rl/wsData.fl,launchControl->getTorqueOutput()); t++; From 1c17696087214fefd4fefaaf2f9a9b4923905686 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sat, 13 Apr 2024 02:42:23 -0400 Subject: [PATCH 16/29] what 36 hours no sleep does to a mf --- src/state_machine.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 0ffd7f5..f28e22b 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -154,13 +154,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) // If dash button held and LC not active if (dash_->get_button_held_duration(2,500) && !mcu_status.get_launch_ctrl_active()) { - int current_controller = static_cast(lcSystem->getActiveSystem()); - int next_controller = current_controller+1; - if (next_controller >= static_cast(launchControlTypes_e::LC_NUM_CONTROLLERS)) - { - next_controller = 0; - } - lcSystem->setActiveSystem(static_cast(next_controller)); + lcSystem->toggleController(millis()); // init new system lcSystem->getController()->initLaunchController(millis()); sendStructOnCan(lcSystem->getController()->getDiagData(),ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); From c09824ae988176f58263e7174914dc911274af6a Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Thu, 18 Apr 2024 00:36:45 -0400 Subject: [PATCH 17/29] fix distance tracker --- include/distance_tracker.h | 35 +++++++------ src/state_machine.cpp | 4 +- test/test_systems/test_distance_tracker.h | 64 +++++++++++++++++++++++ test/test_systems/test_main.cpp | 1 + 4 files changed, 86 insertions(+), 18 deletions(-) create mode 100644 test/test_systems/test_distance_tracker.h diff --git a/include/distance_tracker.h b/include/distance_tracker.h index d844053..4e7aaa2 100644 --- a/include/distance_tracker.h +++ b/include/distance_tracker.h @@ -12,11 +12,11 @@ struct old_new_t struct energy_data_t { - int16_t energy_wh; + uint16_t energy_wh; int16_t eff_inst; - int16_t distance_m; + uint16_t distance_m; int16_t efficiency_kmkwh; - energy_data_t(int16_t wh, int16_t eff, int16_t m, int16_t kmkwh) : energy_wh(wh), eff_inst(eff), distance_m(m), efficiency_kmkwh(kmkwh) {} + energy_data_t(uint16_t wh, int16_t eff, uint16_t m, int16_t kmkwh) : energy_wh(wh), eff_inst(eff), distance_m(m), efficiency_kmkwh(kmkwh) {} }; class distance_tracker_s @@ -29,21 +29,24 @@ class distance_tracker_s void update(float amps, float volts, float rpm, float circumference, unsigned long newtime) { // calc elapsed time - unsigned long elapsed_time_us = newtime - this->time; - float elapsed_time_seconds = static_cast(elapsed_time_us) / 1000000; + unsigned long elapsed_time_ms = newtime - this->time; + float elapsed_time_seconds = static_cast(elapsed_time_ms) / 1000; // update time this->time = newtime; // calculate distance travelled during the last update time distance_km += elapsed_time_seconds * velocity_ms.oldval; + #ifdef ARDUINO Serial.println(velocity_ms.oldval); + #endif // calculate power used during the last update time - energy_kwh += elapsed_time_seconds / 3600 * power_kw.oldval; + float kwh = (elapsed_time_seconds / 3600) * power_kw.oldval; + energy_wh += kwh; // calculate amp hrs used during the last update time - capacity_ah += elapsed_time_seconds / 3600 * current_amps.oldval; + capacity_ah += (elapsed_time_seconds / 3600) * current_amps.oldval; // update efficiencies - efficiency_kmkwh = distance_km/energy_kwh; + efficiency_kmkwh = distance_km/energy_wh; efficiency_instantaneous = (elapsed_time_seconds * velocity_ms.oldval) / (elapsed_time_seconds / 3600 * power_kw.oldval); // set old vals to the previous new one power_kw.oldval = power_kw.newval; @@ -55,24 +58,24 @@ class distance_tracker_s // Calculate power power_kw.newval = amps * volts; // Calculate speed - velocity_ms.newval = rpm * circumference; + velocity_ms.newval = rpm/60 * circumference; } energy_data_t get_data() { - energy_data_t data = energy_data_t(static_cast(energy_kwh*1000), static_cast(efficiency_instantaneous*1000), static_cast(distance_km*1000), static_cast(efficiency_kmkwh*1000)); + printf("%f %f %d\n",energy_wh,energy_wh*10,static_cast(energy_wh*10)); + energy_data_t data = energy_data_t(static_cast(energy_wh*10), static_cast(efficiency_instantaneous*1000), static_cast(distance_km), static_cast(efficiency_kmkwh*1000)); return data; } - + float capacity_ah=0; + float energy_wh = 0; + float distance_km = 0; + float efficiency_kmkwh = 0; + float efficiency_instantaneous = 0; private: old_new_t power_kw; old_new_t current_amps; old_new_t velocity_ms; unsigned long time; - float capacity_ah; - float energy_kwh = 0; - float distance_km = 0; - float efficiency_kmkwh = 0; - float efficiency_instantaneous = 0; }; #endif \ No newline at end of file diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 4d69ddc..dff7c46 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -5,7 +5,7 @@ void StateMachine::init_state_machine(MCU_status &mcu_status) { set_state(mcu_status, MCU_STATE::STARTUP); pedals->init_pedal_handler(); - distance_tracker.tick(micros()); + distance_tracker.tick(millis()); } // Send a state message on every state transition so we don't miss any @@ -147,7 +147,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) if (mcu_status.get_state() == MCU_STATE::READY_TO_DRIVE) { - distance_tracker.update(accumulator->get_acc_current(), accumulator->get_acc_voltage(), pedals->get_wsfl(), WHEEL_CIRCUMFERENCE, micros()); + distance_tracker.update(accumulator->get_acc_current(), accumulator->get_acc_voltage(), pedals->get_wsfl(), WHEEL_CIRCUMFERENCE, millis()); mcu_status.set_distance_travelled(distance_tracker.get_data().distance_m); } pedals->send_readings(); diff --git a/test/test_systems/test_distance_tracker.h b/test/test_systems/test_distance_tracker.h new file mode 100644 index 0000000..caebf93 --- /dev/null +++ b/test/test_systems/test_distance_tracker.h @@ -0,0 +1,64 @@ +#ifndef DISTANCE_TRACKER_TEST +#define DISTANCE_TRACKER_TEST +#include +#include +#include "distance_tracker.h" +#include "parameters.hpp" +/* +distance_tracker works like this: + void update(float amps, float volts, float rpm, float circumference, unsigned long newtime); + + +*/ +/* +get_data() returns this: +struct energy_data_t +{ + int16_t energy_wh; + int16_t eff_inst; + int16_t distance_m; + int16_t efficiency_kmkwh; + energy_data_t(int16_t wh, int16_t eff, int16_t m, int16_t kmkwh) : energy_wh(wh), eff_inst(eff), distance_m(m), efficiency_kmkwh(kmkwh) {} +}; +*/ +TEST(distance_tracker_tests,init_zero) +{ + distance_tracker_s distance_tracker; + EXPECT_EQ(distance_tracker.get_data().energy_wh,0); + EXPECT_EQ(distance_tracker.get_data().eff_inst,0); + EXPECT_EQ(distance_tracker.get_data().distance_m,0); + EXPECT_EQ(distance_tracker.get_data().efficiency_kmkwh,0); +} +TEST(distance_tracker_tests,drive_onekm) +{ + distance_tracker_s distance_tracker; + float amps = 10; + float volts = 100; + const float rpm = 11.58; + + distance_tracker.tick(0); + for (unsigned long i = 0; i <= 1000*7200; i+= 100) + { + distance_tracker.update(amps,volts,rpm,WHEEL_CIRCUMFERENCE,i); + } + // EXPECT_EQ(distance_tracker.get_data().energy_wh,1000); + // EXPECT_EQ(distance_tracker.get_data().eff_inst,0); + // EXPECT_EQ(distance_tracker.get_data().distance_m,1000); + // EXPECT_EQ(distance_tracker.get_data().efficiency_kmkwh,1); + EXPECT_EQ(distance_tracker.capacity_ah,10); + EXPECT_EQ(distance_tracker.energy_wh,1000); + EXPECT_EQ(distance_tracker.distance_km,1000); + EXPECT_EQ(distance_tracker.efficiency_kmkwh,1); + EXPECT_EQ(distance_tracker.efficiency_instantaneous,1); + printf("%fmeters %fah %fwh %fkm/kwh %fkm/kwh\n",distance_tracker.distance_km,distance_tracker.capacity_ah,distance_tracker.energy_wh,distance_tracker.efficiency_kmkwh,distance_tracker.efficiency_instantaneous); +} +/* + float capacity_ah; + float energy_wh = 0; + float distance_km = 0; + float efficiency_kmkwh = 0; + float efficiency_instantaneous = 0;*/ + + + +#endif \ No newline at end of file diff --git a/test/test_systems/test_main.cpp b/test/test_systems/test_main.cpp index c7088b8..a30aad1 100644 --- a/test/test_systems/test_main.cpp +++ b/test/test_systems/test_main.cpp @@ -1,6 +1,7 @@ #include #include "test_launch_control.h" #include "test_mcu_state.h" +#include "test_distance_tracker.h" int main(int argc, char **argv) { From 0d7902097eb36d6586920c6bb73c2b7f23c566a1 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Thu, 18 Apr 2024 03:26:30 -0400 Subject: [PATCH 18/29] label buttons --- include/dashboard.hpp | 12 ++++++------ src/state_machine.cpp | 3 ++- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/include/dashboard.hpp b/include/dashboard.hpp index bd62cda..5d13cd1 100644 --- a/include/dashboard.hpp +++ b/include/dashboard.hpp @@ -11,17 +11,17 @@ class Dashboard public: Dashboard() = default; inline uint8_t get_buttons() const {return button_states;} - + // Not Connected inline bool get_button1() const {return (button_states&0x01);} - // Button 2 (yellow button) currently used for regen + // Green button inline bool get_button2() const {return (button_states&0x02);} - // Button 3 is connected to RTD button as of 3-1-2024 + // Not connected inline bool get_button3() const {return (button_states&0x04);} - // Unused/not connected + // Ready to drive inline bool get_button4() const {return (button_states&0x08);} - // Unused/not connected + // Red button inline bool get_button5() const {return (button_states&0x10);} - // Button 6 (green) used for torque mode + // Yellow button inline bool get_button6() const {return (button_states&0x20);} inline void set_buttons(uint8_t inputs) { button_states = inputs; } diff --git a/src/state_machine.cpp b/src/state_machine.cpp index dff7c46..3bd99cd 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -145,7 +145,8 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) } - if (mcu_status.get_state() == MCU_STATE::READY_TO_DRIVE) + // if (mcu_status.get_state() == MCU_STATE::READY_TO_DRIVE) + if (true) { distance_tracker.update(accumulator->get_acc_current(), accumulator->get_acc_voltage(), pedals->get_wsfl(), WHEEL_CIRCUMFERENCE, millis()); mcu_status.set_distance_travelled(distance_tracker.get_data().distance_m); From a47674031c5f42393430c8ee167b3be5f179fa1e Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Thu, 18 Apr 2024 07:40:57 -0400 Subject: [PATCH 19/29] working, tested on teensy, not driving --- include/FlexCAN_util.hpp | 2 +- include/KS2eCAN.hpp | 1 + include/common_structs.h | 7 ++ include/dashboard.hpp | 2 +- include/distance_tracker.h | 4 -- include/parameters.hpp | 2 + include/state_machine.hpp | 3 + src/launch_controller.cpp | 1 + src/main.cpp | 2 +- src/state_machine.cpp | 137 +++++++++++++++++++++++++++++++++---- 10 files changed, 142 insertions(+), 19 deletions(-) diff --git a/include/FlexCAN_util.hpp b/include/FlexCAN_util.hpp index f2dfba0..cae4348 100644 --- a/include/FlexCAN_util.hpp +++ b/include/FlexCAN_util.hpp @@ -21,7 +21,7 @@ bool sendStructOnCan(T data, uint32_t id) msg.id = id; static_assert(sizeof(data) <= sizeof(msg.buf), "Data size exceeds message buffer size"); memcpy(msg.buf, &data,sizeof(data)); - return WriteToDaqCAN(msg); + return WriteCANToInverter(msg); } void InitCAN(); diff --git a/include/KS2eCAN.hpp b/include/KS2eCAN.hpp index de28708..69c185f 100644 --- a/include/KS2eCAN.hpp +++ b/include/KS2eCAN.hpp @@ -32,6 +32,7 @@ #define ID_VCU_BOARD_ANALOG_READS_TWO 0xCA #define ID_VCU_BASE_LAUNCH_CONTROLLER_INFO 0xCB #define ID_VCU_PEDAL_TRAVEL 0xCC +#define ID_VCU_LAUNCH_CONTROL_COUNTDOWN 0xCD #define ID_DASH_BUTTONS 0xEB #define ID_DASH_FW_VERSION 0xEC #define ID_MC_CURRENT_LIMIT_COMMAND 0x202 diff --git a/include/common_structs.h b/include/common_structs.h index d596e6e..48510bd 100644 --- a/include/common_structs.h +++ b/include/common_structs.h @@ -1,5 +1,6 @@ #ifndef COMMON_STRUCTS_H #define COMMON_STRUCTS_H +#include "parameters.hpp" typedef struct wheelSpeeds_s { float fl; float fr; @@ -8,4 +9,10 @@ typedef struct wheelSpeeds_s { wheelSpeeds_s(float fl, float fr, float rl, float rr) : fl(fl), fr(fr), rl(rl), rr(rr){} } wheelSpeeds_s; + +typedef struct lc_countdown_t +{ + unsigned long release_countdown; + const unsigned long release_delay = LAUNCHCONTROL_RELEASE_DELAY; +} lc_countdown_t; #endif \ No newline at end of file diff --git a/include/dashboard.hpp b/include/dashboard.hpp index 5d13cd1..4cd032d 100644 --- a/include/dashboard.hpp +++ b/include/dashboard.hpp @@ -4,7 +4,7 @@ #include #pragma pack(push,1) - +const int LAUNCH_CONTROL_BUTTON = 5; class Dashboard { diff --git a/include/distance_tracker.h b/include/distance_tracker.h index 4e7aaa2..8f9dbe2 100644 --- a/include/distance_tracker.h +++ b/include/distance_tracker.h @@ -35,9 +35,6 @@ class distance_tracker_s this->time = newtime; // calculate distance travelled during the last update time distance_km += elapsed_time_seconds * velocity_ms.oldval; - #ifdef ARDUINO - Serial.println(velocity_ms.oldval); - #endif // calculate power used during the last update time float kwh = (elapsed_time_seconds / 3600) * power_kw.oldval; energy_wh += kwh; @@ -62,7 +59,6 @@ class distance_tracker_s } energy_data_t get_data() { - printf("%f %f %d\n",energy_wh,energy_wh*10,static_cast(energy_wh*10)); energy_data_t data = energy_data_t(static_cast(energy_wh*10), static_cast(efficiency_instantaneous*1000), static_cast(distance_km), static_cast(efficiency_kmkwh*1000)); return data; } diff --git a/include/parameters.hpp b/include/parameters.hpp index 2ac33b4..ffa4961 100644 --- a/include/parameters.hpp +++ b/include/parameters.hpp @@ -1,5 +1,6 @@ #ifndef PARAMETERS_HPP #define PARAMETERS_HPP +#include #ifndef PI #define PI 3.1415926535897932384626433832795 #endif @@ -24,6 +25,7 @@ #define PID_MODE false //enable cruise control #define PID_TC_MODE false //enable traction control #define EXP_TORQUE_CURVE false //set to TRUE for kustom pedal curve +const unsigned long LAUNCHCONTROL_RELEASE_DELAY = 1500; #define WHEELSPEED_TOOTH_COUNT 18 // Wheel Speed sensor tooth coutn const float WHEEL_CIRCUMFERENCE = 0.229*PI*2; #define RPM_TIMEOUT 1000 // Timeout for wheel speed RPM to reset to 0 diff --git a/include/state_machine.hpp b/include/state_machine.hpp index 86c436c..2744e4c 100644 --- a/include/state_machine.hpp +++ b/include/state_machine.hpp @@ -25,12 +25,15 @@ class StateMachine distance_tracker_s distance_tracker; void set_state(MCU_status &mcu_status, MCU_STATE new_state); void send_state_msg(MCU_status &mcu_status); + Metro can_20hz_timer = Metro(10,1); public: StateMachine(Inverter *inv, Accumulator *acc, Metro *rs_tim, Dashboard *dash, Metro *debug, PedalHandler *pedals, launchControlSystem *lcSys,Metro *ped_t) : pm100(inv), accumulator(acc), timer_ready_sound(rs_tim), dash_(dash), debug_(debug), pedals(pedals), lcSystem(lcSys),pedal_check_(ped_t){}; void init_state_machine(MCU_status &mcu_status); void handle_state_machine(MCU_status &mcu_status); +// void joe_mock_lc(MCU_status* mcu_status, int torq, bool implaus); + }; #endif \ No newline at end of file diff --git a/src/launch_controller.cpp b/src/launch_controller.cpp index fded91f..bcd3d8b 100644 --- a/src/launch_controller.cpp +++ b/src/launch_controller.cpp @@ -6,6 +6,7 @@ void launchController::initLaunchController(unsigned long sysTime) driverTorqueRequest = 0; lcTorqueRequest = 0; outputTorqueCommand = 0; + this->launchElapsedTime = 0; this->setState(launchState::IDLE,sysTime); } launchState launchController::getState() diff --git a/src/main.cpp b/src/main.cpp index 91fb290..e27a4f9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -64,7 +64,7 @@ Dashboard dash; Inverter pm100(&timer_mc_kick_timer, &timer_inverter_enable, &timer_motor_controller_send, &timer_current_limit_send, &dash); Accumulator accum(&precharge_timeout,&ksu_can); PedalHandler pedals(&timer_debug_pedals_raw, &pedal_out_20hz, &pedal_out_1hz, &speedPID, ¤t_rpm, &set_rpm, &throttle_out, &wsfl, &wsfr); -launchControlSystem launchSystem({launchControlTypes_e::LC_DRIVERCONTROL, launchControlTypes_e::LC_LINEAR}); +launchControlSystem launchSystem; // THIS WILL INCLUDE *ALL* LAUNCH MODES BY DEFAULT StateMachine state_machine(&pm100, &accum, &timer_ready_sound, &dash, &debug_tim, &pedals, &launchSystem, &pedal_check); MCU_status mcu_status = MCU_status(); diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 3bd99cd..b0369a4 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -3,7 +3,7 @@ // initializes the mcu status and pedal handler void StateMachine::init_state_machine(MCU_status &mcu_status) { - set_state(mcu_status, MCU_STATE::STARTUP); + set_state(mcu_status, MCU_STATE::READY_TO_DRIVE); pedals->init_pedal_handler(); distance_tracker.tick(millis()); } @@ -151,25 +151,32 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) distance_tracker.update(accumulator->get_acc_current(), accumulator->get_acc_voltage(), pedals->get_wsfl(), WHEEL_CIRCUMFERENCE, millis()); mcu_status.set_distance_travelled(distance_tracker.get_data().distance_m); } + bool _20hz_send = can_20hz_timer.check(); pedals->send_readings(); // If dash button is on and has been on for 750ms // AND the motor is not spinning!! - if (dash_->get_button_held_duration(6,750) && (pm100->getmcMotorRPM() <= 300)) + if ((!dash_->get_button(2))) +{ + if (dash_->get_button_held_duration(6,750) && (pm100->getmcMotorRPM() <= 300) && !mcu_status.get_launch_ctrl_active()) { dash_->set_button_last_pressed_time(0, 6); mcu_status.toggle_max_torque(mcu_status.get_torque_mode()); mcu_status.set_max_torque(torque_mode_list[mcu_status.get_torque_mode() - 1]); send_state_msg(mcu_status); } + } // If dash button held and LC not active - if (dash_->get_button_held_duration(2,500) && !mcu_status.get_launch_ctrl_active()) + if (!dash_->get_button(6)) + { + if (dash_->get_button_held_duration(2,500) && lcSystem->getController()->getState() == launchState::IDLE) { lcSystem->toggleController(millis()); // init new system lcSystem->getController()->initLaunchController(millis()); sendStructOnCan(lcSystem->getController()->getDiagData(),ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); + } } // Do Torque Calcs here int16_t calculated_torque = 0; @@ -206,7 +213,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) pedals->reset_regen(); } } - + // joe_mock_lc(&mcu_status,calculated_torque,impl_occ); // end of functions that run every loop unconditionally // start of state machine conditional functionality @@ -366,6 +373,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) // READY TO DRIVE case MCU_STATE::READY_TO_DRIVE: // -------------------- { +#ifdef CHEESE_RTD //teehee #if USE_INVERTER if (!pm100->check_TS_active()) @@ -386,16 +394,24 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) set_state(mcu_status, MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); break; } - +#endif // Torque calc always runs in the superloop - // Toggle launch control if button 2 & 6 are held for 1 second, while brake is pressed - if (dash_->get_button_held_duration(2, 1000) && dash_->get_button_held_duration(6, 1000) && mcu_status.get_brake_pedal_active()) + // Toggle launch control if button 5 held for 1 second, while brake is pressed + if (dash_->get_button_held_duration(LAUNCH_CONTROL_BUTTON, 1000)){ // && mcu_status.get_brake_pedal_active() { // Toggle launch control (allows deactivating if sitting in it) mcu_status.set_launch_ctrl_active(!(mcu_status.get_launch_ctrl_active())); + // Auto-set to max torque mode + if (mcu_status.get_launch_ctrl_active()) + { + mcu_status.set_max_torque(TORQUE_4); + mcu_status.set_torque_mode(4); + } + send_state_msg(mcu_status); + // Reset the launch controller each time we toggle it lcSystem->getController()->initLaunchController(millis()); - + } #if DEBUG Serial.printf("DEBUG: Set launch control to %d", mcu_status.get_launch_ctrl_active()); #endif @@ -408,9 +424,9 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) case launchState::IDLE: { calculated_torque=0; // Set torque to zero in IDLE - // If button is held for 1 second, APPS is floored (90%), brake is not active, and impl_occ is false + // If button is held, APPS is floored (90%), brake is not active, and impl_occ is false // THEN: go to WAITING_TO_LAUNCH - if (dash_->get_button_held_duration(6, 1000) && (pedals->getAppsTravel() > 0.9) && !(mcu_status.get_brake_pedal_active()) && !impl_occ) + if (dash_->get_button(6) && (pedals->getAppsTravel() > 0.9) && !(mcu_status.get_brake_pedal_active()) && !impl_occ) { lcSystem->getController()->setState(launchState::WAITING_TO_LAUNCH,millis()); break; @@ -421,16 +437,26 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) { calculated_torque=0; // Set torque to zero in WAITING_TO_LAUNCH // If gas is released, return to IDLE - if ((pedals->getAppsTravel() < 0.5) || (calculated_torque <=(0.5*600))) // TODO be less redundant? + if ((pedals->getAppsTravel() < 0.5)) { lcSystem->getController()->setState(launchState::IDLE,millis()); break; } // If gas is still pinned and button has been released for 1000ms, start LAUNCHING - if ((pedals->getAppsTravel()> 0.9) && !dash_->get_button6() && dash_->get_button_released_duration(6,1000)) + if ((pedals->getAppsTravel()> 0.9) && !dash_->get_button6() && dash_->get_button_released_duration(6,LAUNCHCONTROL_RELEASE_DELAY)) { lcSystem->getController()->setState(launchState::LAUNCHING,millis()); + break; } + else if (dash_->get_button6()) + { + dash_->set_button_last_pressed_time(0,6); + } + if (_20hz_send) { + lc_countdown_t countdown_t; + countdown_t.release_countdown = dash_->get_button_last_pressed_time(6); + sendStructOnCan(countdown_t,ID_VCU_LAUNCH_CONTROL_COUNTDOWN); + } break; } case launchState::LAUNCHING: @@ -444,6 +470,9 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) wheelSpeeds_s wheelSpeedData = {pedals->get_wsfl(),pedals->get_wsfr(),pm100->getmcMotorRPM(),pm100->getmcMotorRPM()}; lcSystem->getController()->run(millis(), calculated_torque, wheelSpeedData); calculated_torque = lcSystem->getController()->getTorqueOutput(); + // Yeet data fast when running + // GO FASTER THAN 20HZ TODO + if (_20hz_send) {(lcSystem->getController()->getDiagData(),ID_VCU_BASE_LAUNCH_CONTROLLER_INFO);} break; } case launchState::FINISHED: @@ -475,3 +504,87 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) } #endif } + +// void StateMachine::joe_mock_lc(MCU_status* mcu_status, int torq, bool implaus) +// { +// bool impl_occ = implaus; +// int16_t calculated_torque = torq; +// // Toggle launch control if button 2 & 6 are held for 1 second, while brake is pressed)); +// if (dash_->get_button_held_duration(LAUNCH_CONTROL_BUTTON, 1000)){ +// if (true) +// { +// // Toggle launch control (allows deactivating if sitting in it) +// mcu_status->set_launch_ctrl_active(!(mcu_status->get_launch_ctrl_active())); +// // Reset the launch controller each time we toggle it +// lcSystem->getController()->initLaunchController(millis()); +// this->send_state_msg(*mcu_status); +// Serial.printf("DEBUG: Set launch control to %d\n", mcu_status->get_launch_ctrl_active()); +// } +// } +// if (mcu_status->get_launch_ctrl_active()) +// { +// // Do launch control things +// switch (lcSystem->getController()->getState()) +// { +// case launchState::IDLE: +// { +// calculated_torque=0; // Set torque to zero in IDLE +// // If button is held for 1 second, APPS is floored (90%), brake is not active, and impl_occ is false +// // THEN: go to WAITING_TO_LAUNCH +// float travel = pedals->getAppsTravel(); +// Serial.printf("travel: %f brake %d impl %d\n",travel,mcu_status->get_brake_pedal_active(),impl_occ); +// if (!dash_->get_button(2)) +// { +// Serial.println("stage1"); +// bool dashbutton = dash_->get_button(6); +// bool travelinrange = (travel > 0.7); +// bool brakeactive = !(mcu_status->get_brake_pedal_active()); +// bool impl = !impl_occ; +// if (dashbutton && travelinrange && brakeactive && impl) +// { +// Serial.println("JOE"); +// lcSystem->getController()->setState(launchState::WAITING_TO_LAUNCH,millis()); +// break; +// } +// } +// break; +// } +// case launchState::WAITING_TO_LAUNCH: +// { +// calculated_torque=0; // Set torque to zero in WAITING_TO_LAUNCH +// // If gas is released, return to IDLE +// if ((pedals->getAppsTravel() < 0.5)) // TODO be less redundant? +// { +// lcSystem->getController()->setState(launchState::IDLE,millis()); +// break; +// } +// // If gas is still pinned and button has been released for 1000ms, start LAUNCHING +// if ((pedals->getAppsTravel()> 0.9) && !dash_->get_button6() && dash_->get_button_released_duration(6,1000)) +// { +// lcSystem->getController()->setState(launchState::LAUNCHING,millis()); +// } +// break; +// } +// case launchState::LAUNCHING: +// { +// if ((mcu_status->get_brake_pedal_active()) || impl_occ) +// { +// // Terminate launch control early if the brake is pressed or there was a pedal fault +// lcSystem->getController()->setState(launchState::FINISHED,millis()); +// break; +// } +// wheelSpeeds_s wheelSpeedData = {pedals->get_wsfl(),pedals->get_wsfr(),pm100->getmcMotorRPM(),pm100->getmcMotorRPM()}; +// lcSystem->getController()->run(millis(), calculated_torque, wheelSpeedData); +// calculated_torque = lcSystem->getController()->getTorqueOutput(); +// break; +// } +// case launchState::FINISHED: +// { +// // Finished! exit launch control active! +// mcu_status->set_launch_ctrl_active(0); +// break; +// } +// } +// Serial.printf("State: %d Resultant torque: %d\n",static_cast(lcSystem->getController()->getState()),calculated_torque); +// } +// } \ No newline at end of file From 286152ffc915b6d43af976d1c7e99fa48dafd6c3 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Thu, 18 Apr 2024 07:50:02 -0400 Subject: [PATCH 20/29] fix --- src/state_machine.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/state_machine.cpp b/src/state_machine.cpp index b0369a4..b056039 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -373,7 +373,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) // READY TO DRIVE case MCU_STATE::READY_TO_DRIVE: // -------------------- { -#ifdef CHEESE_RTD //teehee +// #ifdef CHEESE_RTD //teehee #if USE_INVERTER if (!pm100->check_TS_active()) @@ -394,7 +394,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) set_state(mcu_status, MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); break; } -#endif +// #endif // Torque calc always runs in the superloop // Toggle launch control if button 5 held for 1 second, while brake is pressed if (dash_->get_button_held_duration(LAUNCH_CONTROL_BUTTON, 1000)){ // && mcu_status.get_brake_pedal_active() From 00f44de8d0b687235f9fd8f7cdf7e8c30bd8f464 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Thu, 18 Apr 2024 18:35:36 -0400 Subject: [PATCH 21/29] start in startup --- src/state_machine.cpp | 2 +- test/test_systems/test_distance_tracker.h | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/state_machine.cpp b/src/state_machine.cpp index b056039..ba24271 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -3,7 +3,7 @@ // initializes the mcu status and pedal handler void StateMachine::init_state_machine(MCU_status &mcu_status) { - set_state(mcu_status, MCU_STATE::READY_TO_DRIVE); + set_state(mcu_status, MCU_STATE::STARTUP); pedals->init_pedal_handler(); distance_tracker.tick(millis()); } diff --git a/test/test_systems/test_distance_tracker.h b/test/test_systems/test_distance_tracker.h index caebf93..84ae7e1 100644 --- a/test/test_systems/test_distance_tracker.h +++ b/test/test_systems/test_distance_tracker.h @@ -37,7 +37,7 @@ TEST(distance_tracker_tests,drive_onekm) const float rpm = 11.58; distance_tracker.tick(0); - for (unsigned long i = 0; i <= 1000*7200; i+= 100) + for (unsigned long i = 0; i < 1000*3600; i+= 100) { distance_tracker.update(amps,volts,rpm,WHEEL_CIRCUMFERENCE,i); } @@ -45,11 +45,11 @@ TEST(distance_tracker_tests,drive_onekm) // EXPECT_EQ(distance_tracker.get_data().eff_inst,0); // EXPECT_EQ(distance_tracker.get_data().distance_m,1000); // EXPECT_EQ(distance_tracker.get_data().efficiency_kmkwh,1); - EXPECT_EQ(distance_tracker.capacity_ah,10); - EXPECT_EQ(distance_tracker.energy_wh,1000); - EXPECT_EQ(distance_tracker.distance_km,1000); - EXPECT_EQ(distance_tracker.efficiency_kmkwh,1); - EXPECT_EQ(distance_tracker.efficiency_instantaneous,1); + EXPECT_NEAR(distance_tracker.capacity_ah,10,0.001); + EXPECT_NEAR(distance_tracker.energy_wh,1000,0.5); // half a watt hour accuracy + EXPECT_NEAR(distance_tracker.distance_km,1000,1); // One meter accuracy + EXPECT_NEAR(distance_tracker.efficiency_kmkwh,1,0.001); + EXPECT_NEAR(distance_tracker.efficiency_instantaneous,1,0.001); printf("%fmeters %fah %fwh %fkm/kwh %fkm/kwh\n",distance_tracker.distance_km,distance_tracker.capacity_ah,distance_tracker.energy_wh,distance_tracker.efficiency_kmkwh,distance_tracker.efficiency_instantaneous); } /* From dd0495283e09015207a5fbf301d3e94ed3386113 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sun, 12 May 2024 01:30:14 -0400 Subject: [PATCH 22/29] update --- src/inverter.cpp | 3 --- src/state_machine.cpp | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/inverter.cpp b/src/inverter.cpp index d341beb..f481697 100644 --- a/src/inverter.cpp +++ b/src/inverter.cpp @@ -120,9 +120,6 @@ bool Inverter::command_torque(int16_t torque) { return false; } - else{ - return false; - } } // diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 9f06962..034f9d4 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -148,7 +148,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) // if (mcu_status.get_state() == MCU_STATE::READY_TO_DRIVE) if (true) { - distance_tracker.update(accumulator->get_acc_current(), accumulator->get_acc_voltage(), pm100->getmcMotorRPM() * FINAL_DRIVE, WHEEL_CIRCUMFERENCE, micros()); + distance_tracker.update(accumulator->get_acc_current(), accumulator->get_acc_voltage(), pm100->getmcMotorRPM() * FINAL_DRIVE, WHEEL_CIRCUMFERENCE, millis()); mcu_status.set_distance_travelled(distance_tracker.get_data().distance_m); } bool _20hz_send = can_20hz_timer.check(); From 72826e6913205ccac723cb04f93e1fa5bb1e3540 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Mon, 13 May 2024 03:09:39 -0400 Subject: [PATCH 23/29] schizo --- include/KS2eCAN.hpp | 4 +- include/common_structs.h | 6 + include/distance_tracker.h | 22 +-- include/state_machine.hpp | 19 ++- src/main.cpp | 2 + src/state_machine.cpp | 162 ++++++++++++++-------- test/test_systems/test_distance_tracker.h | 17 ++- 7 files changed, 161 insertions(+), 71 deletions(-) diff --git a/include/KS2eCAN.hpp b/include/KS2eCAN.hpp index 69c185f..f1d0995 100644 --- a/include/KS2eCAN.hpp +++ b/include/KS2eCAN.hpp @@ -37,7 +37,9 @@ #define ID_DASH_FW_VERSION 0xEC #define ID_MC_CURRENT_LIMIT_COMMAND 0x202 #define ID_BMS_SOC 0x6B3 //made this real! - +#define ID_VCU_DISTANCE_TRACKER_MOTOR 0xCE +#define ID_VCU_DISTANCE_TRACKER_WHEELSPEED 0xCF +#define ID_VCU_LIFETIME_DATA 0xD0 #define ID_BMS_CURRENT_LIMIT_INFO 0x6B1 #define ID_BMS_PACK_VOLTAGE_INFO 0x6B2 diff --git a/include/common_structs.h b/include/common_structs.h index 48510bd..eacc95c 100644 --- a/include/common_structs.h +++ b/include/common_structs.h @@ -15,4 +15,10 @@ typedef struct lc_countdown_t unsigned long release_countdown; const unsigned long release_delay = LAUNCHCONTROL_RELEASE_DELAY; } lc_countdown_t; + +typedef struct time_and_distance_tracker_t +{ + unsigned long vcu_lifetime_ontime; + unsigned long vcu_lifetime_distance; +} time_and_distance_tracker_t; #endif \ No newline at end of file diff --git a/include/distance_tracker.h b/include/distance_tracker.h index 195685b..8c65323 100644 --- a/include/distance_tracker.h +++ b/include/distance_tracker.h @@ -14,22 +14,23 @@ struct old_new_t struct energy_data_t { uint16_t energy_wh; - int16_t eff_inst; + uint16_t eff_inst; uint16_t distance_m; - int16_t efficiency_kmkwh; - energy_data_t(uint16_t wh, int16_t eff, uint16_t m, int16_t kmkwh) : energy_wh(wh), eff_inst(eff), distance_m(m), efficiency_kmkwh(kmkwh) {} + uint16_t efficiency_kmkwh; + energy_data_t(uint16_t wh, uint16_t eff, uint16_t m, uint16_t kmkwh) : energy_wh(wh), eff_inst(eff), distance_m(m), efficiency_kmkwh(kmkwh) {} }; struct RollingAverage { std::deque buffer; - float sum = 0.0; + float sum; int maxSize; RollingAverage(int maxBufferSize) : maxSize(maxBufferSize) {} void addValue(float value) { buffer.push_back(value); - sum += value; + sum = sum + value; + printf("%f",sum); if (buffer.size() > maxSize) { sum -= buffer.front(); buffer.pop_front(); @@ -37,6 +38,7 @@ struct RollingAverage { } float getAverage() const { + printf("%d, %f",buffer.empty(),buffer.front()); return buffer.empty() ? 0.0 : sum / buffer.size(); } }; @@ -57,7 +59,7 @@ class distance_tracker_s // update time this->time = newtime; // calculate distance travelled during the last update time - distance_km += elapsed_time_seconds * velocity_ms.oldval; + distance_m += elapsed_time_seconds * velocity_ms.oldval; // calculate power used during the last update time float kwh = (elapsed_time_seconds / 3600) * power_kw.oldval; energy_wh += kwh; @@ -66,7 +68,7 @@ class distance_tracker_s capacity_ah += (elapsed_time_seconds / 3600) * current_amps.oldval; // update efficiencies - efficiency_kmkwh = distance_km/energy_wh; + efficiency_kmkwh = distance_m/energy_wh; efficiency_instantaneous = (elapsed_time_seconds * velocity_ms.oldval) / (elapsed_time_seconds / 3600 * power_kw.oldval); avgEff.addValue(efficiency_instantaneous); // set old vals to the previous new one @@ -83,12 +85,12 @@ class distance_tracker_s } energy_data_t get_data() { - energy_data_t data = energy_data_t(static_cast(energy_wh*10), static_cast(avgEff.getAverage()*1000), static_cast(distance_km), static_cast(efficiency_kmkwh*1000)); + energy_data_t data = energy_data_t(static_cast(energy_wh*10), static_cast(avgEff.getAverage()*1000), static_cast(distance_m), static_cast(efficiency_kmkwh*1000)); return data; } float capacity_ah=0; float energy_wh = 0; - float distance_km = 0; + float distance_m = 0; float efficiency_kmkwh = 0; float efficiency_instantaneous = 0; private: @@ -96,7 +98,7 @@ class distance_tracker_s old_new_t current_amps; old_new_t velocity_ms; unsigned long time; - RollingAverage avgEff = RollingAverage(200); + RollingAverage avgEff = RollingAverage(100); }; #endif \ No newline at end of file diff --git a/include/state_machine.hpp b/include/state_machine.hpp index 2744e4c..2f2f22f 100644 --- a/include/state_machine.hpp +++ b/include/state_machine.hpp @@ -2,6 +2,7 @@ #define STATE_MACHINE_HPP #include #include +#include #include "MCU_status.hpp" #include "inverter.hpp" #include "accumulator.hpp" @@ -11,6 +12,7 @@ #include "launch_system.h" #include +#define ODOMETER_EEPROM_ADDR 0 class StateMachine { private: @@ -22,16 +24,29 @@ class StateMachine PedalHandler *pedals; launchControlSystem *lcSystem; Metro *pedal_check_; - distance_tracker_s distance_tracker; + distance_tracker_s distance_tracker_motor; + distance_tracker_s distance_tracker_fl; + // To hold initial values from eeprom: + unsigned long _lifetime_distance = 0; + unsigned long _lifetime_on_time = 0; + time_and_distance_tracker_t time_and_distance_t; void set_state(MCU_status &mcu_status, MCU_STATE new_state); void send_state_msg(MCU_status &mcu_status); - Metro can_20hz_timer = Metro(10,1); + Metro can_100hz_timer = Metro(10,1); + Metro can_20hz_timer = Metro(50,1); + Metro can_10hz_timer = Metro(100,1); + Metro _log_distance_timer_10s = Metro(10000,1); public: StateMachine(Inverter *inv, Accumulator *acc, Metro *rs_tim, Dashboard *dash, Metro *debug, PedalHandler *pedals, launchControlSystem *lcSys,Metro *ped_t) : pm100(inv), accumulator(acc), timer_ready_sound(rs_tim), dash_(dash), debug_(debug), pedals(pedals), lcSystem(lcSys),pedal_check_(ped_t){}; void init_state_machine(MCU_status &mcu_status); void handle_state_machine(MCU_status &mcu_status); + void handle_distance_trackers(MCU_status &mcu_status); + time_and_distance_tracker_t get_lifetime_data() + { + return time_and_distance_t; + } // void joe_mock_lc(MCU_status* mcu_status, int torq, bool implaus); }; diff --git a/src/main.cpp b/src/main.cpp index e27a4f9..2998c2b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -146,6 +146,8 @@ void loop() memcpy(fw_hash_msg.buf, &vcu_status_t, sizeof(vcu_status_t)); WriteCANToInverter(fw_hash_msg); + sendStructOnCan(state_machine.get_lifetime_data(),ID_VCU_LIFETIME_DATA); + // broadcast pedal thresholds information pedal_thresholds_msg.id = ID_VCU_PEDAL_THRESHOLD_SETTINGS; pedal_thresholds_msg.len = 7; diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 034f9d4..7b39197 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -3,9 +3,14 @@ // initializes the mcu status and pedal handler void StateMachine::init_state_machine(MCU_status &mcu_status) { + EEPROM.get(ODOMETER_EEPROM_ADDR,_lifetime_distance); + EEPROM.get(10,_lifetime_on_time); + Serial.printf("Loaded lifetime distance: %d meters (%d km)",_lifetime_distance); + Serial.printf("Loaded lifetime on_time: %d seconds (%d hours)",_lifetime_on_time); set_state(mcu_status, MCU_STATE::STARTUP); pedals->init_pedal_handler(); - distance_tracker.tick(millis()); + distance_tracker_motor.tick(millis()); + distance_tracker_fl.tick(millis()); } // Send a state message on every state transition so we don't miss any @@ -122,6 +127,9 @@ void StateMachine::set_state(MCU_status &mcu_status, MCU_STATE new_state) // Constant logic ---------------------------------------------------------------------------------------------------------------------------------------------------- void StateMachine::handle_state_machine(MCU_status &mcu_status) { + bool _10hz_send = can_10hz_timer.check(); + bool _20hz_send = can_20hz_timer.check(); + bool _100hz_send = can_100hz_timer.check(); // things that are done every loop go here: #if USE_INVERTER @@ -138,45 +146,46 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) mcu_status.set_bms_ok_high(accumulator->get_bms_state()); mcu_status.set_bspd_ok_high(pedals->get_board_sensor_readings()); mcu_status.set_bspd_current_high((accumulator->get_acc_current() > (bspd_current_high_threshold * 10))); - - if (pedals->send_readings()) + pedals->send_readings(); + if (_20hz_send) { - sendStructOnCan(lcSystem->getController()->getDiagData(),ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); + sendStructOnCan(lcSystem->getController()->getDiagData(), ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); } + if (_100hz_send) + { + handle_distance_trackers(mcu_status); + } - // if (mcu_status.get_state() == MCU_STATE::READY_TO_DRIVE) - if (true) + if (_10hz_send) { - distance_tracker.update(accumulator->get_acc_current(), accumulator->get_acc_voltage(), pm100->getmcMotorRPM() * FINAL_DRIVE, WHEEL_CIRCUMFERENCE, millis()); - mcu_status.set_distance_travelled(distance_tracker.get_data().distance_m); + sendStructOnCan(distance_tracker_fl.get_data(),ID_VCU_DISTANCE_TRACKER_WHEELSPEED); + sendStructOnCan(distance_tracker_motor.get_data(),ID_VCU_DISTANCE_TRACKER_MOTOR); + } - bool _20hz_send = can_20hz_timer.check(); - pedals->send_readings(); // If dash button is on and has been on for 750ms // AND the motor is not spinning!! if ((!dash_->get_button(2))) -{ - if (dash_->get_button_held_duration(6,750) && (pm100->getmcMotorRPM() <= 300) && !mcu_status.get_launch_ctrl_active()) { - dash_->set_button_last_pressed_time(0, 6); - mcu_status.toggle_max_torque(mcu_status.get_torque_mode()); - mcu_status.set_max_torque(torque_mode_list[mcu_status.get_torque_mode() - 1]); - send_state_msg(mcu_status); - } + if (dash_->get_button_held_duration(6, 750) && (pm100->getmcMotorRPM() <= 300) && !mcu_status.get_launch_ctrl_active()) + { + dash_->set_button_last_pressed_time(0, 6); + mcu_status.toggle_max_torque(mcu_status.get_torque_mode()); + mcu_status.set_max_torque(torque_mode_list[mcu_status.get_torque_mode() - 1]); + send_state_msg(mcu_status); + } } // If dash button held and LC not active if (!dash_->get_button(6)) { - if (dash_->get_button_held_duration(2,500) && lcSystem->getController()->getState() == launchState::IDLE) - { - lcSystem->toggleController(millis()); - // init new system - lcSystem->getController()->initLaunchController(millis()); - sendStructOnCan(lcSystem->getController()->getDiagData(),ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); - - } + if (dash_->get_button_held_duration(2, 500) && lcSystem->getController()->getState() == launchState::IDLE) + { + lcSystem->toggleController(millis()); + // init new system + lcSystem->getController()->initLaunchController(millis()); + sendStructOnCan(lcSystem->getController()->getDiagData(), ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); + } } // Do Torque Calcs here int16_t calculated_torque = 0; @@ -203,6 +212,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) #endif calculated_torque = pedals->calculate_torque(motor_speed, max_t_actual); + // REGEN if (mcu_status.get_brake_pedal_active() && dash_->get_button2() && calculated_torque < 5) { calculated_torque = pedals->calculate_regen(motor_speed, REGEN_NM); @@ -394,24 +404,25 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) set_state(mcu_status, MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); break; } -// #endif + // #endif // Torque calc always runs in the superloop // Toggle launch control if button 5 held for 1 second, while brake is pressed - if (dash_->get_button_held_duration(LAUNCH_CONTROL_BUTTON, 1000)){ // && mcu_status.get_brake_pedal_active() - { - // Toggle launch control (allows deactivating if sitting in it) - mcu_status.set_launch_ctrl_active(!(mcu_status.get_launch_ctrl_active())); - // Auto-set to max torque mode - if (mcu_status.get_launch_ctrl_active()) + if (dash_->get_button_held_duration(LAUNCH_CONTROL_BUTTON, 1000)) + { // && mcu_status.get_brake_pedal_active() { - mcu_status.set_max_torque(TORQUE_4); - mcu_status.set_torque_mode(4); - } - send_state_msg(mcu_status); + // Toggle launch control (allows deactivating if sitting in it) + mcu_status.set_launch_ctrl_active(!(mcu_status.get_launch_ctrl_active())); + // Auto-set to max torque mode + if (mcu_status.get_launch_ctrl_active()) + { + mcu_status.set_max_torque(TORQUE_4); + mcu_status.set_torque_mode(4); + } + send_state_msg(mcu_status); - // Reset the launch controller each time we toggle it - lcSystem->getController()->initLaunchController(millis()); - } + // Reset the launch controller each time we toggle it + lcSystem->getController()->initLaunchController(millis()); + } #if DEBUG Serial.printf("DEBUG: Set launch control to %d", mcu_status.get_launch_ctrl_active()); #endif @@ -423,40 +434,41 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) { case launchState::IDLE: { - calculated_torque=0; // Set torque to zero in IDLE + calculated_torque = 0; // Set torque to zero in IDLE // If button is held, APPS is floored (90%), brake is not active, and impl_occ is false // THEN: go to WAITING_TO_LAUNCH if (dash_->get_button(6) && (pedals->getAppsTravel() > 0.9) && !(mcu_status.get_brake_pedal_active()) && !impl_occ) { - lcSystem->getController()->setState(launchState::WAITING_TO_LAUNCH,millis()); + lcSystem->getController()->setState(launchState::WAITING_TO_LAUNCH, millis()); break; } break; } case launchState::WAITING_TO_LAUNCH: { - calculated_torque=0; // Set torque to zero in WAITING_TO_LAUNCH + calculated_torque = 0; // Set torque to zero in WAITING_TO_LAUNCH // If gas is released, return to IDLE if ((pedals->getAppsTravel() < 0.5)) { - lcSystem->getController()->setState(launchState::IDLE,millis()); + lcSystem->getController()->setState(launchState::IDLE, millis()); break; } // If gas is still pinned and button has been released for 1000ms, start LAUNCHING - if ((pedals->getAppsTravel()> 0.9) && !dash_->get_button6() && dash_->get_button_released_duration(6,LAUNCHCONTROL_RELEASE_DELAY)) + if ((pedals->getAppsTravel() > 0.9) && !dash_->get_button6() && dash_->get_button_released_duration(6, LAUNCHCONTROL_RELEASE_DELAY)) { - lcSystem->getController()->setState(launchState::LAUNCHING,millis()); + lcSystem->getController()->setState(launchState::LAUNCHING, millis()); break; } else if (dash_->get_button6()) { - dash_->set_button_last_pressed_time(0,6); + dash_->set_button_last_pressed_time(0, 6); } - if (_20hz_send) { + if (_20hz_send) + { lc_countdown_t countdown_t; countdown_t.release_countdown = dash_->get_button_last_pressed_time(6); - sendStructOnCan(countdown_t,ID_VCU_LAUNCH_CONTROL_COUNTDOWN); - } + sendStructOnCan(countdown_t, ID_VCU_LAUNCH_CONTROL_COUNTDOWN); + } break; } case launchState::LAUNCHING: @@ -464,15 +476,18 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) if ((mcu_status.get_brake_pedal_active()) || impl_occ) { // Terminate launch control early if the brake is pressed or there was a pedal fault - lcSystem->getController()->setState(launchState::FINISHED,millis()); + lcSystem->getController()->setState(launchState::FINISHED, millis()); break; } - wheelSpeeds_s wheelSpeedData = {pedals->get_wsfl(),pedals->get_wsfr(),pm100->getmcMotorRPM(),pm100->getmcMotorRPM()}; + wheelSpeeds_s wheelSpeedData = {pedals->get_wsfl(), pedals->get_wsfr(), pm100->getmcMotorRPM(), pm100->getmcMotorRPM()}; lcSystem->getController()->run(millis(), calculated_torque, wheelSpeedData); calculated_torque = lcSystem->getController()->getTorqueOutput(); // Yeet data fast when running // GO FASTER THAN 20HZ TODO - if (_20hz_send) {(lcSystem->getController()->getDiagData(),ID_VCU_BASE_LAUNCH_CONTROLLER_INFO);} + if (_100hz_send) + { + (lcSystem->getController()->getDiagData(), ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); + } break; } case launchState::FINISHED: @@ -505,8 +520,45 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) #endif } +// this sucks +void StateMachine::handle_distance_trackers(MCU_status &mcu_status) +{ +#if DEBUG + Serial.printf("==Handled distance tracker!==\n"); +#endif + bool _10s_timer_fired = _log_distance_timer_10s.check(); + + if (_10s_timer_fired) + { + unsigned long temporary_total_time = _lifetime_on_time + millis()/1000; + EEPROM.put(10,temporary_total_time); + time_and_distance_t.vcu_lifetime_ontime = temporary_total_time; + Serial.printf("Wrote total time: initial: %d millis: %d total: %d",_lifetime_on_time,millis()/1000,temporary_total_time); + } + + if (mcu_status.get_state() == MCU_STATE::READY_TO_DRIVE) + { + distance_tracker_fl.update(accumulator->get_acc_current(), accumulator->get_acc_voltage(), pedals->get_wsfl(), WHEEL_CIRCUMFERENCE, millis()); + distance_tracker_motor.update(accumulator->get_acc_current(), accumulator->get_acc_voltage(), pm100->getmcMotorRPM() * FINAL_DRIVE, WHEEL_CIRCUMFERENCE, millis()); + mcu_status.set_distance_travelled(distance_tracker_motor.get_data().distance_m); + if (_10s_timer_fired) + { + unsigned long temporary_total_distance = _lifetime_distance + distance_tracker_motor.get_data().distance_m; + EEPROM.put(ODOMETER_EEPROM_ADDR,temporary_total_distance); + time_and_distance_t.vcu_lifetime_distance = temporary_total_distance; + Serial.printf("Wrote total distance: initial: %d millis: %d total: %d",_lifetime_distance,distance_tracker_motor.get_data().distance_m,temporary_total_distance); + } + } + else + { + distance_tracker_fl.tick(millis()); + distance_tracker_motor.tick(millis()); + } + +} + // void StateMachine::joe_mock_lc(MCU_status* mcu_status, int torq, bool implaus) -// { +// { // bool impl_occ = implaus; // int16_t calculated_torque = torq; // // Toggle launch control if button 2 & 6 are held for 1 second, while brake is pressed)); @@ -534,8 +586,8 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) // float travel = pedals->getAppsTravel(); // Serial.printf("travel: %f brake %d impl %d\n",travel,mcu_status->get_brake_pedal_active(),impl_occ); // if (!dash_->get_button(2)) -// { -// Serial.println("stage1"); +// { +// Serial.println("stage1"); // bool dashbutton = dash_->get_button(6); // bool travelinrange = (travel > 0.7); // bool brakeactive = !(mcu_status->get_brake_pedal_active()); diff --git a/test/test_systems/test_distance_tracker.h b/test/test_systems/test_distance_tracker.h index 84ae7e1..2662405 100644 --- a/test/test_systems/test_distance_tracker.h +++ b/test/test_systems/test_distance_tracker.h @@ -47,17 +47,28 @@ TEST(distance_tracker_tests,drive_onekm) // EXPECT_EQ(distance_tracker.get_data().efficiency_kmkwh,1); EXPECT_NEAR(distance_tracker.capacity_ah,10,0.001); EXPECT_NEAR(distance_tracker.energy_wh,1000,0.5); // half a watt hour accuracy - EXPECT_NEAR(distance_tracker.distance_km,1000,1); // One meter accuracy + EXPECT_NEAR(distance_tracker.distance_m,1000,1); // One meter accuracy EXPECT_NEAR(distance_tracker.efficiency_kmkwh,1,0.001); EXPECT_NEAR(distance_tracker.efficiency_instantaneous,1,0.001); - printf("%fmeters %fah %fwh %fkm/kwh %fkm/kwh\n",distance_tracker.distance_km,distance_tracker.capacity_ah,distance_tracker.energy_wh,distance_tracker.efficiency_kmkwh,distance_tracker.efficiency_instantaneous); + printf("%fmeters %fah %fwh %fkm/kwh %fkm/kwh\n",distance_tracker.distance_m,distance_tracker.capacity_ah,distance_tracker.energy_wh,distance_tracker.efficiency_kmkwh,distance_tracker.efficiency_instantaneous); + energy_data_t joe = distance_tracker.get_data(); + printf("%dmeters %dwh %dkm/kwh %dkm/kwh\n",joe.distance_m,joe.energy_wh,joe.eff_inst,joe.efficiency_kmkwh); } /* float capacity_ah; float energy_wh = 0; float distance_km = 0; float efficiency_kmkwh = 0; - float efficiency_instantaneous = 0;*/ + float efficiency_instantaneous = 0; +struct energy_data_t +{ + uint16_t energy_wh; + uint16_t eff_inst; + uint16_t distance_m; + uint16_t efficiency_kmkwh; + energy_data_t(uint16_t wh, uint16_t eff, uint16_t m, uint16_t kmkwh) : energy_wh(wh), eff_inst(eff), distance_m(m), efficiency_kmkwh(kmkwh) {} +}; + */ From 97e8604a6c30c1c1fa5538b29b220d70d6e52835 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Tue, 14 May 2024 16:18:40 -0400 Subject: [PATCH 24/29] yeet --- include/distance_tracker.h | 2 +- src/state_machine.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/distance_tracker.h b/include/distance_tracker.h index 8c65323..495d956 100644 --- a/include/distance_tracker.h +++ b/include/distance_tracker.h @@ -38,7 +38,7 @@ struct RollingAverage { } float getAverage() const { - printf("%d, %f",buffer.empty(),buffer.front()); + // printf("%d, %f\n",buffer.empty(),buffer.front()); return buffer.empty() ? 0.0 : sum / buffer.size(); } }; diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 7b39197..5a0cf9c 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -5,8 +5,8 @@ void StateMachine::init_state_machine(MCU_status &mcu_status) { EEPROM.get(ODOMETER_EEPROM_ADDR,_lifetime_distance); EEPROM.get(10,_lifetime_on_time); - Serial.printf("Loaded lifetime distance: %d meters (%d km)",_lifetime_distance); - Serial.printf("Loaded lifetime on_time: %d seconds (%d hours)",_lifetime_on_time); + Serial.printf("Loaded lifetime distance: %d meters (%f km)\n",_lifetime_distance,static_cast(_lifetime_distance)/1000); + Serial.printf("Loaded lifetime on_time: %d seconds (%f hours)\n",_lifetime_on_time,static_cast(_lifetime_on_time)/3600); set_state(mcu_status, MCU_STATE::STARTUP); pedals->init_pedal_handler(); distance_tracker_motor.tick(millis()); @@ -533,7 +533,7 @@ void StateMachine::handle_distance_trackers(MCU_status &mcu_status) unsigned long temporary_total_time = _lifetime_on_time + millis()/1000; EEPROM.put(10,temporary_total_time); time_and_distance_t.vcu_lifetime_ontime = temporary_total_time; - Serial.printf("Wrote total time: initial: %d millis: %d total: %d",_lifetime_on_time,millis()/1000,temporary_total_time); + Serial.printf("Wrote total time: initial: %d millis: %d total: %d\n",_lifetime_on_time,millis()/1000,temporary_total_time); } if (mcu_status.get_state() == MCU_STATE::READY_TO_DRIVE) From b48a80ea639999afd7e794e6e8f48d0d0e5dd47d Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sun, 19 May 2024 23:28:29 -0400 Subject: [PATCH 25/29] add tc --- include/KS2eCAN.hpp | 1 + include/dashboard.hpp | 2 +- include/launch_controller.hpp | 8 ++- include/state_machine.hpp | 6 +- include/tc_system.h | 47 ++++++++++++++++ include/torque_controller.hpp | 98 +++++++++++++++++++++++++++++++++ platformio.ini | 2 +- src/launch_controller.cpp | 4 ++ src/main.cpp | 8 ++- src/pedal_handler.cpp | 2 + src/state_machine.cpp | 19 ++++++- src/tc_system.cpp | 51 +++++++++++++++++ src/torque_controller.cpp | 56 +++++++++++++++++++ test/test_systems/test_main.cpp | 2 +- test/test_systems/test_tc.h | 74 +++++++++++++++++++++++++ 15 files changed, 368 insertions(+), 12 deletions(-) create mode 100644 include/tc_system.h create mode 100644 include/torque_controller.hpp create mode 100644 src/tc_system.cpp create mode 100644 src/torque_controller.cpp create mode 100644 test/test_systems/test_tc.h diff --git a/include/KS2eCAN.hpp b/include/KS2eCAN.hpp index f1d0995..ac9f2e7 100644 --- a/include/KS2eCAN.hpp +++ b/include/KS2eCAN.hpp @@ -40,6 +40,7 @@ #define ID_VCU_DISTANCE_TRACKER_MOTOR 0xCE #define ID_VCU_DISTANCE_TRACKER_WHEELSPEED 0xCF #define ID_VCU_LIFETIME_DATA 0xD0 +#define ID_VCU_TRACTION_CONTROLLER_INFO 0xD1 #define ID_BMS_CURRENT_LIMIT_INFO 0x6B1 #define ID_BMS_PACK_VOLTAGE_INFO 0x6B2 diff --git a/include/dashboard.hpp b/include/dashboard.hpp index 4cd032d..7f93b83 100644 --- a/include/dashboard.hpp +++ b/include/dashboard.hpp @@ -15,7 +15,7 @@ class Dashboard inline bool get_button1() const {return (button_states&0x01);} // Green button inline bool get_button2() const {return (button_states&0x02);} - // Not connected + // Blue button inline bool get_button3() const {return (button_states&0x04);} // Ready to drive inline bool get_button4() const {return (button_states&0x08);} diff --git a/include/launch_controller.hpp b/include/launch_controller.hpp index 736f79d..14bdcb7 100644 --- a/include/launch_controller.hpp +++ b/include/launch_controller.hpp @@ -4,7 +4,9 @@ #include #include #include - +#ifdef ARDUINO +#include +#endif enum class launchState { IDLE = 0, @@ -107,6 +109,10 @@ class launchControllerPID : public launchController } int16_t calculateTorque(unsigned long elapsedTime, int16_t maxTorque, wheelSpeeds_s &wheelSpeedData); launchControlTypes_e getType() {return launchControlTypes_e::LC_PID;} + void setLcPidGains(double kp, double ki, double kd) + { + pid.setGains(d_kp,d_ki,d_kd); + } }; class launchControllerLinear : public launchController diff --git a/include/state_machine.hpp b/include/state_machine.hpp index 2f2f22f..dddaf2e 100644 --- a/include/state_machine.hpp +++ b/include/state_machine.hpp @@ -10,6 +10,7 @@ #include "dashboard.hpp" #include "parameters.hpp" #include "launch_system.h" +#include "tc_system.h" #include #define ODOMETER_EEPROM_ADDR 0 @@ -23,6 +24,7 @@ class StateMachine Metro *debug_; PedalHandler *pedals; launchControlSystem *lcSystem; + torque_control_system *tcSystem; Metro *pedal_check_; distance_tracker_s distance_tracker_motor; distance_tracker_s distance_tracker_fl; @@ -37,8 +39,8 @@ class StateMachine Metro can_10hz_timer = Metro(100,1); Metro _log_distance_timer_10s = Metro(10000,1); public: - StateMachine(Inverter *inv, Accumulator *acc, Metro *rs_tim, Dashboard *dash, Metro *debug, PedalHandler *pedals, launchControlSystem *lcSys,Metro *ped_t) - : pm100(inv), accumulator(acc), timer_ready_sound(rs_tim), dash_(dash), debug_(debug), pedals(pedals), lcSystem(lcSys),pedal_check_(ped_t){}; + StateMachine(Inverter *inv, Accumulator *acc, Metro *rs_tim, Dashboard *dash, Metro *debug, PedalHandler *pedals, launchControlSystem *lcSys, torque_control_system *tcSys,Metro *ped_t) + : pm100(inv), accumulator(acc), timer_ready_sound(rs_tim), dash_(dash), debug_(debug), pedals(pedals), lcSystem(lcSys),tcSystem(tcSys),pedal_check_(ped_t){}; void init_state_machine(MCU_status &mcu_status); void handle_state_machine(MCU_status &mcu_status); diff --git a/include/tc_system.h b/include/tc_system.h new file mode 100644 index 0000000..cf73ce8 --- /dev/null +++ b/include/tc_system.h @@ -0,0 +1,47 @@ +#ifndef TC_SYSTEM_H +#define TC_SYSTEM_H +#include +#include "torque_controller.hpp" +#include +#ifdef ARDUINO +#include +#endif +class torque_control_system +{ +public: + torque_control_system(std::initializer_list enabledTypes = {torque_control_types_e::TC_DRIVERCONTROL, torque_control_types_e::TC_PID}) + { + for (auto type : enabledTypes) + { + // Add only the enabled types to the map + if (tc_map.find(type) != tc_map.end()) + { + enabled_tc_map[type] = tc_map[type]; + } + } + } + + void setActiveSystem(torque_control_types_e systype) { tcType = systype; } + torque_control_types_e getActiveSystem() { return tcType; } + torque_controller *getController() + { + return static_cast(enabled_tc_map[tcType]); + } + void toggleController(unsigned long systime); + void printControllerType(); + +private: + torque_control_types_e tcType = torque_control_types_e::TC_DRIVERCONTROL; + torque_controller tc_base; + torque_controllerPID tc_pid = torque_controllerPID(1.0, 0, 0); + + // This map should contain ALL types + std::unordered_map tc_map = { + {torque_control_types_e::TC_DRIVERCONTROL, &tc_base}, + {torque_control_types_e::TC_PID, &tc_pid}}; + + // This map will only have enabled types + std::unordered_map enabled_tc_map; +}; + +#endif \ No newline at end of file diff --git a/include/torque_controller.hpp b/include/torque_controller.hpp new file mode 100644 index 0000000..5c6d8c9 --- /dev/null +++ b/include/torque_controller.hpp @@ -0,0 +1,98 @@ +#ifndef TORQUE_CONTROLLER_HPP +#define TORQUE_CONTROLLER_HPP +#include +#include +#include +#include +#ifdef ARDUINO +#include +#endif + +enum torque_control_types_e +{ + TC_DRIVERCONTROL = 0, // No firmware limiting, driver throttle directly + TC_PID = 1, // PID slip control + TC_NUM_CONTROLLERS +}; + +struct tc_diag_data_t { + unsigned long launchElapsedTime; + int16_t outputTorqueCommand; + uint8_t launchType; + tc_diag_data_t(unsigned long time, int16_t outputTorque, uint8_t type) + : launchElapsedTime(time), outputTorqueCommand(outputTorque),launchType(type) {} +}; + +class torque_controller +{ +protected: + unsigned long launchElapsedTime = 0; // ms time elapsed from start of launch + int16_t driverTorqueRequest = 0; // Driver torque command if using pedal travel + int16_t lcTorqueRequest = 0; // launch control system torque command + int16_t outputTorqueCommand = 0; // The final torque command that will be output from the launch controller +public: + void inittorque_controller(unsigned long sysTime); // general to all torque_controllers + + virtual int16_t calculate_torque(unsigned long elapsedTime, int16_t maxTorque, wheelSpeeds_s &wheelSpeedData) + { + float torqueOut = 0; + torqueOut = maxTorque; + return static_cast(torqueOut); + }; + virtual torque_control_types_e getType() {return torque_control_types_e::TC_DRIVERCONTROL;} + int getTorqueOutput() const { return outputTorqueCommand; }; // general + tc_diag_data_t getDiagData(); // 8 byte info on torque controller +}; + +class torque_controllerPID : public torque_controller +{ +private: + const double tireSlipHigh = 0.5; + double d_kp = 1.0; + double d_ki = 1.0; + double d_kd = 1.0; + const double output_min = -1.0; // Minimum output of the PID controller + const double output_max = 0; // Max output of the PID controller + double input, setpoint, output; + AutoPID pid; + struct tc_pid_t + { + int16_t _curr_slip = 0; + int16_t _target_slip = 0; + int16_t _kp; + int16_t _ki; + } tc_pid_t; + int joe = sizeof(tc_pid_t); + const uint8_t TC_PID_TIMESTEP = 5; //ms +public: + torque_controllerPID() : pid(&input, &setpoint, &output, output_min, output_max, d_kp, d_ki, d_kd) + { + output=0; + setpoint = tireSlipHigh; + pid.setBangBang(0); + pid.setTimeStep(TC_PID_TIMESTEP); // 5ms is approx how often we send a torque command to the inverter + // 3ms is the fastest we can do it + } + torque_controllerPID(double kp, double ki, double kd) : pid(&input, &setpoint, &output, output_min, output_max, kp, ki, kd) + { + tc_pid_t._curr_slip = 0; + this->d_kd=kd; + this->d_kp = kp; + this->d_ki = ki; + pid.setGains(d_kp,d_ki,d_kd); + output=0; + setpoint = tireSlipHigh; + tc_pid_t._target_slip = static_cast(setpoint*100); + + input = 0; + pid.setBangBang(0); + pid.setTimeStep(TC_PID_TIMESTEP); + } + int16_t calculate_torque(unsigned long elapsedTime, int16_t maxTorque, wheelSpeeds_s &wheelSpeedData); + torque_control_types_e getType() {return torque_control_types_e::TC_PID;} + void setTcPidGains(double kp, double ki, double kd) + { + pid.setGains(d_kp,d_ki,d_kd); + } +}; +#endif \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 1dd44c7..ebbd8b8 100644 --- a/platformio.ini +++ b/platformio.ini @@ -29,7 +29,7 @@ lib_deps = https://github.com/KSU-MS/ksu-autoPID https://github.com/PaulStoffregen/FreqMeasureMulti https://github.com/KSU-MS/pio-git-hash-gen - https://github.com/KSU-MS/ksu-ms-dbc/releases/download/14/can_lib.tar.gz + https://github.com/KSU-MS/ksu-ms-dbc/releases/download/26/can_lib.tar.gz build_unflags = -std=gnu++11 build_flags = -std=c++17 diff --git a/src/launch_controller.cpp b/src/launch_controller.cpp index bcd3d8b..9b40372 100644 --- a/src/launch_controller.cpp +++ b/src/launch_controller.cpp @@ -81,6 +81,10 @@ int16_t launchControllerPID::calculateTorque(unsigned long elapsedTime, int16_t // Calculate front and rear wheel speeds - take average of left and right float frontRpmAvg = ((wheelSpeedData.fl+wheelSpeedData.fr)/2); float rearRpmAvg = ((wheelSpeedData.rl+wheelSpeedData.rr)/2); + + printf("LC Front avg: %f Rear avg: %f\n",frontRpmAvg,rearRpmAvg); + printf("LC WHEELSPEEDS \nFL: %f FR: %f\nRL: %f RR: %f\n",wheelSpeedData.fl,wheelSpeedData.fr,wheelSpeedData.rl,wheelSpeedData.rr); + // avoid zero division if (frontRpmAvg || rearRpmAvg <= 0.001) { diff --git a/src/main.cpp b/src/main.cpp index 2998c2b..c6c3b01 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -58,14 +58,16 @@ Metro timer_can_update = Metro(1000, 1); // Wheel speed shit FreqMeasureMulti wsfl; FreqMeasureMulti wsfr; - +#define ENABLED_LC_TYPES {launchControlTypes_e::LC_DRIVERCONTROL,launchControlTypes_e::LC_LINEAR,launchControlTypes_e::LC_PID} +#define ENABLED_TC_TYPES {torque_control_types_e::TC_DRIVERCONTROL,torque_control_types_e::TC_PID} // objects Dashboard dash; Inverter pm100(&timer_mc_kick_timer, &timer_inverter_enable, &timer_motor_controller_send, &timer_current_limit_send, &dash); Accumulator accum(&precharge_timeout,&ksu_can); PedalHandler pedals(&timer_debug_pedals_raw, &pedal_out_20hz, &pedal_out_1hz, &speedPID, ¤t_rpm, &set_rpm, &throttle_out, &wsfl, &wsfr); -launchControlSystem launchSystem; // THIS WILL INCLUDE *ALL* LAUNCH MODES BY DEFAULT -StateMachine state_machine(&pm100, &accum, &timer_ready_sound, &dash, &debug_tim, &pedals, &launchSystem, &pedal_check); +launchControlSystem launchSystem(ENABLED_LC_TYPES); // THIS WILL INCLUDE *ALL* LAUNCH MODES BY DEFAULT +torque_control_system tractionControlSystem(ENABLED_TC_TYPES); +StateMachine state_machine(&pm100, &accum, &timer_ready_sound, &dash, &debug_tim, &pedals, &launchSystem, &tractionControlSystem, &pedal_check); MCU_status mcu_status = MCU_status(); static CAN_message_t mcu_status_msg; diff --git a/src/pedal_handler.cpp b/src/pedal_handler.cpp index 3126ae5..2efb2f7 100644 --- a/src/pedal_handler.cpp +++ b/src/pedal_handler.cpp @@ -284,7 +284,9 @@ void PedalHandler::verify_pedals( } // idgaf anything below (all wheel speed) +//Get front right WS reading float PedalHandler::get_wsfr() { return wsfr_t.current_rpm; } +//Get front left WS reading float PedalHandler::get_wsfl() { return wsfl_t.current_rpm; } /** * @brief update wheel speed readings diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 5a0cf9c..e869203 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -150,6 +150,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) if (_20hz_send) { sendStructOnCan(lcSystem->getController()->getDiagData(), ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); + sendStructOnCan(tcSystem->getController()->getDiagData(), ID_VCU_TRACTION_CONTROLLER_INFO); } if (_100hz_send) @@ -164,6 +165,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) } +// DASH BUTTON INTERACTIONS // If dash button is on and has been on for 750ms // AND the motor is not spinning!! if ((!dash_->get_button(2))) @@ -176,6 +178,14 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) send_state_msg(mcu_status); } } + +// TODO test this + if (dash_->get_button_held_duration(3,500) && (pm100->getmcMotorRPM() <= 300)) + { + tcSystem->toggleController(millis()); + tcSystem->getController()->inittorque_controller(millis()); + sendStructOnCan(tcSystem->getController()->getDiagData(),ID_VCU_TRACTION_CONTROLLER_INFO); + } // If dash button held and LC not active if (!dash_->get_button(6)) { @@ -187,6 +197,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) sendStructOnCan(lcSystem->getController()->getDiagData(), ID_VCU_BASE_LAUNCH_CONTROLLER_INFO); } } + // End of (most) dash thingies // Do Torque Calcs here int16_t calculated_torque = 0; bool accel_is_plausible = false; @@ -210,8 +221,10 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) #if USE_INVERTER motor_speed = pm100->getmcMotorRPM(); #endif +// TODO test TCs calculated_torque = pedals->calculate_torque(motor_speed, max_t_actual); - + wheelSpeeds_s wheelSpeedData = {pedals->get_wsfl(), pedals->get_wsfr(), pm100->getmcMotorRPM(), pm100->getmcMotorRPM()}; + calculated_torque = tcSystem->getController()->calculate_torque(millis(),calculated_torque,wheelSpeedData); // REGEN if (mcu_status.get_brake_pedal_active() && dash_->get_button2() && calculated_torque < 5) { @@ -531,7 +544,7 @@ void StateMachine::handle_distance_trackers(MCU_status &mcu_status) if (_10s_timer_fired) { unsigned long temporary_total_time = _lifetime_on_time + millis()/1000; - EEPROM.put(10,temporary_total_time); + // EEPROM.put(10,temporary_total_time); time_and_distance_t.vcu_lifetime_ontime = temporary_total_time; Serial.printf("Wrote total time: initial: %d millis: %d total: %d\n",_lifetime_on_time,millis()/1000,temporary_total_time); } @@ -544,7 +557,7 @@ void StateMachine::handle_distance_trackers(MCU_status &mcu_status) if (_10s_timer_fired) { unsigned long temporary_total_distance = _lifetime_distance + distance_tracker_motor.get_data().distance_m; - EEPROM.put(ODOMETER_EEPROM_ADDR,temporary_total_distance); + // EEPROM.put(ODOMETER_EEPROM_ADDR,temporary_total_distance); time_and_distance_t.vcu_lifetime_distance = temporary_total_distance; Serial.printf("Wrote total distance: initial: %d millis: %d total: %d",_lifetime_distance,distance_tracker_motor.get_data().distance_m,temporary_total_distance); } diff --git a/src/tc_system.cpp b/src/tc_system.cpp new file mode 100644 index 0000000..ede8d44 --- /dev/null +++ b/src/tc_system.cpp @@ -0,0 +1,51 @@ +#include "tc_system.h" +// #include +void torque_control_system::toggleController(unsigned long systime) +{ + int current_controller = static_cast(tcType); + int next_controller = current_controller + 1; + while (true) + { + if (next_controller >= static_cast(torque_control_types_e::TC_NUM_CONTROLLERS)) + { + next_controller = 0; + } + if (enabled_tc_map.find(static_cast(next_controller)) != enabled_tc_map.end()) + { + break; // Found an enabled controller + } + next_controller++; + } + + this->setActiveSystem(static_cast(next_controller)); + + // init new system + this->getController()->inittorque_controller(systime); +} + +void torque_control_system::printControllerType() +{ + torque_controller* controller = getController(); + if (controller) + { + switch (controller->getType()){ + case (torque_control_types_e::TC_DRIVERCONTROL): + { + printf("Controller type is tc_Base"); + break; + } + case (torque_control_types_e::TC_PID): + { + printf("Controller type is tc_PID"); + break; + } + case (torque_control_types_e::TC_NUM_CONTROLLERS): + { + printf("? LOL"); + break; + } + default: + break; + } + } +} \ No newline at end of file diff --git a/src/torque_controller.cpp b/src/torque_controller.cpp new file mode 100644 index 0000000..70091b2 --- /dev/null +++ b/src/torque_controller.cpp @@ -0,0 +1,56 @@ +#include "torque_controller.hpp" + +void torque_controller::inittorque_controller(unsigned long sysTime) +{ + driverTorqueRequest = 0; + lcTorqueRequest = 0; + outputTorqueCommand = 0; + this->launchElapsedTime = 0; +} + + +tc_diag_data_t torque_controller::getDiagData() +{ + tc_diag_data_t diagData = tc_diag_data_t{this->launchElapsedTime,this->outputTorqueCommand,static_cast(this->getType())}; + return diagData; +} + +// PID to get optimal slip +int16_t torque_controllerPID::calculate_torque(unsigned long elapsedTime, int16_t maxTorque, wheelSpeeds_s &wheelSpeedData) +{ + driverTorqueRequest = maxTorque; + float torqueOut = 0; + + // Calculate front and rear wheel speeds - take average of left and right + float frontRpmAvg = ((wheelSpeedData.fl+wheelSpeedData.fr)/2); + float rearRpmAvg = ((wheelSpeedData.rl+wheelSpeedData.rr)/2); + printf("TC Front avg: %f Rear avg: %f\n",frontRpmAvg,rearRpmAvg); + printf("TC WHEELSPEEDS \nFL: %f FR: %f\nRL: %f RR: %f\n",wheelSpeedData.fl,wheelSpeedData.fr,wheelSpeedData.rl,wheelSpeedData.rr); + // avoid zero division + if (frontRpmAvg || rearRpmAvg <= 0.001) + { + this->input = 0; // treat it like 0 slip (maybe this is bad) + } + else + { + // Slip = (rear / front) - 1 + // ie. 1000rpm/900rpm = 1.111.. + // 1.111 - 1 = 0.111 slip ratio + this->input = (rearRpmAvg / frontRpmAvg) - 1; + } + + pid.run(elapsedTime); + torqueOut = maxTorque + (output * maxTorque); + lcTorqueRequest = static_cast(torqueOut); // Pre clamping + if (torqueOut > maxTorque) + { + torqueOut = maxTorque; + } + if (torqueOut < 0) + { + torqueOut = 0; + } + tc_pid_t._curr_slip = static_cast(input*100); + outputTorqueCommand = static_cast(torqueOut); // Post-clamping + return outputTorqueCommand; +} diff --git a/test/test_systems/test_main.cpp b/test/test_systems/test_main.cpp index a30aad1..e89d897 100644 --- a/test/test_systems/test_main.cpp +++ b/test/test_systems/test_main.cpp @@ -2,7 +2,7 @@ #include "test_launch_control.h" #include "test_mcu_state.h" #include "test_distance_tracker.h" - +#include "test_tc.h" int main(int argc, char **argv) { diff --git a/test/test_systems/test_tc.h b/test/test_systems/test_tc.h new file mode 100644 index 0000000..4e69308 --- /dev/null +++ b/test/test_systems/test_tc.h @@ -0,0 +1,74 @@ +#ifndef TC_TEST +#define TC_TEST +#include +#include +#include +#include "torque_controller.hpp" +#include "torque_controller.cpp" +#include "tc_system.h" +#include "tc_system.cpp" +#include "AutoPID.h" + +TEST(tcTesting, test_traction_control_exists) +{ + torque_control_system tcSystem; + EXPECT_EQ(tcSystem.getController()->getTorqueOutput(),0); +} + +TEST(tcTesting, test_tc_system_toggle) +{ + torque_control_system tcSystem; + torque_controller* torqueControl = tcSystem.getController(); + for (int i = 0; i < static_cast(torque_control_types_e::TC_NUM_CONTROLLERS); i++) + { + torque_control_types_e c = tcSystem.getController()->getType(); + torque_control_types_e y = static_cast(i); + EXPECT_EQ(c,y); + tcSystem.toggleController(0); + } +} + +TEST(tcTesting, test_tc_pid) +{ + + torque_control_system tcSystem; + torque_control_types_e typeToSet = torque_control_types_e::TC_PID; + tcSystem.setActiveSystem(typeToSet); + torque_control_types_e c = tcSystem.getController()->getType(); + ASSERT_EQ(c,typeToSet); + torque_controller* torqueControl = tcSystem.getController(); + + unsigned long t = 0; + torqueControl->inittorque_controller(t); + for (float i = 10; i < 6000; i += 10) + { + wheelSpeeds_s wsData = wheelSpeeds_s(i,i,i*1.01,i*1.01); + int16_t driver_torque = 2400; + int16_t out = torqueControl->calculate_torque(t+1,driver_torque,wsData); + printf("time: %dms, slip: %f, output torque: %d\n",t,wsData.rl/wsData.fl,out); + t++; + } +} + +TEST(tcTesting, test_tc_linear) +{ + + torque_control_system tcSystem; + torque_control_types_e typeToSet = torque_control_types_e::TC_DRIVERCONTROL; + tcSystem.setActiveSystem(typeToSet); + torque_control_types_e c = tcSystem.getController()->getType(); + ASSERT_EQ(c,typeToSet); + torque_controller* torqueControl = tcSystem.getController(); + + unsigned long t = 0; + torqueControl->inittorque_controller(t); + for (float i = 10; i < 6000; i += 10) + { + wheelSpeeds_s wsData = wheelSpeeds_s(i,i,i*1.01,i*1.01); + int16_t driver_torque = 2400; + int16_t out = torqueControl->calculate_torque(t+1,driver_torque,wsData); + printf("time: %dms, slip: %f, output torque: %d\n",t,wsData.rl/wsData.fl,out); + t++; + } +} +#endif \ No newline at end of file From a038c9349b846815654995d8a0fc7affb0f37271 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Mon, 20 May 2024 01:17:34 -0400 Subject: [PATCH 26/29] whee i fixed it also fixed swappd buttons --- include/distance_tracker.h | 8 ++++++-- include/torque_controller.hpp | 1 + src/state_machine.cpp | 4 ++-- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/include/distance_tracker.h b/include/distance_tracker.h index 495d956..3071caa 100644 --- a/include/distance_tracker.h +++ b/include/distance_tracker.h @@ -22,7 +22,7 @@ struct energy_data_t struct RollingAverage { std::deque buffer; - float sum; + float sum = 0; int maxSize; RollingAverage(int maxBufferSize) : maxSize(maxBufferSize) {} @@ -30,7 +30,7 @@ struct RollingAverage { void addValue(float value) { buffer.push_back(value); sum = sum + value; - printf("%f",sum); + // printf("Sum: %f\n",sum); if (buffer.size() > maxSize) { sum -= buffer.front(); buffer.pop_front(); @@ -70,6 +70,10 @@ class distance_tracker_s // update efficiencies efficiency_kmkwh = distance_m/energy_wh; efficiency_instantaneous = (elapsed_time_seconds * velocity_ms.oldval) / (elapsed_time_seconds / 3600 * power_kw.oldval); + if (power_kw.oldval <= 0.01) + { + efficiency_instantaneous = 0; + } avgEff.addValue(efficiency_instantaneous); // set old vals to the previous new one power_kw.oldval = power_kw.newval; diff --git a/include/torque_controller.hpp b/include/torque_controller.hpp index 5c6d8c9..0b195a3 100644 --- a/include/torque_controller.hpp +++ b/include/torque_controller.hpp @@ -18,6 +18,7 @@ enum torque_control_types_e struct tc_diag_data_t { unsigned long launchElapsedTime; int16_t outputTorqueCommand; + uint8_t joe = 0; uint8_t launchType; tc_diag_data_t(unsigned long time, int16_t outputTorque, uint8_t type) : launchElapsedTime(time), outputTorqueCommand(outputTorque),launchType(type) {} diff --git a/src/state_machine.cpp b/src/state_machine.cpp index e869203..b3f8e60 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -180,7 +180,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) } // TODO test this - if (dash_->get_button_held_duration(3,500) && (pm100->getmcMotorRPM() <= 300)) + if (dash_->get_button_held_duration(4,500) && (pm100->getmcMotorRPM() <= 300)) { tcSystem->toggleController(millis()); tcSystem->getController()->inittorque_controller(millis()); @@ -317,7 +317,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) #endif // Serial.println(dash_->get_button3()); // if start button has been pressed and brake pedal is held down, transition to the next state - if (dash_->get_button4() && mcu_status.get_brake_pedal_active()) + if (dash_->get_button3() && mcu_status.get_brake_pedal_active()) { set_state(mcu_status, MCU_STATE::ENABLING_INVERTER); } From 7c3859f9f299c6b5a63a98b4f45ee910c0058ee6 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Thu, 23 May 2024 19:54:38 -0400 Subject: [PATCH 27/29] comment eeprom writes till on car fr --- src/state_machine.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/state_machine.cpp b/src/state_machine.cpp index b3f8e60..adbe875 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -540,7 +540,7 @@ void StateMachine::handle_distance_trackers(MCU_status &mcu_status) Serial.printf("==Handled distance tracker!==\n"); #endif bool _10s_timer_fired = _log_distance_timer_10s.check(); - + // TODO uncomment the eeprom writes if (_10s_timer_fired) { unsigned long temporary_total_time = _lifetime_on_time + millis()/1000; From e8a15f08cebeb03c1aa02735220f864e6042d6f0 Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sat, 25 May 2024 22:59:34 -0400 Subject: [PATCH 28/29] clamp torque --- src/inverter.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/inverter.cpp b/src/inverter.cpp index f481697..8b604a3 100644 --- a/src/inverter.cpp +++ b/src/inverter.cpp @@ -93,20 +93,29 @@ void Inverter::writeEnableNoTorque() /** * @brief Sends torque command to the inverter - * + * * @param torque the 0 - 3000 torque value (in Nm x 10) * @return true if sent succesfully * @return false if not */ bool Inverter::command_torque(int16_t torque) { + // For now, this will not allow negative torque (regen) + if (torque > (TORQUE_4 * 10)) + { + torque = TORQUE_4*10; + } + else if (torque < 0) + { + torque = 0; + } uint8_t angularVelocity1 = 0, angularVelocity2 = 0; bool emraxDirection = true; // true for forward, false for reverse bool inverterEnable = true; // go brrr uint8_t torqueCommand[] = { 0, 0, angularVelocity1, angularVelocity2, emraxDirection, inverterEnable, 0, 0}; - memcpy(&torqueCommand[0],&torque,sizeof(torque)); + memcpy(&torqueCommand[0], &torque, sizeof(torque)); // Send torque command if timer has fired if (timer_motor_controller_send->check()) { @@ -116,7 +125,7 @@ bool Inverter::command_torque(int16_t torque) memcpy(ctrlMsg.buf, torqueCommand, sizeof(ctrlMsg.buf)); return (WriteCANToInverter(ctrlMsg)); } - else //do nothing + else // do nothing { return false; } From df88c894e90573f12e553b756c0311ebe190849b Mon Sep 17 00:00:00 2001 From: Mathewos Samson Date: Sun, 26 May 2024 21:31:11 -0400 Subject: [PATCH 29/29] enabled eeprom writes and added torque mode write to eeprom --- include/parameters.hpp | 2 +- include/state_machine.hpp | 2 ++ src/main.cpp | 11 +++++++++-- src/pedal_handler.cpp | 4 ++++ src/state_machine.cpp | 7 ++++--- 5 files changed, 20 insertions(+), 6 deletions(-) diff --git a/include/parameters.hpp b/include/parameters.hpp index 872f90b..0d1722e 100644 --- a/include/parameters.hpp +++ b/include/parameters.hpp @@ -29,7 +29,7 @@ const unsigned long LAUNCHCONTROL_RELEASE_DELAY = 1500; #define WHEELSPEED_TOOTH_COUNT 18 // Wheel Speed sensor tooth coutn const float WHEEL_CIRCUMFERENCE = 0.229*PI*2; const float FRONT_SPROCKET_TEETH = 10; -const float REAR_SPROCKET_TEETH = 30; +const float REAR_SPROCKET_TEETH = 29; const float FINAL_DRIVE = FRONT_SPROCKET_TEETH/REAR_SPROCKET_TEETH; #define RPM_TIMEOUT 1000 // Timeout for wheel speed RPM to reset to 0 #define MIN_BRAKE_PEDAL 400 // ~0.5v, set on 2-29-2024 diff --git a/include/state_machine.hpp b/include/state_machine.hpp index dddaf2e..928fa56 100644 --- a/include/state_machine.hpp +++ b/include/state_machine.hpp @@ -14,6 +14,8 @@ #include #define ODOMETER_EEPROM_ADDR 0 +#define ONTIME_EEPROM_ADDR 10 +#define TORQUE_MODE_EEPROM_ADDR 20 class StateMachine { private: diff --git a/src/main.cpp b/src/main.cpp index c6c3b01..c3269be 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -121,8 +121,15 @@ void setup() memcpy(fw_hash_msg.buf, &vcu_status_t, sizeof(vcu_status_t)); mcu_status.set_inverter_powered(true); // note VCU does not control inverter power on rev3 - mcu_status.set_torque_mode(1); // TODO torque modes should be an enum - mcu_status.set_max_torque(TORQUE_1); // TORQUE_1=60nm, 2=120nm, 3=180nm, 4=240nm + uint8_t last_mode = 1; + EEPROM.get(TORQUE_MODE_EEPROM_ADDR,last_mode); + if (last_mode > 4 || last_mode < 1) + { + last_mode = 3; + } + Serial.printf("Last torque mode (from EEPROM: %d)\n",last_mode); + mcu_status.set_torque_mode(last_mode); // TODO torque modes should be an enum + mcu_status.set_max_torque(torque_mode_list[last_mode-1]); // TORQUE_1=60nm, 2=120nm, 3=180nm, 4=240nm state_machine.init_state_machine(mcu_status); } diff --git a/src/pedal_handler.cpp b/src/pedal_handler.cpp index 2efb2f7..a53b23e 100644 --- a/src/pedal_handler.cpp +++ b/src/pedal_handler.cpp @@ -308,6 +308,10 @@ void PedalHandler::update_wheelspeed(unsigned long current_time_millis, wheelspe if (ws->count > 1) { float testRpm = freq->countToFrequency(ws->sum / ws->count) * 60 / WHEELSPEED_TOOTH_COUNT; + if (testRpm > 6000) + { + testRpm = ws->current_rpm; + } ws->current_rpm = testRpm; /*if ( testRpm - prev_rpm < 1) diff --git a/src/state_machine.cpp b/src/state_machine.cpp index adbe875..0caf243 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -4,7 +4,7 @@ void StateMachine::init_state_machine(MCU_status &mcu_status) { EEPROM.get(ODOMETER_EEPROM_ADDR,_lifetime_distance); - EEPROM.get(10,_lifetime_on_time); + EEPROM.get(ONTIME_EEPROM_ADDR,_lifetime_on_time); Serial.printf("Loaded lifetime distance: %d meters (%f km)\n",_lifetime_distance,static_cast(_lifetime_distance)/1000); Serial.printf("Loaded lifetime on_time: %d seconds (%f hours)\n",_lifetime_on_time,static_cast(_lifetime_on_time)/3600); set_state(mcu_status, MCU_STATE::STARTUP); @@ -175,6 +175,7 @@ void StateMachine::handle_state_machine(MCU_status &mcu_status) dash_->set_button_last_pressed_time(0, 6); mcu_status.toggle_max_torque(mcu_status.get_torque_mode()); mcu_status.set_max_torque(torque_mode_list[mcu_status.get_torque_mode() - 1]); + EEPROM.put(TORQUE_MODE_EEPROM_ADDR,mcu_status.get_torque_mode()); send_state_msg(mcu_status); } } @@ -544,7 +545,7 @@ void StateMachine::handle_distance_trackers(MCU_status &mcu_status) if (_10s_timer_fired) { unsigned long temporary_total_time = _lifetime_on_time + millis()/1000; - // EEPROM.put(10,temporary_total_time); + EEPROM.put(ONTIME_EEPROM_ADDR,temporary_total_time); time_and_distance_t.vcu_lifetime_ontime = temporary_total_time; Serial.printf("Wrote total time: initial: %d millis: %d total: %d\n",_lifetime_on_time,millis()/1000,temporary_total_time); } @@ -557,7 +558,7 @@ void StateMachine::handle_distance_trackers(MCU_status &mcu_status) if (_10s_timer_fired) { unsigned long temporary_total_distance = _lifetime_distance + distance_tracker_motor.get_data().distance_m; - // EEPROM.put(ODOMETER_EEPROM_ADDR,temporary_total_distance); + EEPROM.put(ODOMETER_EEPROM_ADDR,temporary_total_distance); time_and_distance_t.vcu_lifetime_distance = temporary_total_distance; Serial.printf("Wrote total distance: initial: %d millis: %d total: %d",_lifetime_distance,distance_tracker_motor.get_data().distance_m,temporary_total_distance); }