From cc9a454bf6e5cce92441ca3041e6d33cd6e756e2 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 09:45:56 -0500 Subject: [PATCH 01/15] moved states out of the state machine for drivetrain startup --- README.md | 8 +-- lib/state_machine/include/MCUStateMachine.h | 3 +- lib/state_machine/include/MCUStateMachine.tpp | 55 +------------------ lib/systems/include/DrivetrainSystem.h | 52 ++++++++++-------- lib/systems/include/DrivetrainSystem.tpp | 43 ++++++++++----- 5 files changed, 64 insertions(+), 97 deletions(-) diff --git a/README.md b/README.md index c35f7923b..885ce3f27 100644 --- a/README.md +++ b/README.md @@ -147,8 +147,6 @@ stateDiagram-v2 trac_sys_na : TRACTIVE_SYSTEM_NOT_ACTIVE trac_sys_a : TRACTIVE_SYSTEM_ACTIVE inv_en : ENABLING_INVERTERS - dt_q_dc_on : WAITING_DRIVETRAIN_QUIT_DC_ON - dt_en : WAITING_DRIVETRAIN_ENABLED rtd_s : WAITING_READY_TO_DRIVE_SOUND rtd : READY_TO_DRIVE @@ -157,11 +155,7 @@ stateDiagram-v2 trac_sys_a --> trac_sys_na: drivetrain voltage _not_ over threshold trac_sys_a --> inv_en: brake and start button pressed inv_en --> trac_sys_na: drivetrain voltage _not_ over threshold - inv_en --> dt_q_dc_on: drivetrain ready - dt_q_dc_on --> trac_sys_a: drivetrain init timeout - dt_q_dc_on --> dt_en: drivetrain quit dc statuses on - dt_en --> trac_sys_a: drivetrain initialization timeout - dt_en --> rtd_s: drivetrain enabled + inv_en --> rtd_s: drivetrain enabled rtd_s --> trac_sys_na: drivetrain voltage _not_ over threshold rtd_s --> rtd: buzzer done buzzing rtd --> trac_sys_na: drivetrain voltage _not_ over threshold diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index e78052cd1..dd08787b3 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -19,8 +19,6 @@ enum class CAR_STATE TRACTIVE_SYSTEM_NOT_ACTIVE = 1, TRACTIVE_SYSTEM_ACTIVE = 2, ENABLING_INVERTERS = 3, - WAITING_DRIVETRAIN_QUIT_DC_ON = 4, - WAITING_DRIVETRAIN_ENABLED = 5, WAITING_READY_TO_DRIVE_SOUND = 6, READY_TO_DRIVE = 7 }; @@ -44,6 +42,7 @@ class MCUStateMachine CAR_STATE get_state() { return current_state_; } private: + void set_state_(CAR_STATE new_state, unsigned long curr_time); /// @brief the function run upon the entry of the car into a new state diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index e5a1336e8..e6c2e9ce2 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -38,52 +38,18 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::ENABLING_INVERTERS: { - // TODO handle the drivetrain state change back to startup phase 1 and/or move this into - // the drivetrain state machine handling if (!drivetrain_->hv_over_threshold_on_drivetrain()) { set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); break; } - - if (drivetrain_->drivetrain_ready()) - { - // entry logic: drivetrain_->enable_drivetrain_hv(current_millis); - set_state_(CAR_STATE::WAITING_DRIVETRAIN_QUIT_DC_ON, current_millis); - break; - } - break; - } - - case CAR_STATE::WAITING_DRIVETRAIN_QUIT_DC_ON: - { - if (drivetrain_->check_drivetrain_quit_dc_on() && !drivetrain_->inverter_init_timeout(current_millis)) - { - set_state_(CAR_STATE::WAITING_DRIVETRAIN_ENABLED, current_millis); - break; - } - else if(drivetrain_->inverter_init_timeout(current_millis)) - { - set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); - break; - } else { - break; - } - break; - } - case CAR_STATE::WAITING_DRIVETRAIN_ENABLED: - { - if (drivetrain_->drivetrain_enabled() && !drivetrain_->inverter_init_timeout(current_millis)) + // TODO handle drivetrain init timeout + if (drivetrain_->handle_inverter_startup()) { set_state_(CAR_STATE::WAITING_READY_TO_DRIVE_SOUND, current_millis); - } - else if (drivetrain_->inverter_init_timeout(current_millis)) - { - set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); - break; - } else { break; } + break; } case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: { @@ -171,10 +137,6 @@ void MCUStateMachine::handle_exit_logic_(CAR_STATE prev_state break; case CAR_STATE::ENABLING_INVERTERS: break; - case CAR_STATE::WAITING_DRIVETRAIN_QUIT_DC_ON: - break; - case CAR_STATE::WAITING_DRIVETRAIN_ENABLED: - break; case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: break; case CAR_STATE::READY_TO_DRIVE: @@ -200,20 +162,9 @@ void MCUStateMachine::handle_entry_logic_(CAR_STATE new_state { break; } - case CAR_STATE::WAITING_DRIVETRAIN_QUIT_DC_ON: - { - drivetrain_->enable_drivetrain_hv(curr_time); - break; - } - case CAR_STATE::WAITING_DRIVETRAIN_ENABLED: - { - drivetrain_->request_enable(); - break; - } case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: // make dashboard sound buzzer buzzer_->activate_buzzer(curr_time); - hal_println("RTDS enabled"); break; case CAR_STATE::READY_TO_DRIVE: diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 9e62be225..2abf9da74 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -5,7 +5,7 @@ #include #include "stdint.h" struct DrivetrainCommand_s -{ +{ float speeds[NUM_MOTORS]; float posTorqueLimits[NUM_MOTORS]; float negTorqueLimits[NUM_MOTORS]; @@ -20,52 +20,58 @@ struct DrivetrainDynamicReport_s float measuredMagnetizingCurrents[NUM_MOTORS]; }; -template +template class DrivetrainSystem { public: /// @brief order of array: 0: FL, 1: FR, 2: RL, 3: RR /// @param inverters inverter pointers - DrivetrainSystem(const std::array &inverters, int init_time_limit_ms ) - : inverters_(inverters),init_time_limit_ms_(init_time_limit_ms) + DrivetrainSystem(const std::array &inverters, int init_time_limit_ms) + : inverters_(inverters), init_time_limit_ms_(init_time_limit_ms) { - + hv_en_requested_ = false; + enable_requested_ = false; // TODO set min_hv_voltage_ } - // startup phase 1 - // status check for start of enable - bool drivetrain_ready(); - /// @param curr_time current system tick time (millis()) that sets the init phase start time - void enable_drivetrain_hv(unsigned long curr_time); - - // startup phase 2 - bool check_drivetrain_quit_dc_on(); - + void setup_retry() { + reset_drivetrain(); + hv_en_requested_ = false; + enable_requested_ = false; + } + bool handle_inverter_startup(unsigned long curr_time); // on entry logic - void request_enable(); void command_drivetrain_no_torque(); - - // final check for drivetrain initialization to check if quit inverter on - bool drivetrain_enabled(); - - // check to see if init time limit has passed + // check to see if init time limit has passed bool inverter_init_timeout(unsigned long curr_time); bool hv_over_threshold_on_drivetrain(); void disable(); bool drivetrain_error_occured(); void reset_drivetrain(); + void command_drivetrain(const DrivetrainCommand_s &data); - void command_drivetrain(const DrivetrainCommand_s& data); private: + // startup statuses: + bool hv_en_requested_, enable_requested_; + /// @param curr_time current system tick time (millis()) that sets the init phase start time + void enable_drivetrain_hv_(unsigned long curr_time); + void request_enable_(); + // startup phase 1 + // status check for start of enable + bool drivetrain_ready_(); + // startup phase 2 + bool check_drivetrain_quit_dc_on_(); + + // final check for drivetrain initialization to check if quit inverter on + bool drivetrain_enabled_(); + uint16_t min_hv_voltage_; - std::array inverters_; + std::array inverters_; int init_time_limit_ms_; unsigned long drivetrain_initialization_phase_start_time_; - }; #include "DrivetrainSystem.tpp" diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index 6afc33d50..a847cac2f 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -6,9 +6,26 @@ bool DrivetrainSystem::inverter_init_timeout(unsigned long curr_ti return ((int)(curr_time - drivetrain_initialization_phase_start_time_) > init_time_limit_ms_); } +template +bool DrivetrainSystem::handle_inverter_startup(unsigned long curr_time) +{ + if (drivetrain_ready_() && !hv_en_requested_) + { + enable_drivetrain_hv_(curr_time); + hv_en_requested_ = true; + } + else if (drivetrain_ready_() && check_drivetrain_quit_dc_on_() && !enable_requested_) + { + request_enable_(); + enable_requested_ = true; + } + bool all_ready = ( drivetrain_ready_() && check_drivetrain_quit_dc_on_() && drivetrain_enabled_() ); + return all_ready; +} + // command functions template -void DrivetrainSystem::enable_drivetrain_hv(unsigned long curr_time) +void DrivetrainSystem::enable_drivetrain_hv_(unsigned long curr_time) { for (auto inv_pointer : inverters_) @@ -19,7 +36,7 @@ void DrivetrainSystem::enable_drivetrain_hv(unsigned long curr_tim } template -void DrivetrainSystem::request_enable() +void DrivetrainSystem::request_enable_() { for (auto inv_pointer : inverters_) { @@ -41,7 +58,7 @@ bool DrivetrainSystem::drivetrain_error_occured() { for (auto inv_pointer : inverters_) { - if(inv_pointer->error()) + if (inv_pointer->error()) { return true; } @@ -59,30 +76,30 @@ void DrivetrainSystem::reset_drivetrain() } template -void DrivetrainSystem::command_drivetrain(const DrivetrainCommand_s& data) +void DrivetrainSystem::command_drivetrain(const DrivetrainCommand_s &data) { // inverters_[0]->handle_command(data.left_front_inverter_cmd); // inverters_[1]->handle_command(data.right_front_inverter_cmd); // inverters_[2]->handle_command(data.left_rear_inverter_cmd); // inverters_[3]->handle_command(data.right_rear_inverter_cmd); - } // feedback functions template bool DrivetrainSystem::hv_over_threshold_on_drivetrain() { - for(auto inv_pointer : inverters_) + for (auto inv_pointer : inverters_) { - if(!(inv_pointer->dc_bus_voltage() > min_hv_voltage_)){ + if (!(inv_pointer->dc_bus_voltage() > min_hv_voltage_)) + { return false; } } return true; } -template -bool DrivetrainSystem::drivetrain_ready() +template +bool DrivetrainSystem::drivetrain_ready_() { for (auto inv_pointer : inverters_) { @@ -94,8 +111,8 @@ bool DrivetrainSystem::drivetrain_ready() return true; } -template -bool DrivetrainSystem::check_drivetrain_quit_dc_on() +template +bool DrivetrainSystem::check_drivetrain_quit_dc_on_() { for (auto inv_pointer : inverters_) { @@ -107,8 +124,8 @@ bool DrivetrainSystem::check_drivetrain_quit_dc_on() return true; } -template -bool DrivetrainSystem::drivetrain_enabled() +template +bool DrivetrainSystem::drivetrain_enabled_() { for (auto inv_pointer : inverters_) { From dc51166906c1213023cf8005d21f1a3e4b773a09 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 11:38:25 -0500 Subject: [PATCH 02/15] finishing tests for state machine --- lib/state_machine/include/MCUStateMachine.h | 5 +- lib/state_machine/include/MCUStateMachine.tpp | 11 + lib/systems/src/Buzzer.cpp | 4 +- test/state_machine_test.h | 210 +++++++++++++++--- 4 files changed, 191 insertions(+), 39 deletions(-) diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index dd08787b3..13652cb3f 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -19,8 +19,8 @@ enum class CAR_STATE TRACTIVE_SYSTEM_NOT_ACTIVE = 1, TRACTIVE_SYSTEM_ACTIVE = 2, ENABLING_INVERTERS = 3, - WAITING_READY_TO_DRIVE_SOUND = 6, - READY_TO_DRIVE = 7 + WAITING_READY_TO_DRIVE_SOUND = 4, + READY_TO_DRIVE = 5 }; template @@ -29,6 +29,7 @@ class MCUStateMachine public: MCUStateMachine(BuzzerController *buzzer, DrivetrainSysType *drivetrain, DashboardInterface *dashboard, PedalsSystem *pedals) { + current_state_ = CAR_STATE::STARTUP; buzzer_ = buzzer; drivetrain_ = drivetrain; dashboard_ = dashboard; diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index e6c2e9ce2..37d2b6a0a 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -57,6 +57,7 @@ void MCUStateMachine::tick_state_machine(unsigned long curren // the drivetrain state machine handling if (!drivetrain_->hv_over_threshold_on_drivetrain()) { + hal_println("drivetrain not over thresh in WRTD?"); set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); break; } @@ -72,8 +73,11 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::READY_TO_DRIVE: { auto data = pedals_->getPedalsSystemData(); + + hal_println("RTD"); if (!drivetrain_->hv_over_threshold_on_drivetrain()) { + hal_println("drivetrain not over thresh?"); set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); break; } @@ -116,6 +120,8 @@ template void MCUStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_time) { hal_println("running exit logic"); + + hal_printf("\n to state: %i\n", new_state); handle_exit_logic_(current_state_, curr_time); current_state_ = new_state; @@ -163,12 +169,17 @@ void MCUStateMachine::handle_entry_logic_(CAR_STATE new_state break; } case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: + { // make dashboard sound buzzer buzzer_->activate_buzzer(curr_time); hal_println("RTDS enabled"); break; + } case CAR_STATE::READY_TO_DRIVE: + { + hal_printf("%i\n", curr_time); hal_println("Ready to drive"); break; } + } } diff --git a/lib/systems/src/Buzzer.cpp b/lib/systems/src/Buzzer.cpp index db87ced23..ace9723cd 100644 --- a/lib/systems/src/Buzzer.cpp +++ b/lib/systems/src/Buzzer.cpp @@ -1,5 +1,5 @@ #include "Buzzer.h" - +#include "Logger.h" void BuzzerController::activate_buzzer(unsigned long act_time) { last_activation_time_ = act_time; @@ -8,7 +8,7 @@ void BuzzerController::activate_buzzer(unsigned long act_time) bool BuzzerController::done(unsigned long curr_time){ - buzzer_on_ = ((curr_time - last_activation_time_) < buzzer_period_); + buzzer_on_ = ((curr_time - last_activation_time_) > buzzer_period_); return buzzer_on_; } \ No newline at end of file diff --git a/test/state_machine_test.h b/test/state_machine_test.h index 123906389..4ee1124db 100644 --- a/test/state_machine_test.h +++ b/test/state_machine_test.h @@ -4,62 +4,83 @@ #include #include "MCUStateMachine.h" - // TODO @ben class DrivetrainMock { public: - bool drivetrain_ready_; + bool drivetrain_inited_; bool hv_thresh_; - bool drivetrain_ready(){}; + bool drivetrain_error_; /// @param curr_time current system tick time (millis()) that sets the init phase start time - void enable_drivetrain_hv(unsigned long curr_time){}; - - // startup phase 2 - bool check_drivetrain_quit_dc_on(){}; - // on entry logic - void request_enable(){}; void command_drivetrain_no_torque(){}; - // final check for drivetrain initialization to check if quit inverter on - bool drivetrain_enabled(){}; - + bool handle_inverter_startup() { return drivetrain_inited_; }; // check to see if init time limit has passed bool inverter_init_timeout(unsigned long curr_time){}; bool hv_over_threshold_on_drivetrain() { return hv_thresh_; }; void disable(){}; - bool drivetrain_error_occured(){}; + bool drivetrain_error_occured() { return drivetrain_error_; }; void command_drivetrain(const DrivetrainCommand_s &data){}; }; -BuzzerController buzzer(500); +void handle_startup(MCUStateMachine &state_machine, unsigned long sys_time, DrivetrainMock &drivetrain, PedalsSystem &pedals, DashboardInterface &dash_interface) +{ + // ticking without hv over threshold testing and ensuring the tractive system not active still + auto sys_time2 = sys_time; -DrivetrainMock drivetrain; + drivetrain.hv_thresh_ = true; -PedalsSystem pedals; -DashboardInterface dash_interface; + state_machine.tick_state_machine(sys_time); + // hv going over threshold -> tractive system active + drivetrain.hv_thresh_ = true; + sys_time2 += 1; + state_machine.tick_state_machine(sys_time); + sys_time2 += 1; + dash_interface.start_button_status_ = true; + AnalogConversion_s pedals1_data; + pedals1_data.raw = 0; + pedals1_data.conversion = 0; + pedals1_data.status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD; + auto pedals2_data = pedals1_data; -MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + AnalogConversion_s pedals3_data; + pedals3_data.raw = 3000; + pedals3_data.conversion = 1.0; + pedals3_data.status = pedals1_data.status; + auto pedals4_data = pedals3_data; + pedals.tick(SysTick_s{}, pedals1_data, pedals2_data, pedals3_data, pedals4_data); + // get to enabling inverters + state_machine.tick_state_machine(sys_time); +} TEST(MCUStateMachineTesting, test_state_machine_init_tick) { + BuzzerController buzzer(50); + DrivetrainMock drivetrain; + PedalsSystem pedals; + DashboardInterface dash_interface; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); unsigned long sys_time = 1000; EXPECT_EQ(state_machine.get_state(), CAR_STATE::STARTUP); state_machine.tick_state_machine(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } - - TEST(MCUStateMachineTesting, test_state_machine_tractive_system_activation) { + BuzzerController buzzer(50); + DrivetrainMock drivetrain; + PedalsSystem pedals; + DashboardInterface dash_interface; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + unsigned long sys_time = 1000; - // ticking without hv over threshold testing and ensuring the tractive system not active still + // ticking without hv over threshold testing and ensuring the tractive system not active still state_machine.tick_state_machine(sys_time); sys_time += 1; drivetrain.hv_thresh_ = false; @@ -69,14 +90,13 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_activation) // hv going over threshold -> tractive system drivetrain.hv_thresh_ = true; - sys_time +=1; + sys_time += 1; state_machine.tick_state_machine(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_ACTIVE); - // hv going under thresh -> tractive system not active - drivetrain.hv_thresh_ =false; - sys_time +=1; + drivetrain.hv_thresh_ = false; + sys_time += 1; state_machine.tick_state_machine(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } @@ -85,7 +105,13 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) { unsigned long sys_time = 1000; - // ticking without hv over threshold testing and ensuring the tractive system not active still + BuzzerController buzzer(50); + DrivetrainMock drivetrain; + PedalsSystem pedals; + DashboardInterface dash_interface; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + + // ticking without hv over threshold testing and ensuring the tractive system not active still state_machine.tick_state_machine(sys_time); sys_time += 1; drivetrain.hv_thresh_ = false; @@ -95,12 +121,11 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) // hv going over threshold -> tractive system drivetrain.hv_thresh_ = true; - sys_time +=1; + sys_time += 1; state_machine.tick_state_machine(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_ACTIVE); - - sys_time +=1; + sys_time += 1; dash_interface.start_button_status_ = true; AnalogConversion_s pedals1_data; @@ -111,33 +136,148 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) AnalogConversion_s pedals3_data; pedals3_data.raw = 3000; - pedals3_data.conversion= 1.0; + pedals3_data.conversion = 1.0; pedals3_data.status = pedals1_data.status; auto pedals4_data = pedals3_data; pedals.tick(SysTick_s{}, pedals1_data, pedals2_data, pedals3_data, pedals4_data); state_machine.tick_state_machine(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); - } -TEST(MCUStateMachineTesting, test_state_machine_drivetrain_quit_test) +// test getting into and out of the waiting RTD and ensuring it stays within the state when we want it to +TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert) { + BuzzerController buzzer(50); + DrivetrainMock drivetrain; + PedalsSystem pedals; + DashboardInterface dash_interface; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + + unsigned long sys_time = 1000; + handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); + EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); + + drivetrain.hv_thresh_ = true; + drivetrain.drivetrain_inited_ = true; + state_machine.tick_state_machine(sys_time); + EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); + sys_time += 20; + + state_machine.tick_state_machine(sys_time); + + EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); + sys_time +=35; + + state_machine.tick_state_machine(sys_time); + EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); } -TEST(MCUStateMachineTesting, test_state_machine_drivetrain_enable) +TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) { + BuzzerController buzzer(50); + DrivetrainMock drivetrain; + PedalsSystem pedals; + DashboardInterface dash_interface; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + unsigned long sys_time = 1000; + + drivetrain.hv_thresh_ = true; + handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); + EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); + drivetrain.drivetrain_inited_ = true; + state_machine.tick_state_machine(sys_time); + + buzzer.activate_buzzer(sys_time); + EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); + sys_time+=20; + + drivetrain.hv_thresh_ = false; + state_machine.tick_state_machine(sys_time); + + EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } -TEST(MCUStateMachineTesting, test_state_machine_rtd_sound) + + +// state transitions out of RTD state +TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_active) { + BuzzerController buzzer(50); + DrivetrainMock drivetrain; + drivetrain.drivetrain_error_ = false; + PedalsSystem pedals; + DashboardInterface dash_interface; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + unsigned long sys_time = 1000; + + handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); + EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); + + + sys_time+=70; + drivetrain.drivetrain_inited_ = true; + drivetrain.hv_thresh_ = true; + state_machine.tick_state_machine(sys_time); + EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); + sys_time+=70; + drivetrain.hv_thresh_ = true; + + state_machine.tick_state_machine(sys_time); + + EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); + + state_machine.tick_state_machine(sys_time); + + EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); + + drivetrain.drivetrain_error_ = true; + + state_machine.tick_state_machine(sys_time); + + EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_ACTIVE); } -TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions) +TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_not_active) { + BuzzerController buzzer(50); + DrivetrainMock drivetrain; + drivetrain.drivetrain_error_ = false; + PedalsSystem pedals; + DashboardInterface dash_interface; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + unsigned long sys_time = 1000; + + handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); + EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); + + + sys_time+=70; + drivetrain.drivetrain_inited_ = true; + drivetrain.hv_thresh_ = true; + state_machine.tick_state_machine(sys_time); + EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); + sys_time+=70; + drivetrain.hv_thresh_ = true; + + state_machine.tick_state_machine(sys_time); + + EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); + + state_machine.tick_state_machine(sys_time); + + EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); + + // drivetrain.drivetrain_error_ = true; + + drivetrain.hv_thresh_ = false; + state_machine.tick_state_machine(sys_time); + + EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } + #endif /* STATE_MACHINE_TEST */ From 012ad0186e79448efb6448c860978ad35b7617d0 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 12:01:05 -0500 Subject: [PATCH 03/15] updated inverter interface to allow for turning on inverters via the inverter enable pins --- lib/interfaces/include/InverterInterface.h | 10 +- lib/interfaces/include/InverterInterface.tpp | 4 +- lib/interfaces/include/MCUInterface.h | 59 +- lib/interfaces/src/MCUInterface.cpp | 16 - src/main.cpp | 8 +- src/old_main.cpp | 3126 +++++++++--------- test/drivetrain_system_test.h | 2 + 7 files changed, 1605 insertions(+), 1620 deletions(-) diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index 4b85b5048..ff226d388 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -9,7 +9,9 @@ #include "MC_temps.h" #include "FlexCAN_T4.h" +#include "MCUInterface.h" +#include struct InverterCommand { float torque_setpoint_nm; @@ -19,10 +21,15 @@ template class InverterInterface { public: - InverterInterface(message_queue *msg_output_queue, uint32_t can_id) + InverterInterface(message_queue *msg_output_queue, uint32_t can_id, int inverter_enable_pin, int inverter_24v_enable_pin) { + msg_queue_ = msg_output_queue; can_id_ = can_id; + pin_inv_en_ = inverter_enable_pin; + pin_inv_24V_en_ = inverter_24v_enable_pin; + pinMode(pin_inv_en_, OUTPUT); + pinMode(pin_inv_24V_en_, OUTPUT); } uint32_t get_id() { return can_id_; }; @@ -64,6 +71,7 @@ class InverterInterface } private: + int pin_inv_en_, pin_inv_24V_en_; void write_cmd_msg_to_queue_(const MC_setpoints_command &msg); int16_t speed_; uint16_t dc_bus_voltage_; diff --git a/lib/interfaces/include/InverterInterface.tpp b/lib/interfaces/include/InverterInterface.tpp index b7a5af48c..f27ba40e8 100644 --- a/lib/interfaces/include/InverterInterface.tpp +++ b/lib/interfaces/include/InverterInterface.tpp @@ -15,7 +15,8 @@ void InverterInterface::write_cmd_msg_to_queue_(const MC_setpoint template void InverterInterface::request_enable_hv() { - + digitalWrite(pin_inv_en_, HIGH); + digitalWrite(pin_inv_24V_en_, HIGH); MC_setpoints_command mc_setpoints_command{}; mc_setpoints_command.set_hv_enable(true); write_cmd_msg_to_queue_(mc_setpoints_command); @@ -24,6 +25,7 @@ void InverterInterface::request_enable_hv() template void InverterInterface::request_enable_inverter() { + MC_setpoints_command mc_setpoints_command{}; mc_setpoints_command.set_speed_setpoint(0); mc_setpoints_command.set_pos_torque_limit(0); diff --git a/lib/interfaces/include/MCUInterface.h b/lib/interfaces/include/MCUInterface.h index 5b976f578..1263d2b10 100644 --- a/lib/interfaces/include/MCUInterface.h +++ b/lib/interfaces/include/MCUInterface.h @@ -9,18 +9,24 @@ #include "SysClock.h" // #include "MCUStateMachine.h" -const int DEFAULT_BMS_OK_READ = 5; // SHDN_D_READ -const int DEFAULT_IMD_OK_READ = 4; // SHDN_C_READ -const int DEFAULT_BSPD_OK_READ = 6; // SHDN_E_READ -const int DEFAULT_SOFTWARE_OK_READ = 25; // SHDN_F_READ Watchdog Combined -const int DEFAULT_BOTS_OK_READ = 3; // SHDN_B_READ -const int DEFAULT_BRAKE_LIGHT_CTRL = 7; -const int DEFAULT_INVERTER_EN = 9; -const int DEFAULT_INVERTER_24V_EN = 8; +const int DEFAULT_BMS_OK_READ = 5; // SHDN_D_READ +const int DEFAULT_IMD_OK_READ = 4; // SHDN_C_READ +const int DEFAULT_BSPD_OK_READ = 6; // SHDN_E_READ +const int DEFAULT_SOFTWARE_OK_READ = 25; // SHDN_F_READ Watchdog Combined +const int DEFAULT_BOTS_OK_READ = 3; // SHDN_B_READ +const int DEFAULT_BRAKE_LIGHT_CTRL = 7; class MCUInterface { private: + + CANBufferType *msg_queue_; + int pin_bms_ok_read_; + int pin_imd_ok_read_; + int pin_bspd_ok_read_; + int pin_software_ok_read_; + int pin_bots_ok_read_; + int pin_brake_light_ctrl_; /* Private utility functions */ void measure_shutdown_circuit_input(); void measure_shutdown_circuit_voltage(); @@ -29,7 +35,6 @@ class MCUInterface /* Outbound CAN message */ MCU_status mcu_status_; /* CAN Tx buffer */ - CANBufferType *msg_queue_; /* Shutdown circuit input */ bool bms_ok_high; @@ -43,34 +48,21 @@ class MCUInterface bool shutdown_e_above_threshold; /* Hardware interface pins */ - int pin_bms_ok_read_; - int pin_imd_ok_read_; - int pin_bspd_ok_read_; - int pin_software_ok_read_; - int pin_bots_ok_read_; - int pin_brake_light_ctrl_; - int pin_inv_en_; - int pin_inv_24V_en_; + public: - MCUInterface(CANBufferType *msg_output_queue, int bms_pin, int imd_pin, int bspd_pin, int sw_ok_pin, int bots_ok_pin, int brake_light_pin, int inv_pin, int inv_24V_pin): - msg_queue_(msg_output_queue), - pin_bms_ok_read_(bms_pin), - pin_imd_ok_read_(imd_pin), - pin_bspd_ok_read_(bspd_pin), - pin_software_ok_read_(sw_ok_pin), - pin_bots_ok_read_(bots_ok_pin), - pin_brake_light_ctrl_(brake_light_pin), - pin_inv_en_(inv_pin), - pin_inv_24V_en_(inv_24V_pin) + MCUInterface(CANBufferType *msg_output_queue, int bms_pin, int imd_pin, int bspd_pin, int sw_ok_pin, int bots_ok_pin, int brake_light_pin): msg_queue_(msg_output_queue), + pin_bms_ok_read_(bms_pin), + pin_imd_ok_read_(imd_pin), + pin_bspd_ok_read_(bspd_pin), + pin_software_ok_read_(sw_ok_pin), + pin_bots_ok_read_(bots_ok_pin), + pin_brake_light_ctrl_(brake_light_pin) { // Set pin mode pinMode(pin_brake_light_ctrl_, OUTPUT); - pinMode(pin_inv_en_, OUTPUT); - pinMode(pin_inv_24V_en_, OUTPUT); }; - MCUInterface(CANBufferType *msg_output_queue): - MCUInterface(msg_output_queue, DEFAULT_BMS_OK_READ, DEFAULT_IMD_OK_READ, DEFAULT_BSPD_OK_READ, DEFAULT_SOFTWARE_OK_READ, DEFAULT_BOTS_OK_READ, DEFAULT_BRAKE_LIGHT_CTRL, DEFAULT_INVERTER_EN, DEFAULT_INVERTER_24V_EN) {}; + MCUInterface(CANBufferType *msg_output_queue) : MCUInterface(msg_output_queue, DEFAULT_BMS_OK_READ, DEFAULT_IMD_OK_READ, DEFAULT_BSPD_OK_READ, DEFAULT_SOFTWARE_OK_READ, DEFAULT_BOTS_OK_READ, DEFAULT_BRAKE_LIGHT_CTRL){}; /* Initialize shutdown circuit input readings */ void init(); @@ -78,9 +70,7 @@ class MCUInterface /* Read from Main ECU */ void read_mcu_status(); /* Write to Main ECU */ - void set_inverter_enable(bool enable); // Called from DrivetrainSystem - void set_inverter_24V_enable(bool enable_24V); // Called from DrivetrainSystem - void set_brake_light(bool brake_pedal_is_active); // Called from PedalInterface/System + void set_brake_light(bool brake_pedal_is_active); // Called from PedalInterface/System /* Feed to state machine */ bool bms_ok_is_high(); @@ -114,7 +104,6 @@ class MCUInterface // bool pack_charge_is_critical, // bool button_is_pressed // ); - }; #endif /* __MCU_INTERFACE_H__ */ diff --git a/lib/interfaces/src/MCUInterface.cpp b/lib/interfaces/src/MCUInterface.cpp index a291f2206..13131f969 100644 --- a/lib/interfaces/src/MCUInterface.cpp +++ b/lib/interfaces/src/MCUInterface.cpp @@ -11,8 +11,6 @@ void MCUInterface::init() { // Enable inverters (?) // Should be called from drivetrain - digitalWrite(pin_inv_en_, HIGH); - digitalWrite(pin_inv_24V_en_, HIGH); } /* Read shutdown system values */ @@ -43,20 +41,6 @@ void MCUInterface::measure_shutdown_circuit_voltage() { } -/* Write inverter enable */ -void MCUInterface::set_inverter_enable(bool enable) { - - digitalWrite(pin_inv_en_, enable); - -} - -/* Write inverter 24V enable */ -void MCUInterface::set_inverter_24V_enable(bool enable_24V) { - - digitalWrite(pin_inv_24V_en_, enable_24V); - -} - /* Write brake light */ void MCUInterface::set_brake_light(bool brake_pedal_is_active) { diff --git a/src/main.cpp b/src/main.cpp index 48d7d9edf..acf431905 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -51,10 +51,10 @@ MCUInterface main_ecu(&CAN3_txBuffer); TelemetryInterface telem_interface(&CAN3_txBuffer); using InverterInterfaceType = InverterInterface; -InverterInterfaceType fl_inv(&CAN2_txBuffer, ID_MC1_SETPOINTS_COMMAND); -InverterInterfaceType fr_inv(&CAN2_txBuffer, ID_MC2_SETPOINTS_COMMAND); -InverterInterfaceType rl_inv(&CAN2_txBuffer, ID_MC3_SETPOINTS_COMMAND); -InverterInterfaceType rr_inv(&CAN2_txBuffer, ID_MC4_SETPOINTS_COMMAND); +InverterInterfaceType fl_inv(&CAN2_txBuffer, ID_MC1_SETPOINTS_COMMAND, 9, 8); +InverterInterfaceType fr_inv(&CAN2_txBuffer, ID_MC2_SETPOINTS_COMMAND, 9, 8); +InverterInterfaceType rl_inv(&CAN2_txBuffer, ID_MC3_SETPOINTS_COMMAND, 9, 8); +InverterInterfaceType rr_inv(&CAN2_txBuffer, ID_MC4_SETPOINTS_COMMAND, 9, 8); /* Declare systems */ diff --git a/src/old_main.cpp b/src/old_main.cpp index 28c26eee0..8f74d7f3b 100644 --- a/src/old_main.cpp +++ b/src/old_main.cpp @@ -1,1576 +1,1576 @@ -// /* -// Teensy 4.1 Main Control Unit code -// Written by Liwei Sun which is why the code is so bad - -// Rev 12 -// */ -// #include "Logger.h" - -// #include -// #include -// #include -// #include -// #define _USE_MATH_DEFINES -// #include -// #include -// #include - -// #include "ADC_SPI.h" -// #include "STEERING_SPI.h" -// #include "kinetis_flexcan.h" -// #include "Metro.h" - -// // IMU -// #include - -// #include "drivers.h" - -// // constants to define for different operation - -// #define DRIVER DEFAULT_DRIVER -// #define TORQUE_1 10 -// #define TORQUE_2 15 -// #define TORQUE_3 21 -// #define MAX_ALLOWED_SPEED 20000 - -// // set to true or false for debugging -// #define DEBUG false -// #define BMS_DEBUG_ENABLE false - -// #include "MCU_rev12_dfs.h" - -// #include "driver_constants.h" +/* + Teensy 4.1 Main Control Unit code + Written by Liwei Sun which is why the code is so bad + + Rev 12 +*/ +#include "Logger.h" + +#include +#include +#include +#include +#define _USE_MATH_DEFINES +#include +#include +#include + +#include "ADC_SPI.h" +#include "STEERING_SPI.h" +#include "kinetis_flexcan.h" +#include "Metro.h" + +// IMU +#include + +#include "drivers.h" + +// constants to define for different operation + +#define DRIVER DEFAULT_DRIVER +#define TORQUE_1 10 +#define TORQUE_2 15 +#define TORQUE_3 21 +#define MAX_ALLOWED_SPEED 20000 + +// set to true or false for debugging +#define DEBUG false +#define BMS_DEBUG_ENABLE false + +#include "MCU_rev12_dfs.h" + +#include "driver_constants.h" -// // Call the ADIS16460 IMU class -// ADIS16460 IMU(IMU_CS, IMU_DATAREADY, IMU_RESET); // Chip Select, Data Ready, Reset Pin Assignments - -// // Outbound CAN message/* */s -// MCU_pedal_readings mcu_pedal_readings; -// MCU_status mcu_status{}; -// MCU_load_cells mcu_load_cells{}; -// MCU_front_potentiometers mcu_front_potentiometers; -// MCU_rear_potentiometers mcu_rear_potentiometers; -// MCU_analog_readings mcu_analog_readings{}; - -// // IMU -// IMU_accelerometer imu_accelerometer; -// IMU_gyroscope imu_gyroscope; -// double imu_velocity; -// double pitch_calibration_angle; - -// MC_status mc_status[4]; -// MC_temps mc_temps[4]; -// MC_energy mc_energy[4]; -// MC_setpoints_command mc_setpoints_command[4]; - -// // Inbound CAN messages -// BMS_coulomb_counts bms_coulomb_counts{}; -// BMS_status bms_status{}; -// BMS_temperatures bms_temperatures{}; -// BMS_voltages bms_voltages{}; -// Dashboard_status dashboard_status{}; - -// //Timers -// Metro timer_imu_integration = Metro(50); -// Metro timer_CAN_imu_accelerometer_send = Metro(50); -// Metro timer_CAN_imu_gyroscope_send = Metro(50); -// Metro timer_CAN_inverter_setpoints_send = Metro(20); -// Metro timer_CAN_coloumb_count_send = Metro(1000); -// Metro timer_CAN_mcu_status_send = Metro(100); -// Metro timer_CAN_mcu_pedal_readings_send = Metro(50); -// Metro timer_CAN_mcu_analog_readings_send = Metro(50); -// Metro timer_CAN_mcu_load_cells_send = Metro(20); -// Metro timer_CAN_mcu_potentiometers_send = Metro(20); - -// Metro timer_ready_sound = Metro(2000); // Time to play RTD sound - -// Metro timer_read_all_adcs = Metro(20); -// Metro timer_steering_spi_read = Metro(1000); -// Metro timer_read_imu = Metro(20); - -// Metro timer_inverter_enable = Metro(5000); -// Metro timer_reset_inverter = Metro(5000); -// Metro timer_watchdog_timer = Metro(7); - -// elapsedMillis pedal_implausability_duration; - -// Metro timer_debug = Metro(200); -// Metro timer_debug2 = Metro(1000); - -// // this abuses Metro timer functionality to stay faulting once a fault has occurred -// // autoreset makes the timer update to the current time and not by the interval -// // this allows me to set the interval as 0 once a fault has occurred, leading to continuous faulting -// // until a CAN message comes in which resets the timer and the interval -// Metro timer_bms_heartbeat = Metro(0, 1); - - -// bool imd_faulting = false; -// bool inverter_restart = false; // True when restarting the inverter -// INVERTER_STARTUP_STATE inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; - -// ADC_SPI ADC1(ADC1_CS, ADC_SPI_SPEED, ECU_SDI, ECU_SDO, ECU_CLK); -// ADC_SPI ADC2(ADC2_CS, ADC_SPI_SPEED, ECU_SDI, ECU_SDO, ECU_CLK); -// ADC_SPI ADC3(ADC3_CS, ADC_SPI_SPEED, ECU_SDI, ECU_SDO, ECU_CLK); - -// STEERING_SPI STEERING(STEERING_CS, STEERING_SPI_SPEED); - -// FlexCAN_T4 FRONT_INV_CAN; -// FlexCAN_T4 REAR_INV_CAN; -// FlexCAN_T4 TELEM_CAN; -// CAN_message_t msg; - -// // coloumb counts -// uint32_t total_discharge; -// unsigned long previous_data_time; - -// int16_t torque_setpoint_array[4]; -// int16_t speed_setpoint_array[4] = {0, 0, 0, 0}; - -// uint16_t prev_load_cell_readings[4] = {0, 0, 0, 0}; -// float load_cell_alpha = 0.95; - -// float filtered_min_cell_voltage = 3.5; -// float cell_voltage_alpha = 0.8; - -// float filtered_max_cell_temp = 40.0; -// float cell_temp_alpha = 0.8; - -// uint16_t current_read = 0; -// uint16_t reference_read = 0; - -// float max_front_power = 0.0; -// float max_rear_power = 0.0; - -// enum launch_states {launch_not_ready, launch_ready, launching}; -// launch_states launch_state = launch_not_ready; -// int16_t launch_speed_target = 0; -// elapsedMillis time_since_launch; -// const uint16_t LAUNCH_READY_ACCEL_THRESHOLD = 100; -// const uint16_t LAUNCH_READY_BRAKE_THRESHOLD = 300; -// const int16_t LAUNCH_READY_SPEED_THRESHOLD = 500; -// const uint16_t LAUNCH_GO_THRESHOLD = 900; -// const uint16_t LAUNCH_STOP_THRESHOLD = 600; -// float launch_rate_target = 0.0; - -// inline void set_all_inverters_dc_on(bool input); -// inline void calculate_pedal_implausibilities(); - -// inline float max_allowed_torque(float maxwatts, float rpm); -// inline void set_inverter_torques_regen_only(); -// inline void set_all_inverters_disabled(); -// inline void set_all_inverters_no_torque(); -// bool check_all_inverters_quit_dc_on(); -// inline void check_TS_active(); -// bool check_all_inverters_system_ready(); - -// inline void set_inverter_torques(); -// uint8_t check_all_inverters_error(); -// bool check_all_inverters_quit_inverter_on(); -// inline void set_all_inverters_inverter_enable(bool input); -// inline void set_all_inverters_no_torque(); - -// inline void set_all_inverters_driver_enable(bool input); - -// bool check_TS_over_HV_threshold(); - - -// void set_state(MCU_STATE new_state); - -// inline void send_CAN_inverter_setpoints() { -// if (timer_CAN_inverter_setpoints_send.check()) { -// mc_setpoints_command[0].write(msg.buf); -// msg.id = ID_MC1_SETPOINTS_COMMAND; -// msg.len = sizeof(mc_setpoints_command[0]); -// FRONT_INV_CAN.write(msg); - -// mc_setpoints_command[1].write(msg.buf); -// msg.id = ID_MC2_SETPOINTS_COMMAND; -// msg.len = sizeof(mc_setpoints_command[1]); -// FRONT_INV_CAN.write(msg); - -// mc_setpoints_command[2].write(msg.buf); -// msg.id = ID_MC3_SETPOINTS_COMMAND; -// msg.len = sizeof(mc_setpoints_command[2]); -// REAR_INV_CAN.write(msg); - -// mc_setpoints_command[3].write(msg.buf); -// msg.id = ID_MC4_SETPOINTS_COMMAND; -// msg.len = sizeof(mc_setpoints_command[3]); -// REAR_INV_CAN.write(msg); -// } -// } - -// inline void send_CAN_mcu_status() { -// if (timer_CAN_mcu_status_send.check()) { -// // Send Main Control Unit status message -// mcu_status.write(msg.buf); -// msg.id = ID_MCU_STATUS; -// msg.len = sizeof(mcu_status); -// TELEM_CAN.write(msg); -// } -// } - -// inline void send_CAN_mcu_pedal_readings() { -// if (timer_CAN_mcu_pedal_readings_send.check()) { -// mcu_pedal_readings.write(msg.buf); -// msg.id = ID_MCU_PEDAL_READINGS; -// msg.len = sizeof(mcu_pedal_readings); -// TELEM_CAN.write(msg); -// } -// } - -// inline void send_CAN_mcu_load_cells() { -// if (timer_CAN_mcu_load_cells_send.check()) { -// mcu_load_cells.write(msg.buf); -// msg.id = ID_MCU_LOAD_CELLS; -// msg.len = sizeof(mcu_load_cells); -// TELEM_CAN.write(msg); -// } -// } - -// inline void send_CAN_mcu_potentiometers() { -// if (timer_CAN_mcu_potentiometers_send.check()) { -// mcu_front_potentiometers.write(msg.buf); -// msg.id = ID_MCU_FRONT_POTS; -// msg.len = sizeof(mcu_front_potentiometers); -// TELEM_CAN.write(msg); -// mcu_rear_potentiometers.write(msg.buf); -// msg.id = ID_MCU_REAR_POTS; -// msg.len = sizeof(mcu_rear_potentiometers); -// TELEM_CAN.write(msg); -// } -// } - -// inline void send_CAN_bms_coulomb_counts() { -// if (timer_CAN_coloumb_count_send.check()) { -// bms_coulomb_counts.write(msg.buf); -// msg.id = ID_BMS_COULOMB_COUNTS; -// msg.len = sizeof(bms_coulomb_counts); -// TELEM_CAN.write(msg); -// } -// } - -// inline void send_CAN_mcu_analog_readings() { -// if (timer_CAN_mcu_analog_readings_send.check()) { -// mcu_analog_readings.write(msg.buf); -// msg.id = ID_MCU_ANALOG_READINGS; -// msg.len = sizeof(mcu_analog_readings); -// TELEM_CAN.write(msg); -// } -// } -// inline void state_machine() { -// switch (mcu_status.get_state()) { -// case MCU_STATE::STARTUP: break; - -// case MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: -// #if DEBUG -// Serial.println("TS NOT ACTIVE"); -// #endif -// // if TS is above HV threshold, move to Tractive System Active -// if (check_TS_over_HV_threshold()) { -// #if DEBUG -// Serial.println("Setting state to TS Active from TS Not Active"); -// #endif -// set_state(MCU_STATE::TRACTIVE_SYSTEM_ACTIVE); -// } -// break; - -// case MCU_STATE::TRACTIVE_SYSTEM_ACTIVE: -// check_TS_active(); -// if (check_all_inverters_system_ready()) { -// set_all_inverters_dc_on(true); -// inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_QUIT_DC_ON; -// } -// if (dashboard_status.get_start_btn() && mcu_status.get_mech_brake_active()) { -// #if DEBUG -// Serial.println("Setting state to Enabling Inverter"); -// #endif -// set_state(MCU_STATE::ENABLING_INVERTER); -// } -// break; - -// case MCU_STATE::ENABLING_INVERTER: -// check_TS_active(); -// // inverter enabling timed out -// if (timer_inverter_enable.check()) { -// #if DEBUG -// Serial.println("Setting state to TS Active from Enabling Inverter"); -// #endif -// set_state(MCU_STATE::TRACTIVE_SYSTEM_ACTIVE); -// } - -// switch (inverter_startup_state) { -// case INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY: -// if (check_all_inverters_system_ready()) { -// set_all_inverters_dc_on(true); -// inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_QUIT_DC_ON; -// } -// break; - -// case INVERTER_STARTUP_STATE::WAIT_QUIT_DC_ON: -// if (check_all_inverters_quit_dc_on()) { -// set_all_inverters_no_torque(); -// set_all_inverters_driver_enable(true); -// set_all_inverters_inverter_enable(true); -// inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_QUIT_INVERTER_ON; -// } -// break; - -// case INVERTER_STARTUP_STATE::WAIT_QUIT_INVERTER_ON: -// if (check_all_inverters_quit_inverter_on()) { -// #if DEBUG -// Serial.println("Setting state to Waiting Ready to Drive Sound"); -// #endif -// set_state(MCU_STATE::WAITING_READY_TO_DRIVE_SOUND); -// } -// break; - -// } -// break; - - -// case MCU_STATE::WAITING_READY_TO_DRIVE_SOUND: -// check_TS_active(); - -// // if the ready to drive sound has been playing for long enough, move to ready to drive mode -// if (timer_ready_sound.check()) { -// #if DEBUG -// Serial.println("Setting state to Ready to Drive"); -// #endif -// set_state(MCU_STATE::READY_TO_DRIVE); -// } -// break; - -// case MCU_STATE::READY_TO_DRIVE: -// check_TS_active(); - -// if (check_all_inverters_error()) { -// set_all_inverters_disabled(); -// set_state(MCU_STATE::TRACTIVE_SYSTEM_ACTIVE); -// } - -// calculate_pedal_implausibilities(); - -// if ( -// pedal_implausability_duration <= 100 && -// mcu_status.get_bms_ok_high() && -// mcu_status.get_imd_ok_high() - -// ) { -// set_inverter_torques(); -// } else if (mcu_status.get_bms_ok_high() && mcu_status.get_imd_ok_high()) { -// set_inverter_torques_regen_only(); -// } else { -// Serial.println("not calculating torque"); -// Serial.printf("no brake implausibility: %d\n", mcu_status.get_no_brake_implausability()); -// Serial.printf("no accel implausibility: %d\n", mcu_status.get_no_accel_implausability()); -// Serial.printf("no accel brake implausibility: %d\n", mcu_status.get_no_accel_brake_implausability()); -// Serial.printf("software is ok: %d\n", mcu_status.get_software_is_ok()); -// Serial.printf("get bms ok high: %d\n", mcu_status.get_bms_ok_high()); -// Serial.printf("get imd ok high: %d\n", mcu_status.get_imd_ok_high()); -// set_all_inverters_no_torque(); -// } - -// break; -// } -// } - -// /* Shared state functinality */ - - -// bool check_TS_over_HV_threshold() { -// for (uint8_t inv = 0; inv < 4; inv++) { -// if (mc_energy[inv].get_dc_bus_voltage() < MIN_HV_VOLTAGE) { -// return false; -// } -// } -// return true; -// } - -// // if TS is below HV threshold, return to Tractive System Not Active -// inline void check_TS_active() { -// if (!check_TS_over_HV_threshold()) { -// #if DEBUG -// Serial.println("Setting state to TS Not Active, because TS is below HV threshold"); -// #endif -// set_state(MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); -// inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; -// } -// } - - -// /* Implementation of software shutdown */ -// inline void software_shutdown() { - -// mcu_status.set_software_is_ok(true); - -// // check inputs -// // BMS heartbeat has not arrived within time interval -// if (timer_bms_heartbeat.check()) { -// timer_bms_heartbeat.interval(0); -// #if DEBUG -// Serial.println("no bms heartbeat"); -// #endif -// mcu_status.set_software_is_ok(false); -// } - -// // add BMS software checks -// // software ok/not ok action -// if (mcu_status.get_software_is_ok()) { -// digitalWrite(SOFTWARE_OK, HIGH); //eventually make this HIGH only if software is ok -// } else { -// digitalWrite(SOFTWARE_OK, LOW); -// } -// /* Watchdog timer */ -// if (timer_watchdog_timer.check()) { -// static bool watchdog_state = HIGH; -// watchdog_state = !watchdog_state; -// digitalWrite(WATCHDOG_INPUT, watchdog_state); -// } - -// } - -// /* Parse incoming CAN messages */ -// void parse_telem_can_message(const CAN_message_t &RX_msg) { -// CAN_message_t rx_msg = RX_msg; -// switch (rx_msg.id) { -// case ID_BMS_TEMPERATURES: -// bms_temperatures.load(rx_msg.buf); -// filtered_max_cell_temp = filtered_max_cell_temp * cell_temp_alpha + (1.0 - cell_temp_alpha) * (bms_temperatures.get_high_temperature() / 100.0) ; -// break; -// case ID_BMS_VOLTAGES: -// bms_voltages.load(rx_msg.buf); -// if (bms_voltages.get_low() < PACK_CHARGE_CRIT_LOWEST_CELL_THRESHOLD || bms_voltages.get_total() < PACK_CHARGE_CRIT_TOTAL_THRESHOLD) { -// mcu_status.set_pack_charge_critical(true); -// } else mcu_status.set_pack_charge_critical(false); -// filtered_min_cell_voltage = filtered_min_cell_voltage * cell_voltage_alpha + (1.0 - cell_voltage_alpha) * (bms_voltages.get_low() / 10000.0); -// break; -// case ID_BMS_COULOMB_COUNTS: bms_coulomb_counts.load(rx_msg.buf); break; -// case ID_BMS_STATUS: -// bms_status.load(rx_msg.buf); -// // BMS heartbeat timer -// timer_bms_heartbeat.reset(); -// timer_bms_heartbeat.interval(BMS_HEARTBEAT_TIMEOUT); -// break; -// case ID_DASHBOARD_STATUS: -// dashboard_status.load(rx_msg.buf); -// /* process dashboard buttons */ -// if (dashboard_status.get_torque_mode_btn()) { -// switch (mcu_status.get_torque_mode()) { -// case 1: -// mcu_status.set_max_torque(TORQUE_2); -// mcu_status.set_torque_mode(2); break; -// case 2: -// mcu_status.set_max_torque(TORQUE_3); -// mcu_status.set_torque_mode(3); break; -// case 3: -// mcu_status.set_max_torque(TORQUE_1); -// mcu_status.set_torque_mode(1); break; -// } -// } -// if (dashboard_status.get_launch_ctrl_btn()) { -// mcu_status.toggle_launch_ctrl_active(); -// } -// if (dashboard_status.get_mc_cycle_btn()) { -// inverter_restart = true; -// timer_reset_inverter.reset(); -// } -// // eliminate all action buttons to not process twice -// dashboard_status.set_button_flags(0); -// break; -// } -// } - -// void parse_front_inv_can_message(const CAN_message_t &RX_msg) { -// CAN_message_t rx_msg = RX_msg; -// switch (rx_msg.id) { -// case ID_MC1_STATUS: mc_status[0].load(rx_msg.buf); break; -// case ID_MC2_STATUS: mc_status[1].load(rx_msg.buf); break; -// case ID_MC1_TEMPS: mc_temps[0].load(rx_msg.buf); break; -// case ID_MC2_TEMPS: mc_temps[1].load(rx_msg.buf); break; -// case ID_MC1_ENERGY: mc_energy[0].load(rx_msg.buf); break; -// case ID_MC2_ENERGY: mc_energy[1].load(rx_msg.buf); break; -// } -// } - -// void parse_rear_inv_can_message(const CAN_message_t &RX_msg) { -// CAN_message_t rx_msg = RX_msg; - -// switch (rx_msg.id) { -// case ID_MC3_STATUS: mc_status[2].load(rx_msg.buf); break; -// case ID_MC4_STATUS: mc_status[3].load(rx_msg.buf); break; -// case ID_MC3_TEMPS: mc_temps[2].load(rx_msg.buf); break; -// case ID_MC4_TEMPS: mc_temps[3].load(rx_msg.buf); break; -// case ID_MC3_ENERGY: mc_energy[2].load(rx_msg.buf); break; -// case ID_MC4_ENERGY: mc_energy[3].load(rx_msg.buf); break; -// } -// } -// //FIXME -// inline void power_off_inverter() { -// digitalWrite(INVERTER_24V_EN, LOW); - -// #if DEBUG -// Serial.println("INVERTER POWER OFF"); -// #endif -// } - - -// /* Handle changes in state */ -// void set_state(MCU_STATE new_state) { -// if (mcu_status.get_state() == new_state) { -// return; -// } - -// // exit logic -// switch (mcu_status.get_state()) { -// case MCU_STATE::STARTUP: break; -// case MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: break; -// case MCU_STATE::TRACTIVE_SYSTEM_ACTIVE: break; -// case MCU_STATE::ENABLING_INVERTER: -// timer_inverter_enable.reset(); -// break; -// case MCU_STATE::WAITING_READY_TO_DRIVE_SOUND: -// // make dashboard stop buzzer -// mcu_status.set_activate_buzzer(false); -// mcu_status.write(msg.buf); -// msg.id = ID_MCU_STATUS; -// msg.len = sizeof(mcu_status); -// TELEM_CAN.write(msg); -// break; -// case MCU_STATE::READY_TO_DRIVE: { -// inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; -// break; -// } -// } - -// mcu_status.set_state(new_state); - -// // entry logic -// switch (new_state) { -// case MCU_STATE::STARTUP: break; -// case MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: break; -// case MCU_STATE::TRACTIVE_SYSTEM_ACTIVE: { -// inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; -// break; -// } -// case MCU_STATE::ENABLING_INVERTER: { -// Serial.println("MCU Sent enable command"); -// timer_inverter_enable.reset(); -// break; -// } -// case MCU_STATE::WAITING_READY_TO_DRIVE_SOUND: -// // make dashboard sound buzzer -// mcu_status.set_activate_buzzer(true); -// mcu_status.write(msg.buf); -// msg.id = ID_MCU_STATUS; -// msg.len = sizeof(mcu_status); -// TELEM_CAN.write(msg); - -// timer_ready_sound.reset(); -// Serial.println("RTDS enabled"); -// break; -// case MCU_STATE::READY_TO_DRIVE: -// Serial.println("Ready to drive"); -// break; -// } -// } - -// inline float float_map(float x, float in_min, float in_max, float out_min, float out_max) -// { -// return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -// } - -// inline void set_inverter_torques_regen_only() { -// int brake1 = map(round(mcu_pedal_readings.get_brake_pedal_1()), START_BRAKE_PEDAL_1, END_BRAKE_PEDAL_1, 0, 2140); -// int brake2 = map(round(mcu_pedal_readings.get_brake_pedal_2()), START_BRAKE_PEDAL_2, END_BRAKE_PEDAL_2, 0, 2140); -// int avg_brake = (brake1 + brake2) / 2; -// if (avg_brake > 1500) { -// avg_brake = 1500; -// } -// if (avg_brake < 0) { -// avg_brake = 0; -// } -// torque_setpoint_array[0] = - avg_brake; -// torque_setpoint_array[1] = - avg_brake; -// torque_setpoint_array[2] = - avg_brake; -// torque_setpoint_array[3] = - avg_brake; -// for (int i = 0; i < 4; i++) { -// if (i < 2) { -// torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * 1.33); -// } else { -// torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * 0.66); -// } - -// } -// for (int i = 0; i < 4; i++) { -// if (torque_setpoint_array[i] >= 0) { -// } -// else { -// int16_t max_speed_regen = 0; -// for (int i = 0; i < sizeof(torque_setpoint_array); i++) { - -// max_speed_regen = (max_speed_regen < mc_status[i].get_speed()) ? mc_status[i].get_speed() : max_speed_regen; - -// } -// float scale_down = 1; -// if (max_speed_regen < 770) { -// scale_down = 0; -// } else if (max_speed_regen > REGEN_OFF_START_THRESHOLD) { -// scale_down = 1; -// } else { -// scale_down = map(max_speed_regen, 770, REGEN_OFF_START_THRESHOLD, 0, 1); -// } -// mc_setpoints_command[i].set_speed_setpoint(0); -// mc_setpoints_command[i].set_pos_torque_limit(0); -// mc_setpoints_command[i].set_neg_torque_limit(max(((int16_t)(torque_setpoint_array[i]) * scale_down) , -2140) ); - -// } -// } -// } - -// inline void set_inverter_torques() { - -// float max_torque = mcu_status.get_max_torque() / 0.0098; // max possible value for torque multiplier, unit in 0.1% nominal torque -// int accel1 = map(round(mcu_pedal_readings.get_accelerator_pedal_1()), START_ACCELERATOR_PEDAL_1, END_ACCELERATOR_PEDAL_1, 0, 2140); -// int accel2 = map(round(mcu_pedal_readings.get_accelerator_pedal_2()), START_ACCELERATOR_PEDAL_2, END_ACCELERATOR_PEDAL_2, 0, 2140); - -// int brake1 = map(round(mcu_pedal_readings.get_brake_pedal_1()), START_BRAKE_PEDAL_1, END_BRAKE_PEDAL_1, 0, 2140); -// int brake2 = map(round(mcu_pedal_readings.get_brake_pedal_2()), START_BRAKE_PEDAL_2, END_BRAKE_PEDAL_2, 0, 2140); - - -// // torque values are greater than the max possible value, set them to max -// if (accel1 > max_torque) { -// accel1 = max_torque; -// } -// if (accel2 > max_torque) { -// accel2 = max_torque; -// } -// int avg_accel = (accel1 + accel2) / 2; -// int avg_brake = (brake1 + brake2) / 2; -// if (avg_accel > max_torque) { -// avg_accel = max_torque; -// } -// if (avg_accel < 0) { -// avg_accel = 0; -// } -// if (avg_accel > max_torque) { -// avg_accel = max_torque; -// } -// if (avg_accel < 0) { -// avg_accel = 0; -// } -// if (avg_brake > 1500) { -// avg_brake = 1500; -// } -// if (avg_brake < 0) { -// avg_brake = 0; -// } - -// float rear_power_balance = 0.66; -// float front_power_balance = 1.0 - rear_power_balance; -// float rear_brake_balance = 0.1; -// float front_brake_balance = 1.0 - rear_brake_balance; - -// int32_t total_torque = 0; -// int32_t total_load_cells = 0; -// float front_load_total; -// float rear_load_total; - -// float attesa_def_split = 0.85; -// float attesa_alt_split = 0.5; -// float fr_slip_clamped; -// float fr_slip_factor = 2.5; // Factor of 5 causes 50/50 split at 20% r/f slip. Lower values allow more slip -// float f_torque; -// float r_torque; -// float rear_lr_slip_clamped; -// float lsd_right_split; // Fraction of rear axle torque going to rear right wheel -// float lsd_slip_factor = 0.5; - -// float avg_speed = 0.0; -// for (int i = 0; i < 4; i++) -// avg_speed += ((float) mc_status[i].get_speed()) / 4.0; -// int16_t start_derating_rpm = 2000; -// int16_t end_derating_rpm = 20000; - -// const float hairpin_rpm_limit = 5600.0; -// const float hairpin_rpm_full = 2800.0; -// float hairpin_rpm_factor = 0.0; -// const float hairpin_steering_min = 80.0; // degrees -// const float hairpin_steering_max = 120.0; // degrees -// float steering_calibration_slope = -0.111; -// float steering_calibration_offset = 260.0; -// // positive steering angle is to the left -// float steering_angle = mcu_analog_readings.get_steering_2() * steering_calibration_slope + steering_calibration_offset; -// float hairpin_reallocation = 0.0; -// float hairpin_steering_factor = 0.0; - -// int16_t max_speed; - -// switch (dashboard_status.get_dial_state()) { -// case 0: -// for (int i = 0; i < 4; i++) { -// speed_setpoint_array[i] = MAX_ALLOWED_SPEED; -// } -// launch_state = launch_not_ready; -// // standard no torque vectoring - -// max_front_power = 19000.0; -// max_rear_power = 36000.0; - -// torque_setpoint_array[0] = avg_accel - avg_brake; -// torque_setpoint_array[1] = avg_accel - avg_brake; -// torque_setpoint_array[2] = avg_accel - avg_brake; -// torque_setpoint_array[3] = avg_accel - avg_brake; - -// for (int i = 0; i < 4; i++) { -// if (torque_setpoint_array[i] >= 0) { -// if (i < 2) { -// torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * front_power_balance); -// } else { -// torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * rear_power_balance); -// } -// } else { -// if (i < 2) { -// torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * front_brake_balance); -// } else { -// torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * rear_brake_balance); -// } -// } -// } -// break; -// case 1: -// for (int i = 0; i < 4; i++) { -// speed_setpoint_array[i] = MAX_ALLOWED_SPEED; -// } -// launch_state = launch_not_ready; -// // Original load cell torque vectoring +// Call the ADIS16460 IMU class +ADIS16460 IMU(IMU_CS, IMU_DATAREADY, IMU_RESET); // Chip Select, Data Ready, Reset Pin Assignments + +// Outbound CAN message/* */s +MCU_pedal_readings mcu_pedal_readings; +MCU_status mcu_status{}; +MCU_load_cells mcu_load_cells{}; +MCU_front_potentiometers mcu_front_potentiometers; +MCU_rear_potentiometers mcu_rear_potentiometers; +MCU_analog_readings mcu_analog_readings{}; + +// IMU +IMU_accelerometer imu_accelerometer; +IMU_gyroscope imu_gyroscope; +double imu_velocity; +double pitch_calibration_angle; + +MC_status mc_status[4]; +MC_temps mc_temps[4]; +MC_energy mc_energy[4]; +MC_setpoints_command mc_setpoints_command[4]; + +// Inbound CAN messages +BMS_coulomb_counts bms_coulomb_counts{}; +BMS_status bms_status{}; +BMS_temperatures bms_temperatures{}; +BMS_voltages bms_voltages{}; +Dashboard_status dashboard_status{}; + +//Timers +Metro timer_imu_integration = Metro(50); +Metro timer_CAN_imu_accelerometer_send = Metro(50); +Metro timer_CAN_imu_gyroscope_send = Metro(50); +Metro timer_CAN_inverter_setpoints_send = Metro(20); +Metro timer_CAN_coloumb_count_send = Metro(1000); +Metro timer_CAN_mcu_status_send = Metro(100); +Metro timer_CAN_mcu_pedal_readings_send = Metro(50); +Metro timer_CAN_mcu_analog_readings_send = Metro(50); +Metro timer_CAN_mcu_load_cells_send = Metro(20); +Metro timer_CAN_mcu_potentiometers_send = Metro(20); + +Metro timer_ready_sound = Metro(2000); // Time to play RTD sound + +Metro timer_read_all_adcs = Metro(20); +Metro timer_steering_spi_read = Metro(1000); +Metro timer_read_imu = Metro(20); + +Metro timer_inverter_enable = Metro(5000); +Metro timer_reset_inverter = Metro(5000); +Metro timer_watchdog_timer = Metro(7); + +elapsedMillis pedal_implausability_duration; + +Metro timer_debug = Metro(200); +Metro timer_debug2 = Metro(1000); + +// this abuses Metro timer functionality to stay faulting once a fault has occurred +// autoreset makes the timer update to the current time and not by the interval +// this allows me to set the interval as 0 once a fault has occurred, leading to continuous faulting +// until a CAN message comes in which resets the timer and the interval +Metro timer_bms_heartbeat = Metro(0, 1); + + +bool imd_faulting = false; +bool inverter_restart = false; // True when restarting the inverter +INVERTER_STARTUP_STATE inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; + +ADC_SPI ADC1(ADC1_CS, ADC_SPI_SPEED, ECU_SDI, ECU_SDO, ECU_CLK); +ADC_SPI ADC2(ADC2_CS, ADC_SPI_SPEED, ECU_SDI, ECU_SDO, ECU_CLK); +ADC_SPI ADC3(ADC3_CS, ADC_SPI_SPEED, ECU_SDI, ECU_SDO, ECU_CLK); + +STEERING_SPI STEERING(STEERING_CS, STEERING_SPI_SPEED); + +FlexCAN_T4 FRONT_INV_CAN; +FlexCAN_T4 REAR_INV_CAN; +FlexCAN_T4 TELEM_CAN; +CAN_message_t msg; + +// coloumb counts +uint32_t total_discharge; +unsigned long previous_data_time; + +int16_t torque_setpoint_array[4]; +int16_t speed_setpoint_array[4] = {0, 0, 0, 0}; + +uint16_t prev_load_cell_readings[4] = {0, 0, 0, 0}; +float load_cell_alpha = 0.95; + +float filtered_min_cell_voltage = 3.5; +float cell_voltage_alpha = 0.8; + +float filtered_max_cell_temp = 40.0; +float cell_temp_alpha = 0.8; + +uint16_t current_read = 0; +uint16_t reference_read = 0; + +float max_front_power = 0.0; +float max_rear_power = 0.0; + +enum launch_states {launch_not_ready, launch_ready, launching}; +launch_states launch_state = launch_not_ready; +int16_t launch_speed_target = 0; +elapsedMillis time_since_launch; +const uint16_t LAUNCH_READY_ACCEL_THRESHOLD = 100; +const uint16_t LAUNCH_READY_BRAKE_THRESHOLD = 300; +const int16_t LAUNCH_READY_SPEED_THRESHOLD = 500; +const uint16_t LAUNCH_GO_THRESHOLD = 900; +const uint16_t LAUNCH_STOP_THRESHOLD = 600; +float launch_rate_target = 0.0; + +inline void set_all_inverters_dc_on(bool input); +inline void calculate_pedal_implausibilities(); + +inline float max_allowed_torque(float maxwatts, float rpm); +inline void set_inverter_torques_regen_only(); +inline void set_all_inverters_disabled(); +inline void set_all_inverters_no_torque(); +bool check_all_inverters_quit_dc_on(); +inline void check_TS_active(); +bool check_all_inverters_system_ready(); + +inline void set_inverter_torques(); +uint8_t check_all_inverters_error(); +bool check_all_inverters_quit_inverter_on(); +inline void set_all_inverters_inverter_enable(bool input); +inline void set_all_inverters_no_torque(); + +inline void set_all_inverters_driver_enable(bool input); + +bool check_TS_over_HV_threshold(); + + +void set_state(MCU_STATE new_state); + +inline void send_CAN_inverter_setpoints() { + if (timer_CAN_inverter_setpoints_send.check()) { + mc_setpoints_command[0].write(msg.buf); + msg.id = ID_MC1_SETPOINTS_COMMAND; + msg.len = sizeof(mc_setpoints_command[0]); + FRONT_INV_CAN.write(msg); + + mc_setpoints_command[1].write(msg.buf); + msg.id = ID_MC2_SETPOINTS_COMMAND; + msg.len = sizeof(mc_setpoints_command[1]); + FRONT_INV_CAN.write(msg); + + mc_setpoints_command[2].write(msg.buf); + msg.id = ID_MC3_SETPOINTS_COMMAND; + msg.len = sizeof(mc_setpoints_command[2]); + REAR_INV_CAN.write(msg); + + mc_setpoints_command[3].write(msg.buf); + msg.id = ID_MC4_SETPOINTS_COMMAND; + msg.len = sizeof(mc_setpoints_command[3]); + REAR_INV_CAN.write(msg); + } +} + +inline void send_CAN_mcu_status() { + if (timer_CAN_mcu_status_send.check()) { + // Send Main Control Unit status message + mcu_status.write(msg.buf); + msg.id = ID_MCU_STATUS; + msg.len = sizeof(mcu_status); + TELEM_CAN.write(msg); + } +} + +inline void send_CAN_mcu_pedal_readings() { + if (timer_CAN_mcu_pedal_readings_send.check()) { + mcu_pedal_readings.write(msg.buf); + msg.id = ID_MCU_PEDAL_READINGS; + msg.len = sizeof(mcu_pedal_readings); + TELEM_CAN.write(msg); + } +} + +inline void send_CAN_mcu_load_cells() { + if (timer_CAN_mcu_load_cells_send.check()) { + mcu_load_cells.write(msg.buf); + msg.id = ID_MCU_LOAD_CELLS; + msg.len = sizeof(mcu_load_cells); + TELEM_CAN.write(msg); + } +} + +inline void send_CAN_mcu_potentiometers() { + if (timer_CAN_mcu_potentiometers_send.check()) { + mcu_front_potentiometers.write(msg.buf); + msg.id = ID_MCU_FRONT_POTS; + msg.len = sizeof(mcu_front_potentiometers); + TELEM_CAN.write(msg); + mcu_rear_potentiometers.write(msg.buf); + msg.id = ID_MCU_REAR_POTS; + msg.len = sizeof(mcu_rear_potentiometers); + TELEM_CAN.write(msg); + } +} + +inline void send_CAN_bms_coulomb_counts() { + if (timer_CAN_coloumb_count_send.check()) { + bms_coulomb_counts.write(msg.buf); + msg.id = ID_BMS_COULOMB_COUNTS; + msg.len = sizeof(bms_coulomb_counts); + TELEM_CAN.write(msg); + } +} + +inline void send_CAN_mcu_analog_readings() { + if (timer_CAN_mcu_analog_readings_send.check()) { + mcu_analog_readings.write(msg.buf); + msg.id = ID_MCU_ANALOG_READINGS; + msg.len = sizeof(mcu_analog_readings); + TELEM_CAN.write(msg); + } +} +inline void state_machine() { + switch (mcu_status.get_state()) { + case MCU_STATE::STARTUP: break; + + case MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: +#if DEBUG + Serial.println("TS NOT ACTIVE"); +#endif + // if TS is above HV threshold, move to Tractive System Active + if (check_TS_over_HV_threshold()) { +#if DEBUG + Serial.println("Setting state to TS Active from TS Not Active"); +#endif + set_state(MCU_STATE::TRACTIVE_SYSTEM_ACTIVE); + } + break; + + case MCU_STATE::TRACTIVE_SYSTEM_ACTIVE: + check_TS_active(); + if (check_all_inverters_system_ready()) { + set_all_inverters_dc_on(true); + inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_QUIT_DC_ON; + } + if (dashboard_status.get_start_btn() && mcu_status.get_mech_brake_active()) { +#if DEBUG + Serial.println("Setting state to Enabling Inverter"); +#endif + set_state(MCU_STATE::ENABLING_INVERTER); + } + break; + + case MCU_STATE::ENABLING_INVERTER: + check_TS_active(); + // inverter enabling timed out + if (timer_inverter_enable.check()) { +#if DEBUG + Serial.println("Setting state to TS Active from Enabling Inverter"); +#endif + set_state(MCU_STATE::TRACTIVE_SYSTEM_ACTIVE); + } + + switch (inverter_startup_state) { + case INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY: + if (check_all_inverters_system_ready()) { + set_all_inverters_dc_on(true); + inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_QUIT_DC_ON; + } + break; + + case INVERTER_STARTUP_STATE::WAIT_QUIT_DC_ON: + if (check_all_inverters_quit_dc_on()) { + set_all_inverters_no_torque(); + set_all_inverters_driver_enable(true); + set_all_inverters_inverter_enable(true); + inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_QUIT_INVERTER_ON; + } + break; + + case INVERTER_STARTUP_STATE::WAIT_QUIT_INVERTER_ON: + if (check_all_inverters_quit_inverter_on()) { +#if DEBUG + Serial.println("Setting state to Waiting Ready to Drive Sound"); +#endif + set_state(MCU_STATE::WAITING_READY_TO_DRIVE_SOUND); + } + break; + + } + break; + + + case MCU_STATE::WAITING_READY_TO_DRIVE_SOUND: + check_TS_active(); + + // if the ready to drive sound has been playing for long enough, move to ready to drive mode + if (timer_ready_sound.check()) { +#if DEBUG + Serial.println("Setting state to Ready to Drive"); +#endif + set_state(MCU_STATE::READY_TO_DRIVE); + } + break; + + case MCU_STATE::READY_TO_DRIVE: + check_TS_active(); + + if (check_all_inverters_error()) { + set_all_inverters_disabled(); + set_state(MCU_STATE::TRACTIVE_SYSTEM_ACTIVE); + } + + calculate_pedal_implausibilities(); + + if ( + pedal_implausability_duration <= 100 && + mcu_status.get_bms_ok_high() && + mcu_status.get_imd_ok_high() + + ) { + set_inverter_torques(); + } else if (mcu_status.get_bms_ok_high() && mcu_status.get_imd_ok_high()) { + set_inverter_torques_regen_only(); + } else { + Serial.println("not calculating torque"); + Serial.printf("no brake implausibility: %d\n", mcu_status.get_no_brake_implausability()); + Serial.printf("no accel implausibility: %d\n", mcu_status.get_no_accel_implausability()); + Serial.printf("no accel brake implausibility: %d\n", mcu_status.get_no_accel_brake_implausability()); + Serial.printf("software is ok: %d\n", mcu_status.get_software_is_ok()); + Serial.printf("get bms ok high: %d\n", mcu_status.get_bms_ok_high()); + Serial.printf("get imd ok high: %d\n", mcu_status.get_imd_ok_high()); + set_all_inverters_no_torque(); + } + + break; + } +} + +/* Shared state functinality */ + + +bool check_TS_over_HV_threshold() { + for (uint8_t inv = 0; inv < 4; inv++) { + if (mc_energy[inv].get_dc_bus_voltage() < MIN_HV_VOLTAGE) { + return false; + } + } + return true; +} + +// if TS is below HV threshold, return to Tractive System Not Active +inline void check_TS_active() { + if (!check_TS_over_HV_threshold()) { +#if DEBUG + Serial.println("Setting state to TS Not Active, because TS is below HV threshold"); +#endif + set_state(MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); + inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; + } +} + + +/* Implementation of software shutdown */ +inline void software_shutdown() { + + mcu_status.set_software_is_ok(true); + + // check inputs + // BMS heartbeat has not arrived within time interval + if (timer_bms_heartbeat.check()) { + timer_bms_heartbeat.interval(0); +#if DEBUG + Serial.println("no bms heartbeat"); +#endif + mcu_status.set_software_is_ok(false); + } + + // add BMS software checks + // software ok/not ok action + if (mcu_status.get_software_is_ok()) { + digitalWrite(SOFTWARE_OK, HIGH); //eventually make this HIGH only if software is ok + } else { + digitalWrite(SOFTWARE_OK, LOW); + } + /* Watchdog timer */ + if (timer_watchdog_timer.check()) { + static bool watchdog_state = HIGH; + watchdog_state = !watchdog_state; + digitalWrite(WATCHDOG_INPUT, watchdog_state); + } + +} + +/* Parse incoming CAN messages */ +void parse_telem_can_message(const CAN_message_t &RX_msg) { + CAN_message_t rx_msg = RX_msg; + switch (rx_msg.id) { + case ID_BMS_TEMPERATURES: + bms_temperatures.load(rx_msg.buf); + filtered_max_cell_temp = filtered_max_cell_temp * cell_temp_alpha + (1.0 - cell_temp_alpha) * (bms_temperatures.get_high_temperature() / 100.0) ; + break; + case ID_BMS_VOLTAGES: + bms_voltages.load(rx_msg.buf); + if (bms_voltages.get_low() < PACK_CHARGE_CRIT_LOWEST_CELL_THRESHOLD || bms_voltages.get_total() < PACK_CHARGE_CRIT_TOTAL_THRESHOLD) { + mcu_status.set_pack_charge_critical(true); + } else mcu_status.set_pack_charge_critical(false); + filtered_min_cell_voltage = filtered_min_cell_voltage * cell_voltage_alpha + (1.0 - cell_voltage_alpha) * (bms_voltages.get_low() / 10000.0); + break; + case ID_BMS_COULOMB_COUNTS: bms_coulomb_counts.load(rx_msg.buf); break; + case ID_BMS_STATUS: + bms_status.load(rx_msg.buf); + // BMS heartbeat timer + timer_bms_heartbeat.reset(); + timer_bms_heartbeat.interval(BMS_HEARTBEAT_TIMEOUT); + break; + case ID_DASHBOARD_STATUS: + dashboard_status.load(rx_msg.buf); + /* process dashboard buttons */ + if (dashboard_status.get_torque_mode_btn()) { + switch (mcu_status.get_torque_mode()) { + case 1: + mcu_status.set_max_torque(TORQUE_2); + mcu_status.set_torque_mode(2); break; + case 2: + mcu_status.set_max_torque(TORQUE_3); + mcu_status.set_torque_mode(3); break; + case 3: + mcu_status.set_max_torque(TORQUE_1); + mcu_status.set_torque_mode(1); break; + } + } + if (dashboard_status.get_launch_ctrl_btn()) { + mcu_status.toggle_launch_ctrl_active(); + } + if (dashboard_status.get_mc_cycle_btn()) { + inverter_restart = true; + timer_reset_inverter.reset(); + } + // eliminate all action buttons to not process twice + dashboard_status.set_button_flags(0); + break; + } +} + +void parse_front_inv_can_message(const CAN_message_t &RX_msg) { + CAN_message_t rx_msg = RX_msg; + switch (rx_msg.id) { + case ID_MC1_STATUS: mc_status[0].load(rx_msg.buf); break; + case ID_MC2_STATUS: mc_status[1].load(rx_msg.buf); break; + case ID_MC1_TEMPS: mc_temps[0].load(rx_msg.buf); break; + case ID_MC2_TEMPS: mc_temps[1].load(rx_msg.buf); break; + case ID_MC1_ENERGY: mc_energy[0].load(rx_msg.buf); break; + case ID_MC2_ENERGY: mc_energy[1].load(rx_msg.buf); break; + } +} + +void parse_rear_inv_can_message(const CAN_message_t &RX_msg) { + CAN_message_t rx_msg = RX_msg; + + switch (rx_msg.id) { + case ID_MC3_STATUS: mc_status[2].load(rx_msg.buf); break; + case ID_MC4_STATUS: mc_status[3].load(rx_msg.buf); break; + case ID_MC3_TEMPS: mc_temps[2].load(rx_msg.buf); break; + case ID_MC4_TEMPS: mc_temps[3].load(rx_msg.buf); break; + case ID_MC3_ENERGY: mc_energy[2].load(rx_msg.buf); break; + case ID_MC4_ENERGY: mc_energy[3].load(rx_msg.buf); break; + } +} +//FIXME +inline void power_off_inverter() { + digitalWrite(INVERTER_24V_EN, LOW); + +#if DEBUG + Serial.println("INVERTER POWER OFF"); +#endif +} + + +/* Handle changes in state */ +void set_state(MCU_STATE new_state) { + if (mcu_status.get_state() == new_state) { + return; + } + + // exit logic + switch (mcu_status.get_state()) { + case MCU_STATE::STARTUP: break; + case MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: break; + case MCU_STATE::TRACTIVE_SYSTEM_ACTIVE: break; + case MCU_STATE::ENABLING_INVERTER: + timer_inverter_enable.reset(); + break; + case MCU_STATE::WAITING_READY_TO_DRIVE_SOUND: + // make dashboard stop buzzer + mcu_status.set_activate_buzzer(false); + mcu_status.write(msg.buf); + msg.id = ID_MCU_STATUS; + msg.len = sizeof(mcu_status); + TELEM_CAN.write(msg); + break; + case MCU_STATE::READY_TO_DRIVE: { + inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; + break; + } + } + + mcu_status.set_state(new_state); + + // entry logic + switch (new_state) { + case MCU_STATE::STARTUP: break; + case MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: break; + case MCU_STATE::TRACTIVE_SYSTEM_ACTIVE: { + inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; + break; + } + case MCU_STATE::ENABLING_INVERTER: { + Serial.println("MCU Sent enable command"); + timer_inverter_enable.reset(); + break; + } + case MCU_STATE::WAITING_READY_TO_DRIVE_SOUND: + // make dashboard sound buzzer + mcu_status.set_activate_buzzer(true); + mcu_status.write(msg.buf); + msg.id = ID_MCU_STATUS; + msg.len = sizeof(mcu_status); + TELEM_CAN.write(msg); + + timer_ready_sound.reset(); + Serial.println("RTDS enabled"); + break; + case MCU_STATE::READY_TO_DRIVE: + Serial.println("Ready to drive"); + break; + } +} + +inline float float_map(float x, float in_min, float in_max, float out_min, float out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +inline void set_inverter_torques_regen_only() { + int brake1 = map(round(mcu_pedal_readings.get_brake_pedal_1()), START_BRAKE_PEDAL_1, END_BRAKE_PEDAL_1, 0, 2140); + int brake2 = map(round(mcu_pedal_readings.get_brake_pedal_2()), START_BRAKE_PEDAL_2, END_BRAKE_PEDAL_2, 0, 2140); + int avg_brake = (brake1 + brake2) / 2; + if (avg_brake > 1500) { + avg_brake = 1500; + } + if (avg_brake < 0) { + avg_brake = 0; + } + torque_setpoint_array[0] = - avg_brake; + torque_setpoint_array[1] = - avg_brake; + torque_setpoint_array[2] = - avg_brake; + torque_setpoint_array[3] = - avg_brake; + for (int i = 0; i < 4; i++) { + if (i < 2) { + torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * 1.33); + } else { + torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * 0.66); + } + + } + for (int i = 0; i < 4; i++) { + if (torque_setpoint_array[i] >= 0) { + } + else { + int16_t max_speed_regen = 0; + for (int i = 0; i < sizeof(torque_setpoint_array); i++) { + + max_speed_regen = (max_speed_regen < mc_status[i].get_speed()) ? mc_status[i].get_speed() : max_speed_regen; + + } + float scale_down = 1; + if (max_speed_regen < 770) { + scale_down = 0; + } else if (max_speed_regen > REGEN_OFF_START_THRESHOLD) { + scale_down = 1; + } else { + scale_down = map(max_speed_regen, 770, REGEN_OFF_START_THRESHOLD, 0, 1); + } + mc_setpoints_command[i].set_speed_setpoint(0); + mc_setpoints_command[i].set_pos_torque_limit(0); + mc_setpoints_command[i].set_neg_torque_limit(max(((int16_t)(torque_setpoint_array[i]) * scale_down) , -2140) ); + + } + } +} + +inline void set_inverter_torques() { + + float max_torque = mcu_status.get_max_torque() / 0.0098; // max possible value for torque multiplier, unit in 0.1% nominal torque + int accel1 = map(round(mcu_pedal_readings.get_accelerator_pedal_1()), START_ACCELERATOR_PEDAL_1, END_ACCELERATOR_PEDAL_1, 0, 2140); + int accel2 = map(round(mcu_pedal_readings.get_accelerator_pedal_2()), START_ACCELERATOR_PEDAL_2, END_ACCELERATOR_PEDAL_2, 0, 2140); + + int brake1 = map(round(mcu_pedal_readings.get_brake_pedal_1()), START_BRAKE_PEDAL_1, END_BRAKE_PEDAL_1, 0, 2140); + int brake2 = map(round(mcu_pedal_readings.get_brake_pedal_2()), START_BRAKE_PEDAL_2, END_BRAKE_PEDAL_2, 0, 2140); + + + // torque values are greater than the max possible value, set them to max + if (accel1 > max_torque) { + accel1 = max_torque; + } + if (accel2 > max_torque) { + accel2 = max_torque; + } + int avg_accel = (accel1 + accel2) / 2; + int avg_brake = (brake1 + brake2) / 2; + if (avg_accel > max_torque) { + avg_accel = max_torque; + } + if (avg_accel < 0) { + avg_accel = 0; + } + if (avg_accel > max_torque) { + avg_accel = max_torque; + } + if (avg_accel < 0) { + avg_accel = 0; + } + if (avg_brake > 1500) { + avg_brake = 1500; + } + if (avg_brake < 0) { + avg_brake = 0; + } + + float rear_power_balance = 0.66; + float front_power_balance = 1.0 - rear_power_balance; + float rear_brake_balance = 0.1; + float front_brake_balance = 1.0 - rear_brake_balance; + + int32_t total_torque = 0; + int32_t total_load_cells = 0; + float front_load_total; + float rear_load_total; + + float attesa_def_split = 0.85; + float attesa_alt_split = 0.5; + float fr_slip_clamped; + float fr_slip_factor = 2.5; // Factor of 5 causes 50/50 split at 20% r/f slip. Lower values allow more slip + float f_torque; + float r_torque; + float rear_lr_slip_clamped; + float lsd_right_split; // Fraction of rear axle torque going to rear right wheel + float lsd_slip_factor = 0.5; + + float avg_speed = 0.0; + for (int i = 0; i < 4; i++) + avg_speed += ((float) mc_status[i].get_speed()) / 4.0; + int16_t start_derating_rpm = 2000; + int16_t end_derating_rpm = 20000; + + const float hairpin_rpm_limit = 5600.0; + const float hairpin_rpm_full = 2800.0; + float hairpin_rpm_factor = 0.0; + const float hairpin_steering_min = 80.0; // degrees + const float hairpin_steering_max = 120.0; // degrees + float steering_calibration_slope = -0.111; + float steering_calibration_offset = 260.0; + // positive steering angle is to the left + float steering_angle = mcu_analog_readings.get_steering_2() * steering_calibration_slope + steering_calibration_offset; + float hairpin_reallocation = 0.0; + float hairpin_steering_factor = 0.0; + + int16_t max_speed; + + switch (dashboard_status.get_dial_state()) { + case 0: + for (int i = 0; i < 4; i++) { + speed_setpoint_array[i] = MAX_ALLOWED_SPEED; + } + launch_state = launch_not_ready; + // standard no torque vectoring + + max_front_power = 19000.0; + max_rear_power = 36000.0; + + torque_setpoint_array[0] = avg_accel - avg_brake; + torque_setpoint_array[1] = avg_accel - avg_brake; + torque_setpoint_array[2] = avg_accel - avg_brake; + torque_setpoint_array[3] = avg_accel - avg_brake; + + for (int i = 0; i < 4; i++) { + if (torque_setpoint_array[i] >= 0) { + if (i < 2) { + torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * front_power_balance); + } else { + torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * rear_power_balance); + } + } else { + if (i < 2) { + torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * front_brake_balance); + } else { + torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * rear_brake_balance); + } + } + } + break; + case 1: + for (int i = 0; i < 4; i++) { + speed_setpoint_array[i] = MAX_ALLOWED_SPEED; + } + launch_state = launch_not_ready; + // Original load cell torque vectoring -// max_front_power = 19000.0; -// max_rear_power = 36000.0; + max_front_power = 19000.0; + max_rear_power = 36000.0; -// load_cell_alpha = 0.95; -// total_torque = 4 * (avg_accel - avg_brake) ; -// total_load_cells = mcu_load_cells.get_FL_load_cell() + mcu_load_cells.get_FR_load_cell() + mcu_load_cells.get_RL_load_cell() + mcu_load_cells.get_RR_load_cell(); -// if (avg_accel >= avg_brake) { -// torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque); -// } else { -// torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); -// torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); -// } -// break; -// case 2: -// max_speed = 0; -// launch_rate_target = 11.76; -// for (int i = 0; i < 4; i++) { -// max_speed = max(max_speed, mc_status[i].get_speed()); -// } -// // max_front_power = 19000.0; -// // max_rear_power = 36000.0; -// max_front_power = 21760.0; -// max_rear_power = 41230.0; - -// switch (launch_state) { -// case launch_not_ready: -// for (int i = 0; i < 4; i++) { -// torque_setpoint_array[i] = (int16_t)(-1 * avg_brake); -// speed_setpoint_array[i] = 0; -// } -// time_since_launch = 0; -// launch_speed_target = 0; - -// // To enter launch_ready, the following conditions must be true: -// // 1. Pedals are not pressed -// // 2. Speed is zero -// if (avg_accel < LAUNCH_READY_ACCEL_THRESHOLD && avg_brake < LAUNCH_READY_BRAKE_THRESHOLD && max_speed < LAUNCH_READY_SPEED_THRESHOLD) { -// launch_state = launch_ready; -// } -// break; -// case launch_ready: -// for (int i = 0; i < 4; i++) { -// torque_setpoint_array[i] = 0; -// speed_setpoint_array[i] = 0; -// } -// time_since_launch = 0; -// launch_speed_target = 0; - -// // Revert to launch_not_ready if brake is pressed or speed is too high -// if (avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD || max_speed >= LAUNCH_READY_SPEED_THRESHOLD) { -// launch_state = launch_not_ready; -// } else { -// // Otherwise, check if launch should begin -// if (avg_accel >= LAUNCH_GO_THRESHOLD) { -// launch_state = launching; -// } -// } - -// break; -// case launching: -// // Exit launch if accel pedal goes past STOP threshold or brake pedal is pressed -// if (avg_accel <= LAUNCH_STOP_THRESHOLD || avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD) { -// launch_state = launch_not_ready; -// break; -// } - -// launch_speed_target = (int16_t)((float) time_since_launch / 1000.0 * launch_rate_target * 60.0 / 1.2767432544 * 11.86); -// launch_speed_target += 1500; -// launch_speed_target = min(20000, max(0, launch_speed_target)); - -// for (int i = 0; i < 4; i++) { -// torque_setpoint_array[i] = 2142; -// speed_setpoint_array[i] = launch_speed_target; -// } -// break; -// default: -// break; -// } - -// break; -// case 3: -// max_speed = 0; -// launch_rate_target = 12.74; -// for (int i = 0; i < 4; i++) { -// max_speed = max(max_speed, mc_status[i].get_speed()); -// } -// // max_front_power = 19000.0; -// // max_rear_power = 36000.0; -// max_front_power = 21760.0; -// max_rear_power = 41230.0; - -// switch (launch_state) { -// case launch_not_ready: -// for (int i = 0; i < 4; i++) { -// torque_setpoint_array[i] = (int16_t)(-1 * avg_brake); -// speed_setpoint_array[i] = 0; -// } -// time_since_launch = 0; -// launch_speed_target = 0; - -// // To enter launch_ready, the following conditions must be true: -// // 1. Pedals are not pressed -// // 2. Speed is zero -// if (avg_accel < LAUNCH_READY_ACCEL_THRESHOLD && avg_brake < LAUNCH_READY_BRAKE_THRESHOLD && max_speed < LAUNCH_READY_SPEED_THRESHOLD) { -// launch_state = launch_ready; -// } -// break; -// case launch_ready: -// for (int i = 0; i < 4; i++) { -// torque_setpoint_array[i] = 0; -// speed_setpoint_array[i] = 0; -// } -// time_since_launch = 0; -// launch_speed_target = 0; - -// // Revert to launch_not_ready if brake is pressed or speed is too high -// if (avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD || max_speed >= LAUNCH_READY_SPEED_THRESHOLD) { -// launch_state = launch_not_ready; -// } else { -// // Otherwise, check if launch should begin -// if (avg_accel >= LAUNCH_GO_THRESHOLD) { -// launch_state = launching; -// } -// } - -// break; -// case launching: -// // Exit launch if accel pedal goes past STOP threshold or brake pedal is pressed -// if (avg_accel <= LAUNCH_STOP_THRESHOLD || avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD) { -// launch_state = launch_not_ready; -// break; -// } - -// launch_speed_target = (int16_t)((float) time_since_launch / 1000.0 * launch_rate_target * 60.0 / 1.2767432544 * 11.86); -// launch_speed_target += 1500; -// launch_speed_target = min(20000, max(0, launch_speed_target)); - -// for (int i = 0; i < 4; i++) { -// torque_setpoint_array[i] = 2142; -// speed_setpoint_array[i] = launch_speed_target; -// } -// break; -// default: -// break; -// } - -// break; -// case 4: -// { -// // Copy pasted from mode 2 with additional derating for endurance -// for (int i = 0; i < 4; i++) { -// speed_setpoint_array[i] = MAX_ALLOWED_SPEED; -// } -// max_front_power = 19000.0; -// max_rear_power = 36000.0; -// launch_state = launch_not_ready; -// // Original load cell torque vectoring -// load_cell_alpha = 0.95; -// total_torque = 4 * (avg_accel - avg_brake) ; -// total_load_cells = mcu_load_cells.get_FL_load_cell() + mcu_load_cells.get_FR_load_cell() + mcu_load_cells.get_RL_load_cell() + mcu_load_cells.get_RR_load_cell(); - -// // Derating -// float derating_factor = float_map(avg_speed, start_derating_rpm, end_derating_rpm, 1.0, 0.0); -// derating_factor = min(1.0, max(0.0, derating_factor)); - -// if (avg_accel >= avg_brake) { -// torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); -// torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); -// torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); -// torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); -// } else { -// torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); -// torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); -// } -// break; -// } -// case 5: -// { -// for (int i = 0; i < 4; i++) { -// speed_setpoint_array[i] = MAX_ALLOWED_SPEED; -// } -// launch_state = launch_not_ready; -// // Original load cell torque vectoring + load_cell_alpha = 0.95; + total_torque = 4 * (avg_accel - avg_brake) ; + total_load_cells = mcu_load_cells.get_FL_load_cell() + mcu_load_cells.get_FR_load_cell() + mcu_load_cells.get_RL_load_cell() + mcu_load_cells.get_RR_load_cell(); + if (avg_accel >= avg_brake) { + torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque); + } else { + torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); + torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); + } + break; + case 2: + max_speed = 0; + launch_rate_target = 11.76; + for (int i = 0; i < 4; i++) { + max_speed = max(max_speed, mc_status[i].get_speed()); + } +// max_front_power = 19000.0; +// max_rear_power = 36000.0; + max_front_power = 21760.0; + max_rear_power = 41230.0; + + switch (launch_state) { + case launch_not_ready: + for (int i = 0; i < 4; i++) { + torque_setpoint_array[i] = (int16_t)(-1 * avg_brake); + speed_setpoint_array[i] = 0; + } + time_since_launch = 0; + launch_speed_target = 0; + + // To enter launch_ready, the following conditions must be true: + // 1. Pedals are not pressed + // 2. Speed is zero + if (avg_accel < LAUNCH_READY_ACCEL_THRESHOLD && avg_brake < LAUNCH_READY_BRAKE_THRESHOLD && max_speed < LAUNCH_READY_SPEED_THRESHOLD) { + launch_state = launch_ready; + } + break; + case launch_ready: + for (int i = 0; i < 4; i++) { + torque_setpoint_array[i] = 0; + speed_setpoint_array[i] = 0; + } + time_since_launch = 0; + launch_speed_target = 0; + + // Revert to launch_not_ready if brake is pressed or speed is too high + if (avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD || max_speed >= LAUNCH_READY_SPEED_THRESHOLD) { + launch_state = launch_not_ready; + } else { + // Otherwise, check if launch should begin + if (avg_accel >= LAUNCH_GO_THRESHOLD) { + launch_state = launching; + } + } + + break; + case launching: + // Exit launch if accel pedal goes past STOP threshold or brake pedal is pressed + if (avg_accel <= LAUNCH_STOP_THRESHOLD || avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD) { + launch_state = launch_not_ready; + break; + } + + launch_speed_target = (int16_t)((float) time_since_launch / 1000.0 * launch_rate_target * 60.0 / 1.2767432544 * 11.86); + launch_speed_target += 1500; + launch_speed_target = min(20000, max(0, launch_speed_target)); + + for (int i = 0; i < 4; i++) { + torque_setpoint_array[i] = 2142; + speed_setpoint_array[i] = launch_speed_target; + } + break; + default: + break; + } + + break; + case 3: + max_speed = 0; + launch_rate_target = 12.74; + for (int i = 0; i < 4; i++) { + max_speed = max(max_speed, mc_status[i].get_speed()); + } +// max_front_power = 19000.0; +// max_rear_power = 36000.0; + max_front_power = 21760.0; + max_rear_power = 41230.0; + + switch (launch_state) { + case launch_not_ready: + for (int i = 0; i < 4; i++) { + torque_setpoint_array[i] = (int16_t)(-1 * avg_brake); + speed_setpoint_array[i] = 0; + } + time_since_launch = 0; + launch_speed_target = 0; + + // To enter launch_ready, the following conditions must be true: + // 1. Pedals are not pressed + // 2. Speed is zero + if (avg_accel < LAUNCH_READY_ACCEL_THRESHOLD && avg_brake < LAUNCH_READY_BRAKE_THRESHOLD && max_speed < LAUNCH_READY_SPEED_THRESHOLD) { + launch_state = launch_ready; + } + break; + case launch_ready: + for (int i = 0; i < 4; i++) { + torque_setpoint_array[i] = 0; + speed_setpoint_array[i] = 0; + } + time_since_launch = 0; + launch_speed_target = 0; + + // Revert to launch_not_ready if brake is pressed or speed is too high + if (avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD || max_speed >= LAUNCH_READY_SPEED_THRESHOLD) { + launch_state = launch_not_ready; + } else { + // Otherwise, check if launch should begin + if (avg_accel >= LAUNCH_GO_THRESHOLD) { + launch_state = launching; + } + } + + break; + case launching: + // Exit launch if accel pedal goes past STOP threshold or brake pedal is pressed + if (avg_accel <= LAUNCH_STOP_THRESHOLD || avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD) { + launch_state = launch_not_ready; + break; + } + + launch_speed_target = (int16_t)((float) time_since_launch / 1000.0 * launch_rate_target * 60.0 / 1.2767432544 * 11.86); + launch_speed_target += 1500; + launch_speed_target = min(20000, max(0, launch_speed_target)); + + for (int i = 0; i < 4; i++) { + torque_setpoint_array[i] = 2142; + speed_setpoint_array[i] = launch_speed_target; + } + break; + default: + break; + } + + break; + case 4: + { + // Copy pasted from mode 2 with additional derating for endurance + for (int i = 0; i < 4; i++) { + speed_setpoint_array[i] = MAX_ALLOWED_SPEED; + } + max_front_power = 19000.0; + max_rear_power = 36000.0; + launch_state = launch_not_ready; + // Original load cell torque vectoring + load_cell_alpha = 0.95; + total_torque = 4 * (avg_accel - avg_brake) ; + total_load_cells = mcu_load_cells.get_FL_load_cell() + mcu_load_cells.get_FR_load_cell() + mcu_load_cells.get_RL_load_cell() + mcu_load_cells.get_RR_load_cell(); + + // Derating + float derating_factor = float_map(avg_speed, start_derating_rpm, end_derating_rpm, 1.0, 0.0); + derating_factor = min(1.0, max(0.0, derating_factor)); + + if (avg_accel >= avg_brake) { + torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); + torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); + torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); + torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); + } else { + torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); + torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); + } + break; + } + case 5: + { + for (int i = 0; i < 4; i++) { + speed_setpoint_array[i] = MAX_ALLOWED_SPEED; + } + launch_state = launch_not_ready; + // Original load cell torque vectoring -// max_front_power = 21760.0; -// max_rear_power = 41240.0; + max_front_power = 21760.0; + max_rear_power = 41240.0; -// load_cell_alpha = 0.95; -// total_torque = 4 * (avg_accel - avg_brake) ; -// total_load_cells = mcu_load_cells.get_FL_load_cell() + mcu_load_cells.get_FR_load_cell() + mcu_load_cells.get_RL_load_cell() + mcu_load_cells.get_RR_load_cell(); -// if (avg_accel >= avg_brake) { -// torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque); -// } else { -// torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); -// torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); -// torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); -// } -// break; -// } -// default: -// Serial.println("uh oh"); -// break; -// } - - - - -// //very start check if mc_energy.get_feedback_torque > 0 -// //power limit to 80kW -// //look at all torques -// //look at motor speeds / convert from rpm to angular speed -// //torque * speed / 1000 (kW) -// // scale down by m/e limits -// //lots of variables for documentation purposes -// //since torque unit to nominal torque and power conversion are linear, the diff can be applied directly to the torque setpoint value. -// if (mc_setpoints_command[0].get_pos_torque_limit() > 0 && mc_setpoints_command[1].get_pos_torque_limit() > 0 -// && mc_setpoints_command[2].get_pos_torque_limit() > 0 && mc_setpoints_command[3].get_pos_torque_limit() > 0) { -// float mech_power = 0; -// float mdiff = 1; -// //float ediff = 1; -// // float pw_lim_factor = 1.0; - -// float voltage_lim_factor = 1.0; -// float temp_lim_factor = 1.0; -// float accu_lim_factor = 1.0; - -// for (int i = 0; i < 4; i++) { -// float torque_in_nm = 9.8 * ((float) mc_setpoints_command[i].get_pos_torque_limit()) / 1000.0; -// float speed_in_rpm = (float) mc_status[i].get_speed(); -// mech_power += 2 * 3.1415 * torque_in_nm * speed_in_rpm / 60.0; -// } - -// // pw_lim_factor = float_map(mech_power, 40000.0, 55000.0, 1.0, 0); -// // pw_lim_factor = max(min(1.0, pw_lim_factor), 0.0); - -// voltage_lim_factor = float_map(filtered_min_cell_voltage, 3.5, 3.2, 1.0, 0.2); -// voltage_lim_factor = max(min(1.0, voltage_lim_factor), 0.2); - -// temp_lim_factor = float_map(filtered_max_cell_temp, 50.0, 58.0, 1.0, 0.2); -// temp_lim_factor = max(min(1.0, temp_lim_factor), 0.2); - -// accu_lim_factor = min(temp_lim_factor, voltage_lim_factor); -// //mech_power /= 1000.0; - -// // float current = (ADC1.read_channel(ADC_CURRENT_CHANNEL) - ADC1.read_channel(ADC_REFERENCE_CHANNEL)); -// // current = ((((current / 819.0) / .1912) / 4.832) - 2.5) * 1000) / 6.67; - -// // -// // float dc_power = (mc_energy[0].get_dc_bus_voltage() * current) / 1000; //mc dc bus voltage - -// //sum up kilowatts to align -// //if mech_power is at 63 kW, it's requesting 80 kW from the motor -// //2 kW safety factor for the more accurate motor readings. -// //as our effiecency increases say 68 kW would be drawing 80kW from the motor -// //as our efficiency decreases say 60 kW would be drawing 80kW from the motor -// //so if efficency is at 60kW and we want 63, we'd be drawing more from the battery triggering a safety problem -// //so if efficency is at 68 kW, 63 would be drawing less power, which is fine but wasted power. -// //if HV DC bus is over 80 kW, it's a violation! -// // 1 kW as a second safety factor. -// //if (mech_power > MECH_POWER_LIMIT) { -// // mdiff = MECH_POWER_LIMIT / mech_power; -// // diff = MECH_POWER_LIMIT / mech_power; -// //} -// // if (dc_power > DC_POWER_LIMIT) { -// // ediff = DC_POWER_LIMIT / dc_power; -// // } -// // if (mech_power > MECH_POWER_LIMIT && dc_power > DC_POWER_LIMIT) { -// // diff = (ediff <= mdiff) ? ediff : mdiff; -// // } -// torque_setpoint_array[0] = (uint16_t) (min((float) torque_setpoint_array[0], max_allowed_torque(max_front_power / 2.0, (float) mc_status[0].get_speed())) * accu_lim_factor); -// torque_setpoint_array[1] = (uint16_t) (min((float) torque_setpoint_array[1], max_allowed_torque(max_front_power / 2.0, (float) mc_status[1].get_speed())) * accu_lim_factor); -// torque_setpoint_array[2] = (uint16_t) (min((float) torque_setpoint_array[2], max_allowed_torque(max_rear_power / 2.0, (float) mc_status[2].get_speed())) * accu_lim_factor); -// torque_setpoint_array[3] = (uint16_t) (min((float) torque_setpoint_array[3], max_allowed_torque(max_rear_power / 2.0, (float) mc_status[3].get_speed())) * accu_lim_factor); -// } - - -// int16_t max_speed_regen = 0; -// for (int i = 0; i < sizeof(torque_setpoint_array); i++) { - -// max_speed_regen = (max_speed_regen < mc_status[i].get_speed()) ? mc_status[i].get_speed() : max_speed_regen; - -// } - -// for (int i = 0; i < 4; i++) { -// torque_setpoint_array[i] = max(-2140, min(2140, torque_setpoint_array[i])); -// } - -// for (int i = 0; i < 4; i++) { -// if (torque_setpoint_array[i] >= 0) { -// mc_setpoints_command[i].set_speed_setpoint(speed_setpoint_array[i]); -// mc_setpoints_command[i].set_pos_torque_limit(min(torque_setpoint_array[i] , 2140)); -// mc_setpoints_command[i].set_neg_torque_limit(0); - -// } -// else { - -// float scale_down = 1; -// if (max_speed_regen < 770) { -// scale_down = 0; -// } else if (max_speed_regen > REGEN_OFF_START_THRESHOLD) { -// scale_down = 1; -// } else { -// scale_down = map(max_speed_regen, 770, REGEN_OFF_START_THRESHOLD, 0, 1); -// } -// mc_setpoints_command[i].set_speed_setpoint(0); -// mc_setpoints_command[i].set_pos_torque_limit(0); -// mc_setpoints_command[i].set_neg_torque_limit(max(((int16_t)(torque_setpoint_array[i]) * scale_down) , -2140) ); - -// } -// } - -// } - -// inline void read_all_adcs() { -// if (timer_read_all_adcs.check()) { - -// prev_load_cell_readings[0] = mcu_load_cells.get_FL_load_cell(); -// prev_load_cell_readings[1] = mcu_load_cells.get_FR_load_cell(); -// prev_load_cell_readings[2] = mcu_load_cells.get_RL_load_cell(); -// prev_load_cell_readings[3] = mcu_load_cells.get_RR_load_cell(); - -// uint16_t adc1_inputs[8]; -// ADC1.read_all_channels(&adc1_inputs[0]); -// mcu_pedal_readings.set_accelerator_pedal_1(adc1_inputs[ADC_ACCEL_1_CHANNEL]); -// mcu_pedal_readings.set_accelerator_pedal_2(adc1_inputs[ADC_ACCEL_2_CHANNEL]); -// mcu_pedal_readings.set_brake_pedal_1(adc1_inputs[ADC_BRAKE_1_CHANNEL]); -// mcu_pedal_readings.set_brake_pedal_2(adc1_inputs[ADC_BRAKE_2_CHANNEL]); -// mcu_analog_readings.set_steering_2(adc1_inputs[ADC_STEERING_2_CHANNEL]); -// current_read = adc1_inputs[ADC_CURRENT_CHANNEL] - adc1_inputs[ADC_REFERENCE_CHANNEL]; -// float current = ((((current_read / 819.0) / .1912) / 4.832) * 1000) / 6.67; -// if (current > 300) { -// current = 300; -// } else if (current < -300) { -// current = -300; -// } -// mcu_analog_readings.set_hall_effect_current((uint16_t)current * 100); - - - -// mcu_load_cells.set_RL_load_cell((uint16_t)((adc1_inputs[ADC_RL_LOAD_CELL_CHANNEL]*LOAD_CELL3_SLOPE + LOAD_CELL3_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[2]*load_cell_alpha)); - -// mcu_status.set_brake_pedal_active(mcu_pedal_readings.get_brake_pedal_1() >= BRAKE_ACTIVE); -// digitalWrite(BRAKE_LIGHT_CTRL, mcu_status.get_brake_pedal_active()); - -// mcu_status.set_mech_brake_active(mcu_pedal_readings.get_brake_pedal_1() >= BRAKE_THRESHOLD_MECH_BRAKE_1); //define in driver_constraints.h (70%) - -// uint16_t adc2_inputs[8]; -// ADC2.read_all_channels(&adc2_inputs[0]); -// mcu_load_cells.set_RR_load_cell((uint16_t)((adc2_inputs[ADC_RR_LOAD_CELL_CHANNEL]*LOAD_CELL4_SLOPE + LOAD_CELL4_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[3]*load_cell_alpha)); -// mcu_load_cells.set_FL_load_cell((uint16_t)((adc2_inputs[ADC_FL_LOAD_CELL_CHANNEL]*LOAD_CELL1_SLOPE + LOAD_CELL1_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[0]*load_cell_alpha)); -// mcu_load_cells.set_FR_load_cell((uint16_t)((adc2_inputs[ADC_FR_LOAD_CELL_CHANNEL]*LOAD_CELL2_SLOPE + LOAD_CELL2_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[1]*load_cell_alpha)); -// mcu_rear_potentiometers.set_pot4(adc2_inputs[SUS_POT_RL]); -// mcu_rear_potentiometers.set_pot6(adc2_inputs[SUS_POT_RR]); -// mcu_front_potentiometers.set_pot1(adc2_inputs[SUS_POT_FL]); - -// uint16_t adc3_inputs[8]; -// ADC3.read_all_channels(&adc3_inputs[0]); -// mcu_analog_readings.set_glv_battery_voltage(adc3_inputs[ADC_GLV_READ_CHANNEL]); -// mcu_front_potentiometers.set_pot3(adc3_inputs[SUS_POT_FR]); -// } -// } - -// inline void read_steering_spi_values() { -// if (timer_steering_spi_read.check()) { -// mcu_analog_readings.set_steering_1(STEERING.read_steering()); - -// } -// } - -// bool check_all_inverters_system_ready() { -// for (uint8_t inv = 0; inv < 4; inv++) { -// if (! mc_status[inv].get_system_ready()) { -// return false; -// } -// } -// return true; -// } - -// bool check_all_inverters_quit_dc_on() { -// for (uint8_t inv = 0; inv < 4; inv++) { -// if (! mc_status[inv].get_quit_dc_on()) { -// return false; -// } -// } -// return true; -// } - -// bool check_all_inverters_quit_inverter_on() { -// for (uint8_t inv = 0; inv < 4; inv++) { -// if (! mc_status[inv].get_quit_inverter_on()) { -// return false; -// } -// } -// return true; -// } - -// uint8_t check_all_inverters_error() { -// uint8_t error_list = 0; //last 4 bits correspond to error bit in status word of each inverter, inverter 1 is rightmost bit; -// for (uint8_t inv = 0; inv < 4; inv++) { -// if (mc_status[inv].get_error()) { -// error_list = error_list | (0x01 << inv); -// } -// } -// if (error_list) { -// mcu_status.set_inverters_error(true); -// } else { -// mcu_status.set_inverters_error(false); -// } -// return error_list; -// } - -// inline void set_all_inverters_no_torque() { -// for (uint8_t inv = 0; inv < 4; inv++) { -// mc_setpoints_command[inv].set_speed_setpoint(0); -// mc_setpoints_command[inv].set_pos_torque_limit(0); -// mc_setpoints_command[inv].set_neg_torque_limit(0); -// } -// } - -// inline void set_all_inverters_torque_limit(int limit) { - -// //const float max_torque = max_torque_Nm / 0.0098; // max possible value for torque multiplier, unit in 0.1% nominal torque -// //inverter torque unit is in 0.1% of nominal torque (9.8Nm), max rated torque is 21Nm, so max possible output is 2142 - -// if (limit >= 0) { -// for (uint8_t inv = 0; inv < 4; inv++) { -// mc_setpoints_command[inv].set_pos_torque_limit(limit); -// mc_setpoints_command[inv].set_neg_torque_limit(0); -// } -// } -// else { -// for (uint8_t inv = 0; inv < 4; inv++) { -// mc_setpoints_command[inv].set_pos_torque_limit(0); -// mc_setpoints_command[inv].set_neg_torque_limit(limit); -// } -// } - -// } - -// inline void set_all_inverters_speed_setpoint(int setpoint) { -// for (uint8_t inv = 0; inv < 4; inv++) { -// mc_setpoints_command[inv].set_speed_setpoint(setpoint); -// } -// } - -// inline void set_all_inverters_disabled() { -// for (uint8_t inv = 0; inv < 4; inv++) { -// mc_setpoints_command[inv].set_inverter_enable(false); -// mc_setpoints_command[inv].set_hv_enable(false); -// mc_setpoints_command[inv].set_driver_enable(false); -// mc_setpoints_command[inv].set_remove_error(false); -// mc_setpoints_command[inv].set_speed_setpoint(0); -// mc_setpoints_command[inv].set_pos_torque_limit(0); -// mc_setpoints_command[inv].set_neg_torque_limit(0); -// } -// } - -// inline void set_all_inverters_dc_on(bool input) { -// for (uint8_t inv = 0; inv < 4; inv++) { -// mc_setpoints_command[inv].set_hv_enable(input); -// } -// } - -// inline void set_all_inverters_driver_enable(bool input) { -// for (uint8_t inv = 0; inv < 4; inv++) { -// mc_setpoints_command[inv].set_driver_enable(input); -// } -// } - -// inline void set_all_inverters_inverter_enable(bool input) { -// for (uint8_t inv = 0; inv < 4; inv++) { -// mc_setpoints_command[inv].set_inverter_enable(input); -// } -// } - -// inline void reset_inverters() { -// if (inverter_restart) { -// uint8_t reset_bit = !mc_setpoints_command[0].get_remove_error(); -// if (timer_reset_inverter.check()) { -// inverter_restart = false; -// reset_bit = 0; -// } -// for (uint8_t inv = 0; inv < 4; inv++) { -// mc_setpoints_command[inv].set_remove_error(reset_bit); -// } -// } -// } - -// /* Read shutdown system values */ -// inline void read_status_values() { -// /* Measure shutdown circuits' input */ -// mcu_status.set_bms_ok_high(digitalRead(BMS_OK_READ)); -// mcu_status.set_imd_ok_high(digitalRead(IMD_OK_READ)); -// mcu_status.set_bspd_ok_high(digitalRead(BSPD_OK_READ)); -// mcu_status.set_software_ok_high(digitalRead(SOFTWARE_OK_READ)); - -// /* Measure shutdown circuits' voltages */ -// mcu_status.set_shutdown_b_above_threshold(digitalRead(BOTS_OK_READ)); -// mcu_status.set_shutdown_c_above_threshold(digitalRead(IMD_OK_READ)); -// mcu_status.set_shutdown_d_above_threshold(digitalRead(BMS_OK_READ)); -// mcu_status.set_shutdown_e_above_threshold(digitalRead(BSPD_OK_READ)); -// } - -// // IMU functions -// inline void read_imu() { -// if (timer_read_imu.check()) { -// double sinAngle = sin(VEHICLE_TILT_ANGLE_X); -// double cosAngle = cos(VEHICLE_TILT_ANGLE_X); -// double accel_x = IMU.regRead(X_ACCL_OUT) * 0.00245; // 0.00245 is the scale -// double accel_y = IMU.regRead(Y_ACCL_OUT) * 0.00245; // 0.00245 is the scale -// double accel_z = IMU.regRead(Z_ACCL_OUT) * 0.00245; // 0.00245 is the scale -// double x_gyro = IMU.regRead(X_GYRO_OUT) * 0.005; // 0.005 is the scale -// double y_gyro = IMU.regRead(Y_GYRO_OUT) * 0.005; // 0.005 is the scale -// double z_gyro = IMU.regRead(Z_GYRO_OUT) * 0.005; // 0.005 is the scale -// double long_accel = ((-accel_y) * cosAngle) + (accel_z * sinAngle); -// double lat_accel = accel_x; -// double vert_accel = -((accel_z * cosAngle) - (-accel_y * sinAngle)); -// double pitch = (y_gyro * cosAngle) + (z_gyro * sinAngle); -// double roll = y_gyro; -// double yaw = (z_gyro * cosAngle) - (x_gyro * sinAngle); -// imu_accelerometer.set_lat_accel((int16_t)(lat_accel * 1000)); // * 0.00245); // 0.00245 is the scale -// imu_accelerometer.set_long_accel((int16_t)(long_accel * 1000)); // * 0.00245); // 0.00245 is the scale -// imu_accelerometer.set_vert_accel((int16_t)(vert_accel * 1000)); // * 0.00245); // 0.00245 is the scale -// // question about yaw, pitch and roll rates? -// imu_gyroscope.set_pitch((int16_t)(pitch * 100)); // * 0.005); // 0.005 is the scale, -// imu_gyroscope.set_yaw((int16_t)(yaw * 100 )); // * 0.005); // 0.005 is the scale -// imu_gyroscope.set_roll((int16_t)(roll * 100)); // * 0.005); // 0.005 is the scale -// } -// } - -// inline void send_CAN_imu_accelerometer() { -// if (timer_CAN_imu_accelerometer_send.check()) { -// imu_accelerometer.write(msg.buf); -// msg.id = ID_IMU_ACCELEROMETER; -// msg.len = sizeof(imu_accelerometer); -// TELEM_CAN.write(msg); -// } -// } - -// inline void send_CAN_imu_gyroscope() { -// if (timer_CAN_imu_gyroscope_send.check()) { -// imu_gyroscope.write(msg.buf); -// msg.id = ID_IMU_GYROSCOPE; -// msg.len = sizeof(imu_gyroscope); -// TELEM_CAN.write(msg); -// } -// } - -// inline void calibrate_imu_velocity(double calibrate_to) { -// imu_velocity = calibrate_to; -// } - -// inline void pitch_angle_calibration() { -// // double z_accl_sum = 0.0; -// // int ctr = 0; -// // time_t start_time, current_time; -// // double elapsed_time; -// // start_time = time(NULL); -// // // Serial.println("Calibration Starts Now"); FOR DEBUGING PURPOSES -// // while (1) { -// // delay(50); -// // z_accl_sum += ((IMU.regRead(Z_ACCL_OUT) * 0.00245)); -// // ctr++; -// // current_time = time(NULL); -// // elapsed_time = difftime(current_time, start_time); -// // if (elapsed_time >= 5) break; // Code runs for 5 seconds. Change for desired duration. -// // } -// // // Serial.println("Calibration Has Ended"); -// // double avg_z_accl = z_accl_sum / ctr; -// // pitch_calibration_angle = std::acos(avg_z_accl / ACCL_DUE_TO_GRAVITY); -// } - -// inline void calculate_pedal_implausibilities() { -// // FSAE EV.5.5 -// // FSAE T.4.2.10 -// if (mcu_pedal_readings.get_accelerator_pedal_1() < MIN_ACCELERATOR_PEDAL_1 || mcu_pedal_readings.get_accelerator_pedal_1() > MAX_ACCELERATOR_PEDAL_1) { -// mcu_status.set_no_accel_implausability(false); -// #if DEBUG -// Serial.println("T.4.2.10 1"); -// #endif -// } -// else if (mcu_pedal_readings.get_accelerator_pedal_2() > MAX_ACCELERATOR_PEDAL_2 || mcu_pedal_readings.get_accelerator_pedal_2() < MIN_ACCELERATOR_PEDAL_2) { -// mcu_status.set_no_accel_implausability(false); -// #if DEBUG -// Serial.println("T.4.2.10 2"); -// #endif -// } -// // check that the pedals are reading within 10% of each other -// // T.4.2.4 -// else if (fabs((mcu_pedal_readings.get_accelerator_pedal_1() - START_ACCELERATOR_PEDAL_1) / (END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1) - -// (mcu_pedal_readings.get_accelerator_pedal_2() - START_ACCELERATOR_PEDAL_2) / (END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2)) > 0.1) { -// #if DEBUG -// Serial.println("T.4.2.4"); -// Serial.printf("pedal 1 - %f\n", (mcu_pedal_readings.get_accelerator_pedal_1() - START_ACCELERATOR_PEDAL_1) / (END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1)); -// Serial.printf("pedal 2 - %f\n", (mcu_pedal_readings.get_accelerator_pedal_2() - START_ACCELERATOR_PEDAL_2) / (END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2)); -// #endif -// mcu_status.set_no_accel_implausability(false); -// } -// else { -// mcu_status.set_no_accel_implausability(true); -// } - -// // BSE check -// // EV.5.6 -// // FSAE T.4.3.4 -// if (mcu_pedal_readings.get_brake_pedal_1() < MIN_BRAKE_PEDAL_1 || mcu_pedal_readings.get_brake_pedal_1() > MAX_BRAKE_PEDAL_1) { -// mcu_status.set_no_brake_implausability(false); -// } -// else if (mcu_pedal_readings.get_brake_pedal_2() > MIN_BRAKE_PEDAL_2 || mcu_pedal_readings.get_brake_pedal_2() < MAX_BRAKE_PEDAL_2) { //negative slope for brake 2 -// mcu_status.set_no_brake_implausability(false); -// } else if (fabs((mcu_pedal_readings.get_brake_pedal_1() - START_BRAKE_PEDAL_1) / (END_BRAKE_PEDAL_1 - START_BRAKE_PEDAL_1) - -// (START_BRAKE_PEDAL_2 - mcu_pedal_readings.get_brake_pedal_2()) / (START_BRAKE_PEDAL_2 - END_BRAKE_PEDAL_2)) > 0.25) { -// mcu_status.set_no_brake_implausability(false); -// } -// else { -// mcu_status.set_no_brake_implausability(true); -// } - -// // FSAE EV.5.7 -// // APPS/Brake Pedal Plausability Check -// if ( -// ( -// (mcu_pedal_readings.get_accelerator_pedal_1() > ((END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1) / 4 + START_ACCELERATOR_PEDAL_1)) -// || -// (mcu_pedal_readings.get_accelerator_pedal_2() > ((END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2) / 4 + START_ACCELERATOR_PEDAL_2)) -// ) -// && mcu_status.get_mech_brake_active() -// ) -// { -// mcu_status.set_no_accel_brake_implausability(false); -// } -// else if -// ( -// (mcu_pedal_readings.get_accelerator_pedal_1() < ((END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1) / 20 + START_ACCELERATOR_PEDAL_1)) -// && -// (mcu_pedal_readings.get_accelerator_pedal_2() < ((END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2) / 20 + START_ACCELERATOR_PEDAL_2)) -// ) -// { -// mcu_status.set_no_accel_brake_implausability(true); -// } -// if (mcu_status.get_no_accel_implausability() && mcu_status.get_no_brake_implausability() && mcu_status.get_no_accel_brake_implausability()) { -// pedal_implausability_duration = 0; -// } -// } - -// inline float max_allowed_torque(float maxwatts, float rpm) { -// float angularspeed = (abs(rpm) + 1) / 60 * 2 * 3.1415; -// float maxnm = min(maxwatts / angularspeed, 20); -// return maxnm / 9.8 * 1000; -// } - -// void setup() { -// // no torque can be provided on startup - - -// mcu_status.set_max_torque(0); -// mcu_status.set_torque_mode(0); -// mcu_status.set_software_is_ok(true); - -// set_all_inverters_disabled(); - - -// // IMU set up -// IMU.regWrite(MSC_CTRL, 0xC1); // Enable Data Ready, set polarity -// delay(20); -// IMU.regWrite(FLTR_CTRL, 0x504); // Set digital filter -// delay(20); -// IMU.regWrite(DEC_RATE, 0), // Disable decimation -// delay(20); - -// pinMode(BRAKE_LIGHT_CTRL, OUTPUT); - -// // change to input if comparator is PUSH PULL -// pinMode(INVERTER_EN, OUTPUT); -// pinMode(INVERTER_24V_EN, OUTPUT); - -// pinMode(WATCHDOG_INPUT, OUTPUT); -// // the initial state of the watchdog is high -// // this is reflected in the static watchdog_state -// // starting high -// digitalWrite(WATCHDOG_INPUT, HIGH); -// pinMode(SOFTWARE_OK, OUTPUT); -// digitalWrite(SOFTWARE_OK, HIGH); - -// pinMode(ECU_CLK, OUTPUT); -// pinMode(ECU_SDI, INPUT); -// pinMode(ECU_SDO, OUTPUT); - -// #if DEBUG -// Serial.begin(115200); -// #endif -// FRONT_INV_CAN.begin(); -// FRONT_INV_CAN.setBaudRate(500000); -// REAR_INV_CAN.begin(); -// REAR_INV_CAN.setBaudRate(500000); -// TELEM_CAN.begin(); -// TELEM_CAN.setBaudRate(500000); -// FRONT_INV_CAN.enableMBInterrupts(); -// REAR_INV_CAN.enableMBInterrupts(); -// TELEM_CAN.enableMBInterrupts(); -// FRONT_INV_CAN.onReceive(parse_front_inv_can_message); -// REAR_INV_CAN.onReceive(parse_rear_inv_can_message); -// TELEM_CAN.onReceive(parse_telem_can_message); -// delay(500); - -// #if DEBUG -// Serial.println("CAN system and serial communication initialized"); -// #endif - - -// // these are false by default -// mcu_status.set_bms_ok_high(false); -// mcu_status.set_imd_ok_high(false); - -// digitalWrite(INVERTER_24V_EN, HIGH); -// digitalWrite(INVERTER_EN, HIGH); -// mcu_status.set_inverters_error(false); - - - -// // present action for 5s -// delay(5000); - -// set_state(MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); -// mcu_status.set_max_torque(TORQUE_3); -// mcu_status.set_torque_mode(3); + load_cell_alpha = 0.95; + total_torque = 4 * (avg_accel - avg_brake) ; + total_load_cells = mcu_load_cells.get_FL_load_cell() + mcu_load_cells.get_FR_load_cell() + mcu_load_cells.get_RL_load_cell() + mcu_load_cells.get_RR_load_cell(); + if (avg_accel >= avg_brake) { + torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque); + } else { + torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); + torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); + torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); + } + break; + } + default: + Serial.println("uh oh"); + break; + } + + + + + //very start check if mc_energy.get_feedback_torque > 0 + //power limit to 80kW + //look at all torques + //look at motor speeds / convert from rpm to angular speed + //torque * speed / 1000 (kW) + // scale down by m/e limits + //lots of variables for documentation purposes + //since torque unit to nominal torque and power conversion are linear, the diff can be applied directly to the torque setpoint value. + if (mc_setpoints_command[0].get_pos_torque_limit() > 0 && mc_setpoints_command[1].get_pos_torque_limit() > 0 + && mc_setpoints_command[2].get_pos_torque_limit() > 0 && mc_setpoints_command[3].get_pos_torque_limit() > 0) { + float mech_power = 0; + float mdiff = 1; + //float ediff = 1; + // float pw_lim_factor = 1.0; + + float voltage_lim_factor = 1.0; + float temp_lim_factor = 1.0; + float accu_lim_factor = 1.0; + + for (int i = 0; i < 4; i++) { + float torque_in_nm = 9.8 * ((float) mc_setpoints_command[i].get_pos_torque_limit()) / 1000.0; + float speed_in_rpm = (float) mc_status[i].get_speed(); + mech_power += 2 * 3.1415 * torque_in_nm * speed_in_rpm / 60.0; + } + + // pw_lim_factor = float_map(mech_power, 40000.0, 55000.0, 1.0, 0); + // pw_lim_factor = max(min(1.0, pw_lim_factor), 0.0); + + voltage_lim_factor = float_map(filtered_min_cell_voltage, 3.5, 3.2, 1.0, 0.2); + voltage_lim_factor = max(min(1.0, voltage_lim_factor), 0.2); + + temp_lim_factor = float_map(filtered_max_cell_temp, 50.0, 58.0, 1.0, 0.2); + temp_lim_factor = max(min(1.0, temp_lim_factor), 0.2); + + accu_lim_factor = min(temp_lim_factor, voltage_lim_factor); + //mech_power /= 1000.0; + + // float current = (ADC1.read_channel(ADC_CURRENT_CHANNEL) - ADC1.read_channel(ADC_REFERENCE_CHANNEL)); + // current = ((((current / 819.0) / .1912) / 4.832) - 2.5) * 1000) / 6.67; + + // + // float dc_power = (mc_energy[0].get_dc_bus_voltage() * current) / 1000; //mc dc bus voltage + + //sum up kilowatts to align + //if mech_power is at 63 kW, it's requesting 80 kW from the motor + //2 kW safety factor for the more accurate motor readings. + //as our effiecency increases say 68 kW would be drawing 80kW from the motor + //as our efficiency decreases say 60 kW would be drawing 80kW from the motor + //so if efficency is at 60kW and we want 63, we'd be drawing more from the battery triggering a safety problem + //so if efficency is at 68 kW, 63 would be drawing less power, which is fine but wasted power. + //if HV DC bus is over 80 kW, it's a violation! + // 1 kW as a second safety factor. + //if (mech_power > MECH_POWER_LIMIT) { + // mdiff = MECH_POWER_LIMIT / mech_power; + // diff = MECH_POWER_LIMIT / mech_power; + //} + // if (dc_power > DC_POWER_LIMIT) { + // ediff = DC_POWER_LIMIT / dc_power; + // } + // if (mech_power > MECH_POWER_LIMIT && dc_power > DC_POWER_LIMIT) { + // diff = (ediff <= mdiff) ? ediff : mdiff; + // } + torque_setpoint_array[0] = (uint16_t) (min((float) torque_setpoint_array[0], max_allowed_torque(max_front_power / 2.0, (float) mc_status[0].get_speed())) * accu_lim_factor); + torque_setpoint_array[1] = (uint16_t) (min((float) torque_setpoint_array[1], max_allowed_torque(max_front_power / 2.0, (float) mc_status[1].get_speed())) * accu_lim_factor); + torque_setpoint_array[2] = (uint16_t) (min((float) torque_setpoint_array[2], max_allowed_torque(max_rear_power / 2.0, (float) mc_status[2].get_speed())) * accu_lim_factor); + torque_setpoint_array[3] = (uint16_t) (min((float) torque_setpoint_array[3], max_allowed_torque(max_rear_power / 2.0, (float) mc_status[3].get_speed())) * accu_lim_factor); + } + + + int16_t max_speed_regen = 0; + for (int i = 0; i < sizeof(torque_setpoint_array); i++) { + + max_speed_regen = (max_speed_regen < mc_status[i].get_speed()) ? mc_status[i].get_speed() : max_speed_regen; + + } + + for (int i = 0; i < 4; i++) { + torque_setpoint_array[i] = max(-2140, min(2140, torque_setpoint_array[i])); + } + + for (int i = 0; i < 4; i++) { + if (torque_setpoint_array[i] >= 0) { + mc_setpoints_command[i].set_speed_setpoint(speed_setpoint_array[i]); + mc_setpoints_command[i].set_pos_torque_limit(min(torque_setpoint_array[i] , 2140)); + mc_setpoints_command[i].set_neg_torque_limit(0); + + } + else { + + float scale_down = 1; + if (max_speed_regen < 770) { + scale_down = 0; + } else if (max_speed_regen > REGEN_OFF_START_THRESHOLD) { + scale_down = 1; + } else { + scale_down = map(max_speed_regen, 770, REGEN_OFF_START_THRESHOLD, 0, 1); + } + mc_setpoints_command[i].set_speed_setpoint(0); + mc_setpoints_command[i].set_pos_torque_limit(0); + mc_setpoints_command[i].set_neg_torque_limit(max(((int16_t)(torque_setpoint_array[i]) * scale_down) , -2140) ); + + } + } + +} + +inline void read_all_adcs() { + if (timer_read_all_adcs.check()) { + + prev_load_cell_readings[0] = mcu_load_cells.get_FL_load_cell(); + prev_load_cell_readings[1] = mcu_load_cells.get_FR_load_cell(); + prev_load_cell_readings[2] = mcu_load_cells.get_RL_load_cell(); + prev_load_cell_readings[3] = mcu_load_cells.get_RR_load_cell(); + + uint16_t adc1_inputs[8]; + ADC1.read_all_channels(&adc1_inputs[0]); + mcu_pedal_readings.set_accelerator_pedal_1(adc1_inputs[ADC_ACCEL_1_CHANNEL]); + mcu_pedal_readings.set_accelerator_pedal_2(adc1_inputs[ADC_ACCEL_2_CHANNEL]); + mcu_pedal_readings.set_brake_pedal_1(adc1_inputs[ADC_BRAKE_1_CHANNEL]); + mcu_pedal_readings.set_brake_pedal_2(adc1_inputs[ADC_BRAKE_2_CHANNEL]); + mcu_analog_readings.set_steering_2(adc1_inputs[ADC_STEERING_2_CHANNEL]); + current_read = adc1_inputs[ADC_CURRENT_CHANNEL] - adc1_inputs[ADC_REFERENCE_CHANNEL]; + float current = ((((current_read / 819.0) / .1912) / 4.832) * 1000) / 6.67; + if (current > 300) { + current = 300; + } else if (current < -300) { + current = -300; + } + mcu_analog_readings.set_hall_effect_current((uint16_t)current * 100); + + + + mcu_load_cells.set_RL_load_cell((uint16_t)((adc1_inputs[ADC_RL_LOAD_CELL_CHANNEL]*LOAD_CELL3_SLOPE + LOAD_CELL3_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[2]*load_cell_alpha)); + + mcu_status.set_brake_pedal_active(mcu_pedal_readings.get_brake_pedal_1() >= BRAKE_ACTIVE); + digitalWrite(BRAKE_LIGHT_CTRL, mcu_status.get_brake_pedal_active()); + + mcu_status.set_mech_brake_active(mcu_pedal_readings.get_brake_pedal_1() >= BRAKE_THRESHOLD_MECH_BRAKE_1); //define in driver_constraints.h (70%) + + uint16_t adc2_inputs[8]; + ADC2.read_all_channels(&adc2_inputs[0]); + mcu_load_cells.set_RR_load_cell((uint16_t)((adc2_inputs[ADC_RR_LOAD_CELL_CHANNEL]*LOAD_CELL4_SLOPE + LOAD_CELL4_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[3]*load_cell_alpha)); + mcu_load_cells.set_FL_load_cell((uint16_t)((adc2_inputs[ADC_FL_LOAD_CELL_CHANNEL]*LOAD_CELL1_SLOPE + LOAD_CELL1_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[0]*load_cell_alpha)); + mcu_load_cells.set_FR_load_cell((uint16_t)((adc2_inputs[ADC_FR_LOAD_CELL_CHANNEL]*LOAD_CELL2_SLOPE + LOAD_CELL2_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[1]*load_cell_alpha)); + mcu_rear_potentiometers.set_pot4(adc2_inputs[SUS_POT_RL]); + mcu_rear_potentiometers.set_pot6(adc2_inputs[SUS_POT_RR]); + mcu_front_potentiometers.set_pot1(adc2_inputs[SUS_POT_FL]); + + uint16_t adc3_inputs[8]; + ADC3.read_all_channels(&adc3_inputs[0]); + mcu_analog_readings.set_glv_battery_voltage(adc3_inputs[ADC_GLV_READ_CHANNEL]); + mcu_front_potentiometers.set_pot3(adc3_inputs[SUS_POT_FR]); + } +} + +inline void read_steering_spi_values() { + if (timer_steering_spi_read.check()) { + mcu_analog_readings.set_steering_1(STEERING.read_steering()); + + } +} + +bool check_all_inverters_system_ready() { + for (uint8_t inv = 0; inv < 4; inv++) { + if (! mc_status[inv].get_system_ready()) { + return false; + } + } + return true; +} + +bool check_all_inverters_quit_dc_on() { + for (uint8_t inv = 0; inv < 4; inv++) { + if (! mc_status[inv].get_quit_dc_on()) { + return false; + } + } + return true; +} + +bool check_all_inverters_quit_inverter_on() { + for (uint8_t inv = 0; inv < 4; inv++) { + if (! mc_status[inv].get_quit_inverter_on()) { + return false; + } + } + return true; +} + +uint8_t check_all_inverters_error() { + uint8_t error_list = 0; //last 4 bits correspond to error bit in status word of each inverter, inverter 1 is rightmost bit; + for (uint8_t inv = 0; inv < 4; inv++) { + if (mc_status[inv].get_error()) { + error_list = error_list | (0x01 << inv); + } + } + if (error_list) { + mcu_status.set_inverters_error(true); + } else { + mcu_status.set_inverters_error(false); + } + return error_list; +} + +inline void set_all_inverters_no_torque() { + for (uint8_t inv = 0; inv < 4; inv++) { + mc_setpoints_command[inv].set_speed_setpoint(0); + mc_setpoints_command[inv].set_pos_torque_limit(0); + mc_setpoints_command[inv].set_neg_torque_limit(0); + } +} + +inline void set_all_inverters_torque_limit(int limit) { + + //const float max_torque = max_torque_Nm / 0.0098; // max possible value for torque multiplier, unit in 0.1% nominal torque + //inverter torque unit is in 0.1% of nominal torque (9.8Nm), max rated torque is 21Nm, so max possible output is 2142 + + if (limit >= 0) { + for (uint8_t inv = 0; inv < 4; inv++) { + mc_setpoints_command[inv].set_pos_torque_limit(limit); + mc_setpoints_command[inv].set_neg_torque_limit(0); + } + } + else { + for (uint8_t inv = 0; inv < 4; inv++) { + mc_setpoints_command[inv].set_pos_torque_limit(0); + mc_setpoints_command[inv].set_neg_torque_limit(limit); + } + } + +} + +inline void set_all_inverters_speed_setpoint(int setpoint) { + for (uint8_t inv = 0; inv < 4; inv++) { + mc_setpoints_command[inv].set_speed_setpoint(setpoint); + } +} + +inline void set_all_inverters_disabled() { + for (uint8_t inv = 0; inv < 4; inv++) { + mc_setpoints_command[inv].set_inverter_enable(false); + mc_setpoints_command[inv].set_hv_enable(false); + mc_setpoints_command[inv].set_driver_enable(false); + mc_setpoints_command[inv].set_remove_error(false); + mc_setpoints_command[inv].set_speed_setpoint(0); + mc_setpoints_command[inv].set_pos_torque_limit(0); + mc_setpoints_command[inv].set_neg_torque_limit(0); + } +} + +inline void set_all_inverters_dc_on(bool input) { + for (uint8_t inv = 0; inv < 4; inv++) { + mc_setpoints_command[inv].set_hv_enable(input); + } +} + +inline void set_all_inverters_driver_enable(bool input) { + for (uint8_t inv = 0; inv < 4; inv++) { + mc_setpoints_command[inv].set_driver_enable(input); + } +} + +inline void set_all_inverters_inverter_enable(bool input) { + for (uint8_t inv = 0; inv < 4; inv++) { + mc_setpoints_command[inv].set_inverter_enable(input); + } +} + +inline void reset_inverters() { + if (inverter_restart) { + uint8_t reset_bit = !mc_setpoints_command[0].get_remove_error(); + if (timer_reset_inverter.check()) { + inverter_restart = false; + reset_bit = 0; + } + for (uint8_t inv = 0; inv < 4; inv++) { + mc_setpoints_command[inv].set_remove_error(reset_bit); + } + } +} + +/* Read shutdown system values */ +inline void read_status_values() { + /* Measure shutdown circuits' input */ + mcu_status.set_bms_ok_high(digitalRead(BMS_OK_READ)); + mcu_status.set_imd_ok_high(digitalRead(IMD_OK_READ)); + mcu_status.set_bspd_ok_high(digitalRead(BSPD_OK_READ)); + mcu_status.set_software_ok_high(digitalRead(SOFTWARE_OK_READ)); + + /* Measure shutdown circuits' voltages */ + mcu_status.set_shutdown_b_above_threshold(digitalRead(BOTS_OK_READ)); + mcu_status.set_shutdown_c_above_threshold(digitalRead(IMD_OK_READ)); + mcu_status.set_shutdown_d_above_threshold(digitalRead(BMS_OK_READ)); + mcu_status.set_shutdown_e_above_threshold(digitalRead(BSPD_OK_READ)); +} + +// IMU functions +inline void read_imu() { + if (timer_read_imu.check()) { + double sinAngle = sin(VEHICLE_TILT_ANGLE_X); + double cosAngle = cos(VEHICLE_TILT_ANGLE_X); + double accel_x = IMU.regRead(X_ACCL_OUT) * 0.00245; // 0.00245 is the scale + double accel_y = IMU.regRead(Y_ACCL_OUT) * 0.00245; // 0.00245 is the scale + double accel_z = IMU.regRead(Z_ACCL_OUT) * 0.00245; // 0.00245 is the scale + double x_gyro = IMU.regRead(X_GYRO_OUT) * 0.005; // 0.005 is the scale + double y_gyro = IMU.regRead(Y_GYRO_OUT) * 0.005; // 0.005 is the scale + double z_gyro = IMU.regRead(Z_GYRO_OUT) * 0.005; // 0.005 is the scale + double long_accel = ((-accel_y) * cosAngle) + (accel_z * sinAngle); + double lat_accel = accel_x; + double vert_accel = -((accel_z * cosAngle) - (-accel_y * sinAngle)); + double pitch = (y_gyro * cosAngle) + (z_gyro * sinAngle); + double roll = y_gyro; + double yaw = (z_gyro * cosAngle) - (x_gyro * sinAngle); + imu_accelerometer.set_lat_accel((int16_t)(lat_accel * 1000)); // * 0.00245); // 0.00245 is the scale + imu_accelerometer.set_long_accel((int16_t)(long_accel * 1000)); // * 0.00245); // 0.00245 is the scale + imu_accelerometer.set_vert_accel((int16_t)(vert_accel * 1000)); // * 0.00245); // 0.00245 is the scale + // question about yaw, pitch and roll rates? + imu_gyroscope.set_pitch((int16_t)(pitch * 100)); // * 0.005); // 0.005 is the scale, + imu_gyroscope.set_yaw((int16_t)(yaw * 100 )); // * 0.005); // 0.005 is the scale + imu_gyroscope.set_roll((int16_t)(roll * 100)); // * 0.005); // 0.005 is the scale + } +} + +inline void send_CAN_imu_accelerometer() { + if (timer_CAN_imu_accelerometer_send.check()) { + imu_accelerometer.write(msg.buf); + msg.id = ID_IMU_ACCELEROMETER; + msg.len = sizeof(imu_accelerometer); + TELEM_CAN.write(msg); + } +} + +inline void send_CAN_imu_gyroscope() { + if (timer_CAN_imu_gyroscope_send.check()) { + imu_gyroscope.write(msg.buf); + msg.id = ID_IMU_GYROSCOPE; + msg.len = sizeof(imu_gyroscope); + TELEM_CAN.write(msg); + } +} + +inline void calibrate_imu_velocity(double calibrate_to) { + imu_velocity = calibrate_to; +} + +inline void pitch_angle_calibration() { + // double z_accl_sum = 0.0; + // int ctr = 0; + // time_t start_time, current_time; + // double elapsed_time; + // start_time = time(NULL); + // // Serial.println("Calibration Starts Now"); FOR DEBUGING PURPOSES + // while (1) { + // delay(50); + // z_accl_sum += ((IMU.regRead(Z_ACCL_OUT) * 0.00245)); + // ctr++; + // current_time = time(NULL); + // elapsed_time = difftime(current_time, start_time); + // if (elapsed_time >= 5) break; // Code runs for 5 seconds. Change for desired duration. + // } + // // Serial.println("Calibration Has Ended"); + // double avg_z_accl = z_accl_sum / ctr; + // pitch_calibration_angle = std::acos(avg_z_accl / ACCL_DUE_TO_GRAVITY); +} + +inline void calculate_pedal_implausibilities() { + // FSAE EV.5.5 + // FSAE T.4.2.10 + if (mcu_pedal_readings.get_accelerator_pedal_1() < MIN_ACCELERATOR_PEDAL_1 || mcu_pedal_readings.get_accelerator_pedal_1() > MAX_ACCELERATOR_PEDAL_1) { + mcu_status.set_no_accel_implausability(false); +#if DEBUG + Serial.println("T.4.2.10 1"); +#endif + } + else if (mcu_pedal_readings.get_accelerator_pedal_2() > MAX_ACCELERATOR_PEDAL_2 || mcu_pedal_readings.get_accelerator_pedal_2() < MIN_ACCELERATOR_PEDAL_2) { + mcu_status.set_no_accel_implausability(false); +#if DEBUG + Serial.println("T.4.2.10 2"); +#endif + } + // check that the pedals are reading within 10% of each other + // T.4.2.4 + else if (fabs((mcu_pedal_readings.get_accelerator_pedal_1() - START_ACCELERATOR_PEDAL_1) / (END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1) - + (mcu_pedal_readings.get_accelerator_pedal_2() - START_ACCELERATOR_PEDAL_2) / (END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2)) > 0.1) { +#if DEBUG + Serial.println("T.4.2.4"); + Serial.printf("pedal 1 - %f\n", (mcu_pedal_readings.get_accelerator_pedal_1() - START_ACCELERATOR_PEDAL_1) / (END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1)); + Serial.printf("pedal 2 - %f\n", (mcu_pedal_readings.get_accelerator_pedal_2() - START_ACCELERATOR_PEDAL_2) / (END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2)); +#endif + mcu_status.set_no_accel_implausability(false); + } + else { + mcu_status.set_no_accel_implausability(true); + } + + // BSE check + // EV.5.6 + // FSAE T.4.3.4 + if (mcu_pedal_readings.get_brake_pedal_1() < MIN_BRAKE_PEDAL_1 || mcu_pedal_readings.get_brake_pedal_1() > MAX_BRAKE_PEDAL_1) { + mcu_status.set_no_brake_implausability(false); + } + else if (mcu_pedal_readings.get_brake_pedal_2() > MIN_BRAKE_PEDAL_2 || mcu_pedal_readings.get_brake_pedal_2() < MAX_BRAKE_PEDAL_2) { //negative slope for brake 2 + mcu_status.set_no_brake_implausability(false); + } else if (fabs((mcu_pedal_readings.get_brake_pedal_1() - START_BRAKE_PEDAL_1) / (END_BRAKE_PEDAL_1 - START_BRAKE_PEDAL_1) - + (START_BRAKE_PEDAL_2 - mcu_pedal_readings.get_brake_pedal_2()) / (START_BRAKE_PEDAL_2 - END_BRAKE_PEDAL_2)) > 0.25) { + mcu_status.set_no_brake_implausability(false); + } + else { + mcu_status.set_no_brake_implausability(true); + } + + // FSAE EV.5.7 + // APPS/Brake Pedal Plausability Check + if ( + ( + (mcu_pedal_readings.get_accelerator_pedal_1() > ((END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1) / 4 + START_ACCELERATOR_PEDAL_1)) + || + (mcu_pedal_readings.get_accelerator_pedal_2() > ((END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2) / 4 + START_ACCELERATOR_PEDAL_2)) + ) + && mcu_status.get_mech_brake_active() + ) + { + mcu_status.set_no_accel_brake_implausability(false); + } + else if + ( + (mcu_pedal_readings.get_accelerator_pedal_1() < ((END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1) / 20 + START_ACCELERATOR_PEDAL_1)) + && + (mcu_pedal_readings.get_accelerator_pedal_2() < ((END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2) / 20 + START_ACCELERATOR_PEDAL_2)) + ) + { + mcu_status.set_no_accel_brake_implausability(true); + } + if (mcu_status.get_no_accel_implausability() && mcu_status.get_no_brake_implausability() && mcu_status.get_no_accel_brake_implausability()) { + pedal_implausability_duration = 0; + } +} + +inline float max_allowed_torque(float maxwatts, float rpm) { + float angularspeed = (abs(rpm) + 1) / 60 * 2 * 3.1415; + float maxnm = min(maxwatts / angularspeed, 20); + return maxnm / 9.8 * 1000; +} + +void setup() { + // no torque can be provided on startup + + + mcu_status.set_max_torque(0); + mcu_status.set_torque_mode(0); + mcu_status.set_software_is_ok(true); + + set_all_inverters_disabled(); + + + // IMU set up + IMU.regWrite(MSC_CTRL, 0xC1); // Enable Data Ready, set polarity + delay(20); + IMU.regWrite(FLTR_CTRL, 0x504); // Set digital filter + delay(20); + IMU.regWrite(DEC_RATE, 0), // Disable decimation + delay(20); + + pinMode(BRAKE_LIGHT_CTRL, OUTPUT); + + // change to input if comparator is PUSH PULL + pinMode(INVERTER_EN, OUTPUT); + pinMode(INVERTER_24V_EN, OUTPUT); + + pinMode(WATCHDOG_INPUT, OUTPUT); + // the initial state of the watchdog is high + // this is reflected in the static watchdog_state + // starting high + digitalWrite(WATCHDOG_INPUT, HIGH); + pinMode(SOFTWARE_OK, OUTPUT); + digitalWrite(SOFTWARE_OK, HIGH); + + pinMode(ECU_CLK, OUTPUT); + pinMode(ECU_SDI, INPUT); + pinMode(ECU_SDO, OUTPUT); + +#if DEBUG + Serial.begin(115200); +#endif + FRONT_INV_CAN.begin(); + FRONT_INV_CAN.setBaudRate(500000); + REAR_INV_CAN.begin(); + REAR_INV_CAN.setBaudRate(500000); + TELEM_CAN.begin(); + TELEM_CAN.setBaudRate(500000); + FRONT_INV_CAN.enableMBInterrupts(); + REAR_INV_CAN.enableMBInterrupts(); + TELEM_CAN.enableMBInterrupts(); + FRONT_INV_CAN.onReceive(parse_front_inv_can_message); + REAR_INV_CAN.onReceive(parse_rear_inv_can_message); + TELEM_CAN.onReceive(parse_telem_can_message); + delay(500); + +#if DEBUG + Serial.println("CAN system and serial communication initialized"); +#endif + + + // these are false by default + mcu_status.set_bms_ok_high(false); + mcu_status.set_imd_ok_high(false); + + digitalWrite(INVERTER_24V_EN, HIGH); + digitalWrite(INVERTER_EN, HIGH); + mcu_status.set_inverters_error(false); + + + + // present action for 5s + delay(5000); + + set_state(MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); + mcu_status.set_max_torque(TORQUE_3); + mcu_status.set_torque_mode(3); -// /* Set up total discharge readings */ -// //setup_total_discharge(); - -// } - -// void loop() { -// FRONT_INV_CAN.events(); -// REAR_INV_CAN.events(); -// TELEM_CAN.events(); - -// read_all_adcs(); -// //read_steering_spi_values(); -// read_status_values(); -// read_imu(); - -// send_CAN_mcu_status(); -// send_CAN_mcu_pedal_readings(); -// send_CAN_mcu_load_cells(); -// send_CAN_mcu_potentiometers(); -// send_CAN_mcu_analog_readings(); -// send_CAN_imu_accelerometer(); -// send_CAN_imu_gyroscope(); -// send_CAN_inverter_setpoints(); - -// // /* Finish restarting the inverter when timer expires */ -// check_all_inverters_error(); -// reset_inverters(); -// // -// // /* handle state functionality */ -// state_machine(); -// software_shutdown(); - - - -// if (timer_debug.check()) { -// Serial.println("ERROR"); -// Serial.println(check_all_inverters_error()); -// Serial.println(mc_energy[0].get_dc_bus_voltage()); -// Serial.println(mc_temps[0].get_diagnostic_number()); -// Serial.println(mc_temps[1].get_diagnostic_number()); -// Serial.println(mc_temps[2].get_diagnostic_number()); -// Serial.println(mc_temps[3].get_diagnostic_number()); -// Serial.println(); -// Serial.println(mcu_pedal_readings.get_accelerator_pedal_1()); -// Serial.println(mcu_pedal_readings.get_accelerator_pedal_2()); -// Serial.println(mcu_pedal_readings.get_brake_pedal_1()); -// Serial.println(mcu_pedal_readings.get_brake_pedal_2()); -// //calculate_pedal_implausibilities(); -// // Serial.println(mcu_status.get_no_accel_implausability()); -// // Serial.println(mcu_status.get_no_brake_implausability()); -// // Serial.println(mcu_status.get_no_accel_brake_implausability()); -// // int brake1 = map(round(mcu_pedal_readings.get_brake_pedal_1()), START_BRAKE_PEDAL_1, END_BRAKE_PEDAL_1, 0, 2140); -// // int brake2 = map(round(mcu_pedal_readings.get_brake_pedal_2()), START_BRAKE_PEDAL_2, END_BRAKE_PEDAL_2, 0, 2140); -// // Serial.println(brake1); -// // Serial.println(brake2); -// Serial.println(); -// Serial.println("LOAD CELLS"); -// Serial.println(mcu_load_cells.get_FL_load_cell()); -// Serial.println(mcu_load_cells.get_FR_load_cell()); -// Serial.println(mcu_load_cells.get_RL_load_cell()); -// Serial.println(mcu_load_cells.get_RR_load_cell()); -// Serial.println("SUS POTS"); -// Serial.println(mcu_front_potentiometers.get_pot1()); -// Serial.println(mcu_front_potentiometers.get_pot3()); -// Serial.println(mcu_rear_potentiometers.get_pot4()); -// Serial.println(mcu_rear_potentiometers.get_pot6()); + /* Set up total discharge readings */ + //setup_total_discharge(); + +} + +void loop() { + FRONT_INV_CAN.events(); + REAR_INV_CAN.events(); + TELEM_CAN.events(); + + read_all_adcs(); + //read_steering_spi_values(); + read_status_values(); + read_imu(); + + send_CAN_mcu_status(); + send_CAN_mcu_pedal_readings(); + send_CAN_mcu_load_cells(); + send_CAN_mcu_potentiometers(); + send_CAN_mcu_analog_readings(); + send_CAN_imu_accelerometer(); + send_CAN_imu_gyroscope(); + send_CAN_inverter_setpoints(); + + // /* Finish restarting the inverter when timer expires */ + check_all_inverters_error(); + reset_inverters(); + // + // /* handle state functionality */ + state_machine(); + software_shutdown(); + + + + if (timer_debug.check()) { + Serial.println("ERROR"); + Serial.println(check_all_inverters_error()); + Serial.println(mc_energy[0].get_dc_bus_voltage()); + Serial.println(mc_temps[0].get_diagnostic_number()); + Serial.println(mc_temps[1].get_diagnostic_number()); + Serial.println(mc_temps[2].get_diagnostic_number()); + Serial.println(mc_temps[3].get_diagnostic_number()); + Serial.println(); + Serial.println(mcu_pedal_readings.get_accelerator_pedal_1()); + Serial.println(mcu_pedal_readings.get_accelerator_pedal_2()); + Serial.println(mcu_pedal_readings.get_brake_pedal_1()); + Serial.println(mcu_pedal_readings.get_brake_pedal_2()); + //calculate_pedal_implausibilities(); +// Serial.println(mcu_status.get_no_accel_implausability()); +// Serial.println(mcu_status.get_no_brake_implausability()); +// Serial.println(mcu_status.get_no_accel_brake_implausability()); +// int brake1 = map(round(mcu_pedal_readings.get_brake_pedal_1()), START_BRAKE_PEDAL_1, END_BRAKE_PEDAL_1, 0, 2140); +// int brake2 = map(round(mcu_pedal_readings.get_brake_pedal_2()), START_BRAKE_PEDAL_2, END_BRAKE_PEDAL_2, 0, 2140); +// Serial.println(brake1); +// Serial.println(brake2); + Serial.println(); + Serial.println("LOAD CELLS"); + Serial.println(mcu_load_cells.get_FL_load_cell()); + Serial.println(mcu_load_cells.get_FR_load_cell()); + Serial.println(mcu_load_cells.get_RL_load_cell()); + Serial.println(mcu_load_cells.get_RR_load_cell()); + Serial.println("SUS POTS"); + Serial.println(mcu_front_potentiometers.get_pot1()); + Serial.println(mcu_front_potentiometers.get_pot3()); + Serial.println(mcu_rear_potentiometers.get_pot4()); + Serial.println(mcu_rear_potentiometers.get_pot6()); -// Serial.println(torque_setpoint_array[0]); -// Serial.println(torque_setpoint_array[1]); -// Serial.println(torque_setpoint_array[2]); -// Serial.println(torque_setpoint_array[3]); -// Serial.println("MOTOR TEMPS"); -// Serial.println(mc_temps[0].get_motor_temp()); -// Serial.println(mc_temps[1].get_motor_temp()); -// Serial.println(mc_temps[2].get_motor_temp()); -// Serial.println(mc_temps[3].get_motor_temp()); -// Serial.println(mc_temps[3].get_igbt_temp()); -// Serial.println("IMU"); -// Serial.println(imu_accelerometer.get_vert_accel()); -// Serial.println(imu_gyroscope.get_yaw()); -// Serial.println("dial"); -// Serial.println(dashboard_status.get_dial_state()); -// } - -// } + Serial.println(torque_setpoint_array[0]); + Serial.println(torque_setpoint_array[1]); + Serial.println(torque_setpoint_array[2]); + Serial.println(torque_setpoint_array[3]); + Serial.println("MOTOR TEMPS"); + Serial.println(mc_temps[0].get_motor_temp()); + Serial.println(mc_temps[1].get_motor_temp()); + Serial.println(mc_temps[2].get_motor_temp()); + Serial.println(mc_temps[3].get_motor_temp()); + Serial.println(mc_temps[3].get_igbt_temp()); + Serial.println("IMU"); + Serial.println(imu_accelerometer.get_vert_accel()); + Serial.println(imu_gyroscope.get_yaw()); + Serial.println("dial"); + Serial.println(dashboard_status.get_dial_state()); + } + +} diff --git a/test/drivetrain_system_test.h b/test/drivetrain_system_test.h index a9f82ff5e..64ee903b0 100644 --- a/test/drivetrain_system_test.h +++ b/test/drivetrain_system_test.h @@ -2,4 +2,6 @@ #define DRIVETRAIN_SYSTEM_TEST // TODO @ben +// TODO handle startup sequence stuff +// #endif /* DRIVETRAIN_SYSTEM_TEST */ From 5b5126125793a83f504c91942cae9d4d8abb2321 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 12:06:00 -0500 Subject: [PATCH 04/15] updating can message sending for inverter --- lib/interfaces/include/InverterInterface.tpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/interfaces/include/InverterInterface.tpp b/lib/interfaces/include/InverterInterface.tpp index f27ba40e8..e4cf846c0 100644 --- a/lib/interfaces/include/InverterInterface.tpp +++ b/lib/interfaces/include/InverterInterface.tpp @@ -7,9 +7,9 @@ void InverterInterface::write_cmd_msg_to_queue_(const MC_setpoint msg.id = can_id_; msg.len = sizeof(msg_in); uint8_t buf[sizeof(CAN_message_t)]; - memmove(buf, &msg, sizeof(msg_in)); msg_in.write(msg.buf); - msg_queue_->push_back(msg, sizeof(CAN_message_t)); + memmove(buf, &msg, sizeof(msg_in)); + msg_queue_->push_back(buf, sizeof(CAN_message_t)); } template From 71ededb9cdf10d31b32e46c75d2445fcfd81c650 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 12:25:03 -0500 Subject: [PATCH 05/15] started work on cleaning up pedals system and put the torque control mux in the state machine --- lib/interfaces/include/InverterInterface.tpp | 1 - lib/state_machine/include/MCUStateMachine.h | 11 +++++++---- lib/state_machine/include/MCUStateMachine.tpp | 9 ++------- lib/systems/include/PedalsSystem.h | 13 ++++++++++--- 4 files changed, 19 insertions(+), 15 deletions(-) diff --git a/lib/interfaces/include/InverterInterface.tpp b/lib/interfaces/include/InverterInterface.tpp index e4cf846c0..8c8c8e197 100644 --- a/lib/interfaces/include/InverterInterface.tpp +++ b/lib/interfaces/include/InverterInterface.tpp @@ -25,7 +25,6 @@ void InverterInterface::request_enable_hv() template void InverterInterface::request_enable_inverter() { - MC_setpoints_command mc_setpoints_command{}; mc_setpoints_command.set_speed_setpoint(0); mc_setpoints_command.set_pos_torque_limit(0); diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index 13652cb3f..101961ad3 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -27,13 +27,18 @@ template class MCUStateMachine { public: - MCUStateMachine(BuzzerController *buzzer, DrivetrainSysType *drivetrain, DashboardInterface *dashboard, PedalsSystem *pedals) + MCUStateMachine(BuzzerController *buzzer, + DrivetrainSysType *drivetrain, + DashboardInterface *dashboard, + PedalsSystem *pedals, + TorqueControllerMux *mux) { current_state_ = CAR_STATE::STARTUP; buzzer_ = buzzer; drivetrain_ = drivetrain; dashboard_ = dashboard; pedals_ = pedals; + controller_mux_ = mux; } /// @brief our components can use this time to tell when to do things. We can set this ourselves for testing purposes instead of using metro timers @@ -43,7 +48,6 @@ class MCUStateMachine CAR_STATE get_state() { return current_state_; } private: - void set_state_(CAR_STATE new_state, unsigned long curr_time); /// @brief the function run upon the entry of the car into a new state @@ -65,8 +69,7 @@ class MCUStateMachine AMSInterface *bms_; // IMDInterface *imd_; - TorqueControllerMux* controller_mux_; - + TorqueControllerMux *controller_mux_; }; #include "MCUStateMachine.tpp" #endif /* MCUSTATEMACHINE */ diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index 37d2b6a0a..c9c430654 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -74,7 +74,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren { auto data = pedals_->getPedalsSystemData(); - hal_println("RTD"); if (!drivetrain_->hv_over_threshold_on_drivetrain()) { hal_println("drivetrain not over thresh?"); @@ -87,18 +86,14 @@ void MCUStateMachine::tick_state_machine(unsigned long curren set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); break; } - // TODO migrate the handling of the pedals / move to the new pedals system - // PedalsDriverInterface data; - // auto pedals_data = pedals_->evaluate_pedals(data, current_millis); - // auto dashboard_data = dashboard_->evaluate_dashboard(dash_data); - // TODO: below in the scope of this function + // TODO:integrate the ams and the IMD if ( // bms_->ok_high() && // imd_->ok_high() && !data.implausibilityExceededMaxDuration) { - // drivetrain_->command_drivetrain(controller_mux_->get_drivetrain_input(pedals_data, dashboard_data)); + drivetrain_->command_drivetrain(controller_mux_->getDrivetrainCommand()); } else { diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h index 4a677b118..0c57af1e8 100644 --- a/lib/systems/include/PedalsSystem.h +++ b/lib/systems/include/PedalsSystem.h @@ -61,12 +61,17 @@ class PedalsSystem bool evaluate_brake_and_accel_pressed_(); bool pedal_is_active_(int sense_1, int sense_2, const PedalsSystemParameters_s &pedalParams, float percent_threshold); - + public: // Constructors - PedalsSystem(PedalsSystemParameters_s* parametersExt) + PedalsSystem(PedalsSystemParameters_s parametersExt) { - parameters_ = *parametersExt; + parameters_ = parametersExt; + } + + void update_parameters(const PedalsSystemParameters_s &new_params) + { + parameters_ = new_params; } PedalsSystem() { @@ -89,6 +94,8 @@ class PedalsSystem const AnalogConversion_s &brake1, const AnalogConversion_s &brake2 ); + + const PedalsSystemData_s& getPedalsSystemData() { return data_; From 60e07f52bc0aa8de4cd75ae49b92c3d1a665920b Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 12:28:36 -0500 Subject: [PATCH 06/15] fixing state machine test with tc mux --- test/state_machine_test.h | 49 +++++++++++++++++++++++---------------- 1 file changed, 29 insertions(+), 20 deletions(-) diff --git a/test/state_machine_test.h b/test/state_machine_test.h index 4ee1124db..8a98c8db2 100644 --- a/test/state_machine_test.h +++ b/test/state_machine_test.h @@ -63,7 +63,8 @@ TEST(MCUStateMachineTesting, test_state_machine_init_tick) DrivetrainMock drivetrain; PedalsSystem pedals; DashboardInterface dash_interface; - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + TorqueControllerMux tc_mux; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); unsigned long sys_time = 1000; EXPECT_EQ(state_machine.get_state(), CAR_STATE::STARTUP); state_machine.tick_state_machine(sys_time); @@ -76,8 +77,8 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_activation) DrivetrainMock drivetrain; PedalsSystem pedals; DashboardInterface dash_interface; - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); - + TorqueControllerMux tc_mux; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); unsigned long sys_time = 1000; // ticking without hv over threshold testing and ensuring the tractive system not active still @@ -109,7 +110,8 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) DrivetrainMock drivetrain; PedalsSystem pedals; DashboardInterface dash_interface; - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + TorqueControllerMux tc_mux; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); // ticking without hv over threshold testing and ensuring the tractive system not active still state_machine.tick_state_machine(sys_time); @@ -151,7 +153,8 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert) DrivetrainMock drivetrain; PedalsSystem pedals; DashboardInterface dash_interface; - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + TorqueControllerMux tc_mux; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); unsigned long sys_time = 1000; handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); @@ -166,7 +169,7 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert) state_machine.tick_state_machine(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); - sys_time +=35; + sys_time += 35; state_machine.tick_state_machine(sys_time); @@ -179,7 +182,9 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) DrivetrainMock drivetrain; PedalsSystem pedals; DashboardInterface dash_interface; - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + TorqueControllerMux tc_mux; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); + unsigned long sys_time = 1000; drivetrain.hv_thresh_ = true; @@ -188,10 +193,10 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) drivetrain.drivetrain_inited_ = true; state_machine.tick_state_machine(sys_time); - + buzzer.activate_buzzer(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); - sys_time+=20; + sys_time += 20; drivetrain.hv_thresh_ = false; state_machine.tick_state_machine(sys_time); @@ -199,8 +204,6 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } - - // state transitions out of RTD state TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_active) { @@ -210,19 +213,22 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_acti drivetrain.drivetrain_error_ = false; PedalsSystem pedals; DashboardInterface dash_interface; - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + TorqueControllerMux tc_mux; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); + + + unsigned long sys_time = 1000; handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); - - sys_time+=70; + sys_time += 70; drivetrain.drivetrain_inited_ = true; drivetrain.hv_thresh_ = true; state_machine.tick_state_machine(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); - sys_time+=70; + sys_time += 70; drivetrain.hv_thresh_ = true; state_machine.tick_state_machine(sys_time); @@ -248,19 +254,23 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_not_ drivetrain.drivetrain_error_ = false; PedalsSystem pedals; DashboardInterface dash_interface; - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals); + + TorqueControllerMux tc_mux; + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); + + + unsigned long sys_time = 1000; handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); - - sys_time+=70; + sys_time += 70; drivetrain.drivetrain_inited_ = true; drivetrain.hv_thresh_ = true; state_machine.tick_state_machine(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); - sys_time+=70; + sys_time += 70; drivetrain.hv_thresh_ = true; state_machine.tick_state_machine(sys_time); @@ -279,5 +289,4 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_not_ EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } - #endif /* STATE_MACHINE_TEST */ From aba88c6ab4e92e71c446ed9cc511c339d5b69bc1 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 13:50:19 -0500 Subject: [PATCH 07/15] going back for now --- lib/systems/include/PedalsSystem.h | 125 ++++++++++++-------------- lib/systems/include/SysClock.h | 4 +- lib/systems/src/PedalsSystem.cpp | 136 ++++++++++++----------------- 3 files changed, 112 insertions(+), 153 deletions(-) diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h index 0c57af1e8..f66644658 100644 --- a/lib/systems/include/PedalsSystem.h +++ b/lib/systems/include/PedalsSystem.h @@ -1,19 +1,8 @@ -#ifndef __PEDALSSYSTEM_H__ -#define __PEDALSSYSTEM_H__ +#ifndef PEDALSSYSTEM +#define PEDALSSYSTEM #include #include -#include "AnalogSensorsInterface.h" -#include - -// Definitions -const int PEDALS_IMPLAUSIBLE_DURATION = 100; // Implausibility must be caught within 100ms -const float PEDALS_IMPLAUSIBLE_PERCENT = 0.10; // 10% allowed deviation FSAE T.4.2.4 -const float PEDALS_MARGINAL_PERCENT = 0.07; // Report pedals are marginal. Allows us to detect pedals may need recalibration -const float PEDALS_RAW_TOO_LOW = 0.5 / 5 * 4096; // FSAE T.4.2.10 Pedals are implausible below 0.5V raw reading -const float PEDALS_RAW_TOO_HIGH = 4.5 / 5 * 4096; // FSAE T.4.2.10 Pedals are implausible above 4.5V raw reading - -// Enums enum class PedalsStatus_e { PEDALS_NOMINAL = 0, @@ -21,86 +10,82 @@ enum class PedalsStatus_e PEDALS_IMPLAUSIBLE = 2, }; -enum class PedalsCommanded_e +struct PedalsDriverInterface +{ + int accelPedalPosition1; + int accelPedalPosition2; + int brakePedalPosition1; + int brakePedalPosition2; +}; + +struct PedalsSystemInterface { - PEDALS_NONE_PRESSED = 0, - PEDALS_ACCEL_PRESSED = 1, - PEDALS_BRAKE_PRESSED = 2, - PEDALS_BOTH_PRESSED = 3, + bool accelImplausible; + bool brakeImplausible; + bool brakeAndAccelPressedImplausibility; + bool isBraking; + int requestedTorque; }; struct PedalsSystemData_s { - PedalsCommanded_e pedalsCommand; - PedalsStatus_e accelStatus; - PedalsStatus_e brakeStatus; + + bool accelImplausible; + bool brakeImplausible; bool implausibilityExceededMaxDuration; bool persistentImplausibilityDetected; float accelPercent; float brakePercent; }; -struct PedalsSystemParameters_s +/// @brief Pedals params struct that will hold min / max that will be used for evaluateion. +// NOTE: min and max may be need to be flipped depending on the sensor. (looking at you brake pedal sensor 2) +struct PedalsParams { - float pedalsImplausiblePercent; - float pedalsMarginalPercent; - float pedalsRawTooLow; - float pedalsRawTooHigh; - float accelPressedThreshold = 0.10; - float brakePressedThreshold = 0.05; + int min_sense_1; + int min_sense_2; + int max_sense_1; + int max_sense_2; + int start_sense_1; + int start_sense_2; + int end_sense_1; + int end_sense_2; }; class PedalsSystem { -private: -// Data - PedalsSystemParameters_s parameters_; - int implausibilityDetectedTime_; - PedalsSystemData_s data_; - - bool evaluate_brake_and_accel_pressed_(); - - bool pedal_is_active_(int sense_1, int sense_2, const PedalsSystemParameters_s &pedalParams, float percent_threshold); - public: -// Constructors - PedalsSystem(PedalsSystemParameters_s parametersExt) - { - parameters_ = parametersExt; - } + void tick( + const SysTick_s &sysClock, + const AnalogConversion_s &accel1, + const AnalogConversion_s &accel2, + const AnalogConversion_s &brake1, + const AnalogConversion_s &brake2); - void update_parameters(const PedalsSystemParameters_s &new_params) + const PedalsSystemData_s &getPedalsSystemData() { - parameters_ = new_params; + return data_; } PedalsSystem() { - parameters_.pedalsImplausiblePercent = PEDALS_IMPLAUSIBLE_PERCENT; - parameters_.pedalsMarginalPercent = PEDALS_MARGINAL_PERCENT; - parameters_.pedalsRawTooLow = PEDALS_RAW_TOO_LOW; - parameters_.pedalsRawTooHigh = PEDALS_RAW_TOO_HIGH; - parameters_.accelPressedThreshold = 0.1; - parameters_.brakePressedThreshold = 0.05; - - } + implausibilityStartTime_ = 0; + // Setting of min and maxes for pedals via config file + }; + PedalsSystemInterface evaluate_pedals( + const PedalsDriverInterface &pedal_data, unsigned long curr_time); + bool max_duration_of_implausibility_exceeded(unsigned long curr_time); + bool mech_brake_active(); -// Functions - - // bool max_duration_of_implausibility_exceeded(unsigned long t); - void tick( - const SysTick_s &sysClock, - const AnalogConversion_s &accel1, - const AnalogConversion_s &accel2, - const AnalogConversion_s &brake1, - const AnalogConversion_s &brake2 - ); +private: + std::tuple linearize_accel_pedal_values_(int accel1, int accel2); - - const PedalsSystemData_s& getPedalsSystemData() - { - return data_; - } -}; + bool evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams ¶ms, float max_percent_differnce); -#endif /* __PEDALSSYSTEM_H__ */ + bool evaluate_brake_and_accel_pressed_(const PedalsDriverInterface &data); + bool pedal_is_active_(int sense1, int sense_2, const PedalsParams &pedalParams, float percent_threshold); + PedalsParams accelParams_; + PedalsParams brakeParams_; + unsigned long implausibilityStartTime_; +}; +#endif /* PEDALSSYSTEM */ diff --git a/lib/systems/include/SysClock.h b/lib/systems/include/SysClock.h index 7cb2580c8..934e0e16c 100644 --- a/lib/systems/include/SysClock.h +++ b/lib/systems/include/SysClock.h @@ -14,8 +14,8 @@ struct TriggerBits_s struct SysTick_s { - long millis; - long micros; + unsigned long millis; + unsigned long micros; TriggerBits_s triggers; }; diff --git a/lib/systems/src/PedalsSystem.cpp b/lib/systems/src/PedalsSystem.cpp index a791ddd19..020e4b2d6 100644 --- a/lib/systems/src/PedalsSystem.cpp +++ b/lib/systems/src/PedalsSystem.cpp @@ -1,111 +1,85 @@ #include "PedalsSystem.h" -void PedalsSystem::tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2) +// TODO parameterize percentages in constructor +PedalsSystemInterface PedalsSystem::evaluate_pedals(const PedalsDriverInterface &data, unsigned long curr_time) { - float accelAverage = (accel1.conversion + accel2.conversion) / 2.0; - float brakeAverage = (brake1.conversion + brake2.conversion) / 2.0; + PedalsSystemInterface out; + out.accelImplausible = evaluate_pedal_implausibilities_(data.accelPedalPosition1, data.accelPedalPosition2, accelParams_, 0.1); + out.brakeImplausible = evaluate_pedal_implausibilities_(data.brakePedalPosition1, data.brakePedalPosition2, brakeParams_, 0.25); + out.brakeAndAccelPressedImplausibility = evaluate_brake_and_accel_pressed_(data); + bool implausibility = (out.brakeAndAccelPressedImplausibility || out.brakeImplausible || out.accelImplausible); - bool accelInRange1 = (accel1.raw < parameters_.pedalsRawTooHigh) && (accel1.raw > parameters_.pedalsRawTooLow); - bool accelInRange2 = (accel2.raw < parameters_.pedalsRawTooHigh) && (accel2.raw > parameters_.pedalsRawTooLow); - bool brakeInRange1 = (brake1.raw < parameters_.pedalsRawTooHigh) && (brake1.raw > parameters_.pedalsRawTooLow); - bool brakeInRange2 = (brake2.raw < parameters_.pedalsRawTooHigh) && (brake2.raw > parameters_.pedalsRawTooLow); - - if (accelInRange1 && accelInRange2) - data_.accelPercent = accelAverage; - else if (accelInRange1) - data_.accelPercent = accel1.conversion; - else if (accelInRange2) - data_.accelPercent = accel2.conversion; - else - data_.accelPercent = 0.0; - - if (brakeInRange1 && brakeInRange2) - data_.brakePercent = brakeAverage; - else if (brakeInRange1) - data_.brakePercent = brake1.conversion; - else if (brakeInRange2) - data_.brakePercent = brake2.conversion; - else - data_.brakePercent = 0.0; - - // Check instantaneous implausibility - // T.4.2.4 - // T.4.2.10 - float accelDeviation = abs(accel1.conversion - accel2.conversion) / accelAverage; - if ((accelDeviation >= parameters_.pedalsImplausiblePercent) || !accelInRange1 || !accelInRange2) + if (implausibility && (implausibilityStartTime_ == 0)) { - data_.accelStatus = PedalsStatus_e::PEDALS_IMPLAUSIBLE; + implausibilityStartTime_ = curr_time; } - else if (accelDeviation >= parameters_.pedalsMarginalPercent && accelDeviation < parameters_.pedalsImplausiblePercent) + else if (!implausibility) { - data_.accelStatus = PedalsStatus_e::PEDALS_MARGINAL; - } - else - { - data_.accelStatus = PedalsStatus_e::PEDALS_NOMINAL; + implausibilityStartTime_ = 0; } - float brakeDeviation = abs(brake1.conversion - brake2.conversion) / brakeAverage; - if ((brakeDeviation >= parameters_.pedalsImplausiblePercent) || !brakeInRange1 || !brakeInRange2) - { - data_.brakeStatus = PedalsStatus_e::PEDALS_IMPLAUSIBLE; - } - else if (brakeDeviation >= parameters_.pedalsMarginalPercent && brakeDeviation < parameters_.pedalsImplausiblePercent) + return out; +} + +// TODO parameterize duration in constructor +bool PedalsSystem::max_duration_of_implausibility_exceeded(unsigned long curr_time) +{ + if (implausibilityStartTime_ != 0) { - data_.brakeStatus = PedalsStatus_e::PEDALS_MARGINAL; + return ((curr_time - implausibilityStartTime_) > 100); } else { - data_.brakeStatus = PedalsStatus_e::PEDALS_NOMINAL; + return false; } +} - // Check if both pedals are pressed - bool accelPressed = data_.accelPercent > parameters_.accelPressedThreshold; - bool brakePressed = data_.brakePercent > parameters_.brakePressedThreshold; - if (accelPressed && brakePressed) - data_.pedalsCommand = PedalsCommanded_e::PEDALS_BOTH_PRESSED; - else if (accelPressed) - data_.pedalsCommand = PedalsCommanded_e::PEDALS_ACCEL_PRESSED; - else if (brakePressed) - data_.pedalsCommand = PedalsCommanded_e::PEDALS_BRAKE_PRESSED; - else - data_.pedalsCommand = PedalsCommanded_e::PEDALS_NONE_PRESSED; +bool PedalsSystem::evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams ¶ms, float max_percent_diff) +{ + // FSAE EV.5.5 + // FSAE T.4.2.10 + bool pedal_1_less_than_min = (sense_1 < params.min_sense_1); + bool pedal_2_less_than_min = (sense_2 < params.min_sense_2); + bool pedal_1_greater_than_max = (sense_1 > params.max_sense_1); + bool pedal_2_greater_than_max = (sense_2 > params.max_sense_2); - // Check for persistent implausibility - if ((data_.accelStatus == PedalsStatus_e::PEDALS_IMPLAUSIBLE) || (data_.brakeStatus == PedalsStatus_e::PEDALS_IMPLAUSIBLE)) + // check that the pedals are reading within 10% of each other + // T.4.2.4 + float scaled_pedal_1 = (sense_1 - params.start_sense_1) / (params.end_sense_1 - params.start_sense_1); + float scaled_pedal_2 = (sense_2 - params.start_sense_2) / (params.end_sense_2 - params.start_sense_2); + bool sens_not_within_req_percent = (fabs(scaled_pedal_1 - scaled_pedal_2) > max_percent_diff); + + if ( + pedal_1_less_than_min || + pedal_2_less_than_min || + pedal_1_greater_than_max || + pedal_2_greater_than_max) + { + return true; + } + else if (sens_not_within_req_percent) { - if (implausibilityDetectedTime_ == 0) - implausibilityDetectedTime_ = tick.millis; - if (tick.millis - implausibilityDetectedTime_ >= PEDALS_IMPLAUSIBLE_DURATION) - data_.persistentImplausibilityDetected = true; + return true; } else { - implausibilityDetectedTime_ = 0; - data_.persistentImplausibilityDetected = false; + return false; } } -// -// bool PedalsSystem::mech_brake_active() -// { -// return pedal_is_active_(); -// } - -bool PedalsSystem::evaluate_brake_and_accel_pressed_() +bool PedalsSystem::evaluate_brake_and_accel_pressed_(const PedalsDriverInterface &data) { - // bool accel_pressed = pedal_is_active_(data.accelPedalPosition1, data.accelPedalPosition2, accelParams_, 0.1); - // bool brake_pressed = pedal_is_active_(data.brakePedalPosition1, data.brakePedalPosition2, brakeParams_, 0.05); - - // return (accel_pressed && brake_pressed); + bool accel_pressed = pedal_is_active_(data.accelPedalPosition1, data.accelPedalPosition2, accelParams_, 0.1); + bool brake_pressed = pedal_is_active_(data.brakePedalPosition1, data.brakePedalPosition2, brakeParams_, 0.05); + return (accel_pressed && brake_pressed); } -bool PedalsSystem::pedal_is_active_(int sense_1, int sense_2, const PedalsSystemParameters_s &pedalParams, float percent_threshold) +bool PedalsSystem::pedal_is_active_(int sense_1, int sense_2, const PedalsParams &pedalParams, float percent_threshold) { - // bool pedal_1_is_active = (sense_1 > (((pedalParams.end_sense_1 - pedalParams.start_sense_1) * percent_threshold) + pedalParams.start_sense_1)); - // bool pedal_2_is_active = (sense_2 > (((pedalParams.end_sense_2 - pedalParams.start_sense_2) * percent_threshold) + pedalParams.start_sense_2)); + bool pedal_1_is_active = (sense_1 > (((pedalParams.end_sense_1 - pedalParams.start_sense_1) * percent_threshold) + pedalParams.start_sense_1)); + bool pedal_2_is_active = (sense_2 > (((pedalParams.end_sense_2 - pedalParams.start_sense_2) * percent_threshold) + pedalParams.start_sense_2)); - // return (pedal_1_is_active || pedal_2_is_active); -} + return (pedal_1_is_active || pedal_2_is_active); +} \ No newline at end of file From 008d9fcefbac0d89db79543afb13922f41aa90d4 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 14:45:15 -0500 Subject: [PATCH 08/15] current status of pedals --- lib/systems/include/PedalsSystem.h | 37 ++++++++++++++++-------------- lib/systems/src/PedalsSystem.cpp | 5 +++- 2 files changed, 24 insertions(+), 18 deletions(-) diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h index f66644658..5df26020d 100644 --- a/lib/systems/include/PedalsSystem.h +++ b/lib/systems/include/PedalsSystem.h @@ -2,6 +2,7 @@ #define PEDALSSYSTEM #include #include +#include "AnalogSensorsInterface.h" enum class PedalsStatus_e { @@ -10,22 +11,22 @@ enum class PedalsStatus_e PEDALS_IMPLAUSIBLE = 2, }; -struct PedalsDriverInterface -{ - int accelPedalPosition1; - int accelPedalPosition2; - int brakePedalPosition1; - int brakePedalPosition2; -}; +// struct PedalsDriverInterface +// { +// int accelPedalPosition1; +// int accelPedalPosition2; +// int brakePedalPosition1; +// int brakePedalPosition2; +// }; -struct PedalsSystemInterface -{ - bool accelImplausible; - bool brakeImplausible; - bool brakeAndAccelPressedImplausibility; - bool isBraking; - int requestedTorque; -}; +// struct PedalsSystemInterface +// { +// bool accelImplausible; +// bool brakeImplausible; +// bool brakeAndAccelPressedImplausibility; +// bool isBraking; +// int requestedTorque; +// }; struct PedalsSystemData_s { @@ -71,12 +72,14 @@ class PedalsSystem implausibilityStartTime_ = 0; // Setting of min and maxes for pedals via config file }; - PedalsSystemInterface evaluate_pedals( - const PedalsDriverInterface &pedal_data, unsigned long curr_time); + void tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2); + + PedalsSystemInterface evaluate_pedals(const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2, unsigned long curr_time); bool max_duration_of_implausibility_exceeded(unsigned long curr_time); bool mech_brake_active(); private: + PedalsSystemData_s data_; std::tuple linearize_accel_pedal_values_(int accel1, int accel2); bool evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams ¶ms, float max_percent_differnce); diff --git a/lib/systems/src/PedalsSystem.cpp b/lib/systems/src/PedalsSystem.cpp index 020e4b2d6..3f8b91332 100644 --- a/lib/systems/src/PedalsSystem.cpp +++ b/lib/systems/src/PedalsSystem.cpp @@ -1,7 +1,10 @@ #include "PedalsSystem.h" // TODO parameterize percentages in constructor -PedalsSystemInterface PedalsSystem::evaluate_pedals(const PedalsDriverInterface &data, unsigned long curr_time) +void PedalsSystem::tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2) +{ +} +PedalsSystemInterface PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2, unsigned long curr_time) { PedalsSystemInterface out; out.accelImplausible = evaluate_pedal_implausibilities_(data.accelPedalPosition1, data.accelPedalPosition2, accelParams_, 0.1); From 7ba8ce0d91030e126277e0266a9213cb0eeb4b70 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 16:22:03 -0500 Subject: [PATCH 09/15] updating pedals --- lib/state_machine/include/MCUStateMachine.tpp | 2 +- lib/systems/include/PedalsSystem.h | 62 +++++++------------ lib/systems/src/PedalsSystem.cpp | 59 +++++++++++------- lib/systems/src/TorqueControllers.cpp | 2 +- 4 files changed, 63 insertions(+), 62 deletions(-) diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index c9c430654..ddcb4eb31 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -28,7 +28,7 @@ void MCUStateMachine::tick_state_machine(unsigned long curren set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); break; } - if (dashboard_->startButtonPressed() && (data.pedalsCommand == PedalsCommanded_e::PEDALS_BRAKE_PRESSED)) + if (dashboard_->startButtonPressed() && (data.brakePressed)) { set_state_(CAR_STATE::ENABLING_INVERTERS, current_millis); break; diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h index 5df26020d..437d8b91d 100644 --- a/lib/systems/include/PedalsSystem.h +++ b/lib/systems/include/PedalsSystem.h @@ -11,30 +11,14 @@ enum class PedalsStatus_e PEDALS_IMPLAUSIBLE = 2, }; -// struct PedalsDriverInterface -// { -// int accelPedalPosition1; -// int accelPedalPosition2; -// int brakePedalPosition1; -// int brakePedalPosition2; -// }; - -// struct PedalsSystemInterface -// { -// bool accelImplausible; -// bool brakeImplausible; -// bool brakeAndAccelPressedImplausibility; -// bool isBraking; -// int requestedTorque; -// }; - struct PedalsSystemData_s { - + bool accelImplausible; bool brakeImplausible; + bool brakePressed; + bool brakeAndAccelPressedImplausibility; bool implausibilityExceededMaxDuration; - bool persistentImplausibilityDetected; float accelPercent; float brakePercent; }; @@ -47,22 +31,12 @@ struct PedalsParams int min_sense_2; int max_sense_1; int max_sense_2; - int start_sense_1; - int start_sense_2; - int end_sense_1; - int end_sense_2; + float activation_percentage; }; class PedalsSystem { public: - void tick( - const SysTick_s &sysClock, - const AnalogConversion_s &accel1, - const AnalogConversion_s &accel2, - const AnalogConversion_s &brake1, - const AnalogConversion_s &brake2); - const PedalsSystemData_s &getPedalsSystemData() { return data_; @@ -72,20 +46,32 @@ class PedalsSystem implausibilityStartTime_ = 0; // Setting of min and maxes for pedals via config file }; - void tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2); + void tick(const SysTick_s &tick, + const AnalogConversion_s &accel1, + const AnalogConversion_s &accel2, + const AnalogConversion_s &brake1, + const AnalogConversion_s &brake2); - PedalsSystemInterface evaluate_pedals(const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2, unsigned long curr_time); - bool max_duration_of_implausibility_exceeded(unsigned long curr_time); - bool mech_brake_active(); + PedalsSystemData_s evaluate_pedals(const AnalogConversion_s &accel1, + const AnalogConversion_s &accel2, + const AnalogConversion_s &brake1, + const AnalogConversion_s &brake2, + unsigned long curr_time); private: PedalsSystemData_s data_; - std::tuple linearize_accel_pedal_values_(int accel1, int accel2); - bool evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams ¶ms, float max_percent_differnce); + bool max_duration_of_implausibility_exceeded_(unsigned long curr_time); + bool evaluate_pedal_implausibilities_(const AnalogConversion_s &pedalData1, + const AnalogConversion_s &pedalData2, + const PedalsParams ¶ms, + float max_percent_diff); - bool evaluate_brake_and_accel_pressed_(const PedalsDriverInterface &data); - bool pedal_is_active_(int sense1, int sense_2, const PedalsParams &pedalParams, float percent_threshold); + bool evaluate_brake_and_accel_pressed_(const AnalogConversion_s &accelPedalData1, + const AnalogConversion_s &accelPedalData2, + const AnalogConversion_s &brakePedalData1, + const AnalogConversion_s &brakePedalData2); + bool pedal_is_active_(float pedal1ConvertedData, float pedal2ConvertedData, float percent_threshold); PedalsParams accelParams_; PedalsParams brakeParams_; unsigned long implausibilityStartTime_; diff --git a/lib/systems/src/PedalsSystem.cpp b/lib/systems/src/PedalsSystem.cpp index 3f8b91332..8f8ed6a4a 100644 --- a/lib/systems/src/PedalsSystem.cpp +++ b/lib/systems/src/PedalsSystem.cpp @@ -3,13 +3,18 @@ // TODO parameterize percentages in constructor void PedalsSystem::tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2) { + data_ = evaluate_pedals(accel1, accel2, brake1, brake2, tick.millis); } -PedalsSystemInterface PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2, unsigned long curr_time) +PedalsSystemData_s PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel1, + const AnalogConversion_s &accel2, + const AnalogConversion_s &brake1, + const AnalogConversion_s &brake2, + unsigned long curr_time) { - PedalsSystemInterface out; - out.accelImplausible = evaluate_pedal_implausibilities_(data.accelPedalPosition1, data.accelPedalPosition2, accelParams_, 0.1); - out.brakeImplausible = evaluate_pedal_implausibilities_(data.brakePedalPosition1, data.brakePedalPosition2, brakeParams_, 0.25); - out.brakeAndAccelPressedImplausibility = evaluate_brake_and_accel_pressed_(data); + PedalsSystemData_s out; + out.accelImplausible = evaluate_pedal_implausibilities_(accel1, accel2, accelParams_, 0.1); + out.brakeImplausible = evaluate_pedal_implausibilities_(brake1, brake2, brakeParams_, 0.25); + out.brakeAndAccelPressedImplausibility = evaluate_brake_and_accel_pressed_(accel1, accel2, brake1, brake2); bool implausibility = (out.brakeAndAccelPressedImplausibility || out.brakeImplausible || out.accelImplausible); if (implausibility && (implausibilityStartTime_ == 0)) @@ -20,12 +25,16 @@ PedalsSystemInterface PedalsSystem::evaluate_pedals(const AnalogConversion_s &ac { implausibilityStartTime_ = 0; } - + + out.accelPercent = (accel1.conversion + accel2.conversion) / 2.0; + out.brakePercent = (brake1.conversion + brake2.conversion) / 2.0; + out.brakePressed = pedal_is_active_(brake1.conversion, brake2.conversion, brakeParams_.activation_percentage); + out.implausibilityExceededMaxDuration = max_duration_of_implausibility_exceeded_(curr_time); return out; } // TODO parameterize duration in constructor -bool PedalsSystem::max_duration_of_implausibility_exceeded(unsigned long curr_time) +bool PedalsSystem::max_duration_of_implausibility_exceeded_(unsigned long curr_time) { if (implausibilityStartTime_ != 0) { @@ -37,21 +46,23 @@ bool PedalsSystem::max_duration_of_implausibility_exceeded(unsigned long curr_ti } } -bool PedalsSystem::evaluate_pedal_implausibilities_(int sense_1, int sense_2, const PedalsParams ¶ms, float max_percent_diff) +bool PedalsSystem::evaluate_pedal_implausibilities_(const AnalogConversion_s &pedalData1, + const AnalogConversion_s &pedalData2, + const PedalsParams ¶ms, + float max_percent_diff) { // FSAE EV.5.5 // FSAE T.4.2.10 - bool pedal_1_less_than_min = (sense_1 < params.min_sense_1); - bool pedal_2_less_than_min = (sense_2 < params.min_sense_2); - bool pedal_1_greater_than_max = (sense_1 > params.max_sense_1); - bool pedal_2_greater_than_max = (sense_2 > params.max_sense_2); + bool pedal_1_less_than_min = (pedalData1.raw < params.min_sense_1); + bool pedal_2_less_than_min = (pedalData2.raw < params.min_sense_2); + bool pedal_1_greater_than_max = (pedalData1.raw > params.max_sense_1); + bool pedal_2_greater_than_max = (pedalData2.raw > params.max_sense_2); // check that the pedals are reading within 10% of each other // T.4.2.4 - float scaled_pedal_1 = (sense_1 - params.start_sense_1) / (params.end_sense_1 - params.start_sense_1); - float scaled_pedal_2 = (sense_2 - params.start_sense_2) / (params.end_sense_2 - params.start_sense_2); - bool sens_not_within_req_percent = (fabs(scaled_pedal_1 - scaled_pedal_2) > max_percent_diff); + bool sens_not_within_req_percent = (fabs(pedalData1.conversion - pedalData2.conversion) > max_percent_diff); + bool pedalsClamped = (pedalData1.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED || pedalData2.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED); if ( pedal_1_less_than_min || pedal_2_less_than_min || @@ -60,7 +71,7 @@ bool PedalsSystem::evaluate_pedal_implausibilities_(int sense_1, int sense_2, co { return true; } - else if (sens_not_within_req_percent) + else if (sens_not_within_req_percent || pedalsClamped) { return true; } @@ -70,19 +81,23 @@ bool PedalsSystem::evaluate_pedal_implausibilities_(int sense_1, int sense_2, co } } -bool PedalsSystem::evaluate_brake_and_accel_pressed_(const PedalsDriverInterface &data) +bool PedalsSystem::evaluate_brake_and_accel_pressed_(const AnalogConversion_s &accelPedalData1, + const AnalogConversion_s &accelPedalData2, + const AnalogConversion_s &brakePedalData1, + const AnalogConversion_s &brakePedalData2 + ) { - bool accel_pressed = pedal_is_active_(data.accelPedalPosition1, data.accelPedalPosition2, accelParams_, 0.1); - bool brake_pressed = pedal_is_active_(data.brakePedalPosition1, data.brakePedalPosition2, brakeParams_, 0.05); + bool accel_pressed = pedal_is_active_(accelPedalData1.conversion, accelPedalData2.conversion, accelParams_.activation_percentage); // .1 + bool brake_pressed = pedal_is_active_(brakePedalData2.conversion, brakePedalData1.conversion, brakeParams_.activation_percentage); // 0.05 return (accel_pressed && brake_pressed); } -bool PedalsSystem::pedal_is_active_(int sense_1, int sense_2, const PedalsParams &pedalParams, float percent_threshold) +bool PedalsSystem::pedal_is_active_(float pedal1ConvertedData, float pedal2ConvertedData, float percent_threshold) { - bool pedal_1_is_active = (sense_1 > (((pedalParams.end_sense_1 - pedalParams.start_sense_1) * percent_threshold) + pedalParams.start_sense_1)); - bool pedal_2_is_active = (sense_2 > (((pedalParams.end_sense_2 - pedalParams.start_sense_2) * percent_threshold) + pedalParams.start_sense_2)); + bool pedal_1_is_active = pedal1ConvertedData >= percent_threshold; + bool pedal_2_is_active = pedal2ConvertedData >= percent_threshold; return (pedal_1_is_active || pedal_2_is_active); } \ No newline at end of file diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp index 0b4a12034..f58ec04a7 100644 --- a/lib/systems/src/TorqueControllers.cpp +++ b/lib/systems/src/TorqueControllers.cpp @@ -27,7 +27,7 @@ void TorqueControllerSimple::tick(const SysTick_s& tick, const PedalsSystemData_ // Calculate torque commands at 100hz if (tick.triggers.trigger100) { - if ((pedalsData.pedalsCommand != PedalsCommanded_e::PEDALS_BOTH_PRESSED) && (pedalsData.persistentImplausibilityDetected == false)) + if ((!pedalsData.brakeAndAccelPressedImplausibility) && (pedalsData.implausibilityExceededMaxDuration== false)) { // Both pedals are not pressed and no implausibility has been detected // accelRequest goes between 1.0 and -1.0 From 01764403d96701e1e077fcdb08f0d90891ea6de0 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 16:35:36 -0500 Subject: [PATCH 10/15] adding todos for pedal system testing --- lib/systems/src/PedalsSystem.cpp | 5 ++-- test/pedals_system_test.h | 47 +++++++++++++++++++------------- 2 files changed, 30 insertions(+), 22 deletions(-) diff --git a/lib/systems/src/PedalsSystem.cpp b/lib/systems/src/PedalsSystem.cpp index 8f8ed6a4a..a31901816 100644 --- a/lib/systems/src/PedalsSystem.cpp +++ b/lib/systems/src/PedalsSystem.cpp @@ -25,7 +25,7 @@ PedalsSystemData_s PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel { implausibilityStartTime_ = 0; } - + out.accelPercent = (accel1.conversion + accel2.conversion) / 2.0; out.brakePercent = (brake1.conversion + brake2.conversion) / 2.0; out.brakePressed = pedal_is_active_(brake1.conversion, brake2.conversion, brakeParams_.activation_percentage); @@ -84,8 +84,7 @@ bool PedalsSystem::evaluate_pedal_implausibilities_(const AnalogConversion_s &pe bool PedalsSystem::evaluate_brake_and_accel_pressed_(const AnalogConversion_s &accelPedalData1, const AnalogConversion_s &accelPedalData2, const AnalogConversion_s &brakePedalData1, - const AnalogConversion_s &brakePedalData2 - ) + const AnalogConversion_s &brakePedalData2) { bool accel_pressed = pedal_is_active_(accelPedalData1.conversion, accelPedalData2.conversion, accelParams_.activation_percentage); // .1 diff --git a/test/pedals_system_test.h b/test/pedals_system_test.h index 1ce1d37f9..cf3e3ce9b 100644 --- a/test/pedals_system_test.h +++ b/test/pedals_system_test.h @@ -15,31 +15,40 @@ author: Lucas Plant #include -// struct PedalIsActiveTestCase -// { -// // defines input and output params +struct PedalIsActiveTestCase +{ + // defines input and output params -// // input params -// PedalsDriverInterface pedalsinput; + // input params + PedalsDriverInterface pedalsinput; -// // expected output -// bool expect_active; -// }; + // expected output + bool expect_active; +}; +// TODO accel and brake implausibility for greater than max or less than min -// TEST(PedalsSystemTesting, test_pedal_is_active) -// { -// PedalsParams params_for_test = {1, 1, 10, 10, 1, 1, 9, 9}; -// PedalsSystem pedals_system(params_for_test, params_for_test); -// std::array test_cases{{{{0, 0, 3, 3}, true}}}; +// TODO accel and brake implausibility for values not within acceptable percentage differences -// for (const auto &test_case : test_cases) -// { -// auto out = pedals_system.mech_brake_active(test_case.pedalsinput); -// EXPECT_EQ(test_case.expect_active, out); -// } -// } +// TODO test accel and brake implausibility for both pressed at same time + +// TODO test accel and brake activation + +// TODO test implausibility duration alert + +TEST(PedalsSystemTesting, test_pedal_is_active) +{ + PedalsParams params_for_test = {1, 1, 10, 10, 1, 1, 9, 9}; + PedalsSystem pedals_system(params_for_test, params_for_test); + std::array test_cases{{{{0, 0, 3, 3}, true}}}; + + for (const auto &test_case : test_cases) + { + auto out = pedals_system.mech_brake_active(test_case.pedalsinput); + EXPECT_EQ(test_case.expect_active, out); + } +} From 40579f089603a52b93975a27ede19ef5503a5b6e Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 18:54:49 -0500 Subject: [PATCH 11/15] finished pedals system testing and re-integration --- lib/systems/include/PedalsSystem.h | 15 ++-- lib/systems/src/PedalsSystem.cpp | 3 +- src/main.cpp | 2 +- test/pedals_system_test.h | 107 ++++++++++++++++++++++------- test/state_machine_test.h | 14 ++-- 5 files changed, 97 insertions(+), 44 deletions(-) diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h index 437d8b91d..788106906 100644 --- a/lib/systems/include/PedalsSystem.h +++ b/lib/systems/include/PedalsSystem.h @@ -4,13 +4,7 @@ #include #include "AnalogSensorsInterface.h" -enum class PedalsStatus_e -{ - PEDALS_NOMINAL = 0, - PEDALS_MARGINAL = 1, - PEDALS_IMPLAUSIBLE = 2, -}; - +#include "SysClock.h" struct PedalsSystemData_s { @@ -41,11 +35,14 @@ class PedalsSystem { return data_; } - PedalsSystem() + PedalsSystem(const PedalsParams &accelParams, const PedalsParams &brakeParams) { - implausibilityStartTime_ = 0; + accelParams_ = accelParams; + brakeParams_ = brakeParams; + implausibilityStartTime_ = 0; // ms // Setting of min and maxes for pedals via config file }; + void tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, diff --git a/lib/systems/src/PedalsSystem.cpp b/lib/systems/src/PedalsSystem.cpp index a31901816..25b794476 100644 --- a/lib/systems/src/PedalsSystem.cpp +++ b/lib/systems/src/PedalsSystem.cpp @@ -1,5 +1,5 @@ #include "PedalsSystem.h" - +#include // TODO parameterize percentages in constructor void PedalsSystem::tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2) { @@ -69,6 +69,7 @@ bool PedalsSystem::evaluate_pedal_implausibilities_(const AnalogConversion_s &pe pedal_1_greater_than_max || pedal_2_greater_than_max) { + return true; } else if (sens_not_within_req_percent || pedalsClamped) diff --git a/src/main.cpp b/src/main.cpp index acf431905..64f9d3199 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -61,7 +61,7 @@ InverterInterfaceType rr_inv(&CAN2_txBuffer, ID_MC4_SETPOINTS_COMMAND, 9, 8); SysClock sys_clock; BuzzerController buzzer(BUZZER_ON_INTERVAL); SafetySystem safety_system(&ams_interface, &wd_interface); // Tie ams and wd interface to safety system (by pointers) -PedalsSystem pedals; +PedalsSystem pedals({100, 100, 3000, 3000, 0.1}, {100, 100, 3000, 3000, 0.05}); SteeringSystem steering_system(&steering1); // Unify member reference and pointers? tied by reference in this case using DrivetrainSystemType = DrivetrainSystem; auto drivetrain = DrivetrainSystemType({&fl_inv, &fr_inv, &rl_inv, &rr_inv}, INVERTER_ENABLING_TIMEOUT_INTERVAL); // Tie inverter interfaces to drivetrain system (by pointers) diff --git a/test/pedals_system_test.h b/test/pedals_system_test.h index cf3e3ce9b..c3b28fd30 100644 --- a/test/pedals_system_test.h +++ b/test/pedals_system_test.h @@ -14,43 +14,98 @@ author: Lucas Plant #include "PedalsSystem.h" #include - -struct PedalIsActiveTestCase +TEST(PedalsSystemTesting, test_accel_and_brake_limits_plausibility) { - // defines input and output params - - // input params - PedalsDriverInterface pedalsinput; - - // expected output - bool expect_active; -}; + // accel min and max on raw + AnalogConversion_s test_accel1_val = {0, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_accel2_val = {200, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_brake_val = {200, 0.01, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + PedalsSystem pedals({100, 100, 3000, 3000, 0.1}, {100, 100, 3000, 3000, 0.1}); + auto data = pedals.evaluate_pedals(test_accel1_val, test_accel2_val, test_brake_val, test_brake_val, 1000); + EXPECT_TRUE(data.accelImplausible); + data = pedals.evaluate_pedals(test_accel2_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + EXPECT_TRUE(data.accelImplausible); + test_accel1_val.raw = 4000; + data = pedals.evaluate_pedals(test_accel2_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + EXPECT_TRUE(data.accelImplausible); + test_accel1_val.raw = 300; + // brake min and max on raw + test_brake_val.raw = 0; + data = pedals.evaluate_pedals(test_accel2_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + EXPECT_TRUE(data.brakeImplausible); + test_brake_val.raw = 40000; + data = pedals.evaluate_pedals(test_accel2_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + EXPECT_TRUE(data.brakeImplausible); + + test_accel1_val.raw = 200; + test_brake_val.raw = 200; + + data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + + EXPECT_FALSE(data.brakeImplausible); + EXPECT_FALSE(data.accelImplausible); +} +TEST(PedalsSystemTesting, test_accel_and_brake_percentages_implausibility) +{ + // accel percentage outside of range + AnalogConversion_s test_accel1_val = {200, 0.0, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_accel2_val = {200, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_brake_val = {200, 0.01, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + PedalsSystem pedals({100, 100, 3000, 3000, 0.1}, {100, 100, 3000, 3000, 0.1}); + auto data = pedals.evaluate_pedals(test_accel2_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + + EXPECT_FALSE(data.brakeImplausible); + EXPECT_TRUE(data.accelImplausible); + + // brake percentage outside of range + test_brake_val.conversion = 0.5; + auto test_b2 = test_brake_val; + test_b2.conversion = 0.0; + data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_b2, test_brake_val, 1000); + EXPECT_TRUE(data.brakeImplausible); + EXPECT_FALSE(data.accelImplausible); + + data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + + EXPECT_FALSE(data.brakeImplausible); + EXPECT_FALSE(data.accelImplausible); +} -// TODO accel and brake implausibility for greater than max or less than min +TEST(PedalsSystemTesting, test_accel_and_brake_pressed_at_same_time_and_activation) +{ + AnalogConversion_s test_accel1_val = {1000, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_brake_val = {1000, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; -// TODO accel and brake implausibility for values not within acceptable percentage differences + PedalsSystem pedals({100, 100, 3000, 3000, 0.1}, {100, 100, 3000, 3000, 0.1}); + auto data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + EXPECT_TRUE(data.brakeAndAccelPressedImplausibility); + EXPECT_TRUE(data.brakePressed); -// TODO test accel and brake implausibility for both pressed at same time + test_brake_val.conversion = 0.0; -// TODO test accel and brake activation + data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1000); -// TODO test implausibility duration alert + EXPECT_FALSE(data.brakeAndAccelPressedImplausibility); + EXPECT_FALSE(data.brakePressed); +} -TEST(PedalsSystemTesting, test_pedal_is_active) +TEST(PedalsSystemTesting, test_implausibility_duration) { - PedalsParams params_for_test = {1, 1, 10, 10, 1, 1, 9, 9}; - PedalsSystem pedals_system(params_for_test, params_for_test); - std::array test_cases{{{{0, 0, 3, 3}, true}}}; - - for (const auto &test_case : test_cases) - { - auto out = pedals_system.mech_brake_active(test_case.pedalsinput); - EXPECT_EQ(test_case.expect_active, out); - } -} + AnalogConversion_s test_accel1_val = {1000, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_brake_val = {1000, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + PedalsSystem pedals({100, 100, 3000, 3000, 0.1}, {100, 100, 3000, 3000, 0.1}); + auto data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + EXPECT_TRUE(data.brakeAndAccelPressedImplausibility); + EXPECT_TRUE(data.brakePressed); + EXPECT_FALSE(data.implausibilityExceededMaxDuration); + data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1200); + EXPECT_TRUE(data.implausibilityExceededMaxDuration); +} +// TODO test implausibility duration alert + #endif /* PEDALS_SYSTEM_TEST */ diff --git a/test/state_machine_test.h b/test/state_machine_test.h index 8a98c8db2..393b2e8ab 100644 --- a/test/state_machine_test.h +++ b/test/state_machine_test.h @@ -61,7 +61,7 @@ TEST(MCUStateMachineTesting, test_state_machine_init_tick) { BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals; + PedalsSystem pedals({},{}); DashboardInterface dash_interface; TorqueControllerMux tc_mux; MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); @@ -75,7 +75,7 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_activation) { BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals; + PedalsSystem pedals({},{}); DashboardInterface dash_interface; TorqueControllerMux tc_mux; MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); @@ -108,7 +108,7 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals; + PedalsSystem pedals({},{}); DashboardInterface dash_interface; TorqueControllerMux tc_mux; MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); @@ -151,7 +151,7 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert) { BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals; + PedalsSystem pedals({},{}); DashboardInterface dash_interface; TorqueControllerMux tc_mux; MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); @@ -180,7 +180,7 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) { BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals; + PedalsSystem pedals({},{}); DashboardInterface dash_interface; TorqueControllerMux tc_mux; MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); @@ -211,7 +211,7 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_acti BuzzerController buzzer(50); DrivetrainMock drivetrain; drivetrain.drivetrain_error_ = false; - PedalsSystem pedals; + PedalsSystem pedals({},{}); DashboardInterface dash_interface; TorqueControllerMux tc_mux; MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux); @@ -252,7 +252,7 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_not_ BuzzerController buzzer(50); DrivetrainMock drivetrain; drivetrain.drivetrain_error_ = false; - PedalsSystem pedals; + PedalsSystem pedals({},{}); DashboardInterface dash_interface; TorqueControllerMux tc_mux; From 54eb7fe7db4f724f87f1a8bf0a639d792928b809 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 20:23:51 -0500 Subject: [PATCH 12/15] added todos for drivetrain system testing and mock inverter interface for use within --- lib/systems/include/DrivetrainSystem.h | 5 +- lib/systems/include/DrivetrainSystem.tpp | 11 ++-- lib/systems/include/TorqueControllers.h | 5 +- lib/systems/src/TorqueControllerMux.cpp | 13 ++--- lib/systems/src/TorqueControllers.cpp | 65 ++++++++++-------------- test/drivetrain_system_test.h | 53 ++++++++++++++++++- test/main.cpp | 2 +- test/pedals_system_test.h | 2 - 8 files changed, 94 insertions(+), 62 deletions(-) diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 2abf9da74..34e739cdf 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -6,9 +6,8 @@ #include "stdint.h" struct DrivetrainCommand_s { - float speeds[NUM_MOTORS]; - float posTorqueLimits[NUM_MOTORS]; - float negTorqueLimits[NUM_MOTORS]; + float speeds_rpm[NUM_MOTORS]; + float torqueSetpoints[NUM_MOTORS]; }; struct DrivetrainDynamicReport_s diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index a847cac2f..9a2403fef 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -78,11 +78,12 @@ void DrivetrainSystem::reset_drivetrain() template void DrivetrainSystem::command_drivetrain(const DrivetrainCommand_s &data) { - - // inverters_[0]->handle_command(data.left_front_inverter_cmd); - // inverters_[1]->handle_command(data.right_front_inverter_cmd); - // inverters_[2]->handle_command(data.left_rear_inverter_cmd); - // inverters_[3]->handle_command(data.right_rear_inverter_cmd); + int index = 0; + for (auto inv_pointer : inverters_) + { + inv_pointer->handle_command({data.torqueSetpoints[index], data.speeds_rpm[index]}); + index++; + } } // feedback functions diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 4b3587092..5484ec378 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -52,9 +52,8 @@ class TorqueControllerNone : public TorqueController { private: DrivetrainCommand_s data_ = { - .speeds = {0.0, 0.0, 0.0, 0.0}, - .posTorqueLimits = {0.0, 0.0, 0.0, 0.0}, - .negTorqueLimits = {0.0, 0.0, 0.0, 0.0} + .speeds_rpm = {0.0, 0.0, 0.0, 0.0}, + .torqueSetpoints= {0.0, 0.0, 0.0, 0.0} }; public: TorqueControllerNone(DrivetrainCommand_s& writeout) diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 35bd86b94..c46bb713a 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -53,15 +53,12 @@ void TorqueControllerMux::tick( bool torqueDeltaPreventsModeChange = false; for (int i = 0; i < NUM_MOTORS; i++) { - float posTorqueDelta = abs( - controllerCommands_[static_cast(muxMode_)].posTorqueLimits[i] - - controllerCommands_[static_cast(dialModeMap_[dashboardDialMode])].posTorqueLimits[i] + float torqueDelta = abs( + controllerCommands_[static_cast(muxMode_)].torqueSetpoints[i] + - controllerCommands_[static_cast(dialModeMap_[dashboardDialMode])].torqueSetpoints[i] ); - float negTorqueDelta = abs( - controllerCommands_[static_cast(muxMode_)].negTorqueLimits[i] - - controllerCommands_[static_cast(dialModeMap_[dashboardDialMode])].negTorqueLimits[i] - ); - if (posTorqueDelta > maxTorqueDeltaForModeChange || negTorqueDelta > maxTorqueDeltaForModeChange) + + if (torqueDelta > maxTorqueDeltaForModeChange) { torqueDeltaPreventsModeChange = true; break; diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp index f58ec04a7..b98664dae 100644 --- a/lib/systems/src/TorqueControllers.cpp +++ b/lib/systems/src/TorqueControllers.cpp @@ -16,7 +16,7 @@ static inline void TCPosTorqueLimit(DrivetrainCommand_s &command, float torqueLi { for (int i = 0; i < NUM_MOTORS; i++) { - command.posTorqueLimits[i] = std::min(command.posTorqueLimits[i], torqueLimit); + command.torqueSetpoints[i] = std::min(command.torqueSetpoints[i], torqueLimit); } } @@ -37,40 +37,31 @@ void TorqueControllerSimple::tick(const SysTick_s& tick, const PedalsSystemData_ if (accelRequest > 0.0) { // Positive torque request - data_.speeds[FL] = AMK_MAX_RPM; - data_.speeds[FR] = AMK_MAX_RPM; - data_.speeds[RL] = AMK_MAX_RPM; - data_.speeds[RR] = AMK_MAX_RPM; + data_.speeds_rpm[FL] = AMK_MAX_RPM; + data_.speeds_rpm[FR] = AMK_MAX_RPM; + data_.speeds_rpm[RL] = AMK_MAX_RPM; + data_.speeds_rpm[RR] = AMK_MAX_RPM; - data_.posTorqueLimits[FL] = torqueRequest * frontTorqueScale_; - data_.posTorqueLimits[FR] = torqueRequest * frontTorqueScale_; - data_.posTorqueLimits[RL] = torqueRequest * rearTorqueScale_; - data_.posTorqueLimits[RR] = torqueRequest * rearTorqueScale_; + data_.torqueSetpoints[FL] = torqueRequest * frontTorqueScale_; + data_.torqueSetpoints[FR] = torqueRequest * frontTorqueScale_; + data_.torqueSetpoints[RL] = torqueRequest * rearTorqueScale_; + data_.torqueSetpoints[RR] = torqueRequest * rearTorqueScale_; - data_.negTorqueLimits[FL] = 0.0; - data_.negTorqueLimits[FR] = 0.0; - data_.negTorqueLimits[RL] = 0.0; - data_.negTorqueLimits[RR] = 0.0; } else { // Negative torque request - torqueRequest = MAX_REGEN_TORQUE * torqueRequest * -1.0; // TODO: determine whether negative torque limits are signed + torqueRequest = MAX_REGEN_TORQUE * torqueRequest * -1.0; - data_.speeds[FL] = 0.0; - data_.speeds[FR] = 0.0; - data_.speeds[RL] = 0.0; - data_.speeds[RR] = 0.0; + data_.speeds_rpm[FL] = 0.0; + data_.speeds_rpm[FR] = 0.0; + data_.speeds_rpm[RL] = 0.0; + data_.speeds_rpm[RR] = 0.0; - data_.posTorqueLimits[FL] = 0.0; - data_.posTorqueLimits[FR] = 0.0; - data_.posTorqueLimits[RL] = 0.0; - data_.posTorqueLimits[RR] = 0.0; - - data_.negTorqueLimits[FL] = torqueRequest; - data_.negTorqueLimits[FR] = torqueRequest; - data_.negTorqueLimits[RL] = torqueRequest; - data_.negTorqueLimits[RR] = torqueRequest; + data_.torqueSetpoints[FL] = torqueRequest; + data_.torqueSetpoints[FR] = torqueRequest; + data_.torqueSetpoints[RL] = torqueRequest; + data_.torqueSetpoints[RR] = torqueRequest; } // Apply the torque limit @@ -80,20 +71,16 @@ void TorqueControllerSimple::tick(const SysTick_s& tick, const PedalsSystemData_ { // Both pedals are pressed or an implausibility has been detected // Zero out torque - data_.speeds[FL] = 0.0; - data_.speeds[FR] = 0.0; - data_.speeds[RL] = 0.0; - data_.speeds[RR] = 0.0; + data_.speeds_rpm[FL] = 0.0; + data_.speeds_rpm[FR] = 0.0; + data_.speeds_rpm[RL] = 0.0; + data_.speeds_rpm[RR] = 0.0; - data_.posTorqueLimits[FL] = 0.0; - data_.posTorqueLimits[FR] = 0.0; - data_.posTorqueLimits[RL] = 0.0; - data_.posTorqueLimits[RR] = 0.0; + data_.torqueSetpoints[FL] = 0.0; + data_.torqueSetpoints[FR] = 0.0; + data_.torqueSetpoints[RL] = 0.0; + data_.torqueSetpoints[RR] = 0.0; - data_.negTorqueLimits[FL] = 0.0; - data_.negTorqueLimits[FR] = 0.0; - data_.negTorqueLimits[RL] = 0.0; - data_.negTorqueLimits[RR] = 0.0; } writeout_ = data_; diff --git a/test/drivetrain_system_test.h b/test/drivetrain_system_test.h index 64ee903b0..c6064ac14 100644 --- a/test/drivetrain_system_test.h +++ b/test/drivetrain_system_test.h @@ -1,7 +1,58 @@ #ifndef DRIVETRAIN_SYSTEM_TEST #define DRIVETRAIN_SYSTEM_TEST +#include "DrivetrainSystem.h" // TODO @ben // TODO handle startup sequence stuff -// + +class InverterMock +{ +public: + int request_enable_hv_count_, request_enable_inverter_count_; + + InverterMock() + { + int request_enable_hv_count_ = 0; + int request_enable_inverter_count_ = 0; + error_ = false; + system_ready_ = false; + dc_quit_on_ = false; + quit_inverter_on_ = false; + }; + + bool error_, system_ready_, dc_quit_on_, quit_inverter_on_; + uint16_t voltage_; + + // want to ensure that we are only sending requests once at a time + // stage 1 + void request_enable_hv() + { + request_enable_hv_count_++; + dc_quit_on_ = true; + }; + // stage 2 + void request_enable_inverter() + { + request_enable_inverter_count_++; + quit_inverter_on_ = true; + }; + + void command_no_torque(){}; + bool error() { return error_; }; + bool inverter_system_ready() { return system_ready_; }; + void command_reset() { error_ = false; }; + uint16_t dc_bus_voltage() { return voltage_; }; + bool dc_quit_on() { return dc_quit_on_; } + bool quit_inverter_on() { return quit_inverter_on_; } +}; + +// TODO +TEST(DrivetrainSystemTesting, test_drivetrain_startup) +{ + InverterMock inv_fl, inv_fr, inv_rl, inv_rr; + DrivetrainSystem dt({&inv_fl, &inv_fr, &inv_rl, &inv_rr}, 1000); +} +// TODO test startup timeout + +// TODO test commanding of drivetrain to ensure that the data is getting accross correctly #endif /* DRIVETRAIN_SYSTEM_TEST */ diff --git a/test/main.cpp b/test/main.cpp index 2b93feab9..f2be5155c 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -2,7 +2,7 @@ #include "state_machine_test.h" #include "pedals_system_test.h" - +#include "drivetrain_system_test.h" int main(int argc, char **argv) diff --git a/test/pedals_system_test.h b/test/pedals_system_test.h index c3b28fd30..9566350b5 100644 --- a/test/pedals_system_test.h +++ b/test/pedals_system_test.h @@ -5,7 +5,6 @@ rough draft author: Lucas Plant */ -// TODO @ben #ifndef PEDALS_SYSTEM_TEST #define PEDALS_SYSTEM_TEST @@ -106,6 +105,5 @@ TEST(PedalsSystemTesting, test_implausibility_duration) EXPECT_TRUE(data.implausibilityExceededMaxDuration); } -// TODO test implausibility duration alert #endif /* PEDALS_SYSTEM_TEST */ From 0c049df7548010dfc32b262ef061c757ecf4c9f1 Mon Sep 17 00:00:00 2001 From: Dopp-IO <42275088+Dopp-IO@users.noreply.github.com> Date: Wed, 7 Feb 2024 20:29:40 -0500 Subject: [PATCH 13/15] data flow in superloop. test_env c/g++ versions --- platformio.ini | 2 ++ src/main.cpp | 40 +++++++++++++++++++++++++++++----------- 2 files changed, 31 insertions(+), 11 deletions(-) diff --git a/platformio.ini b/platformio.ini index 787b90005..f6e226371 100644 --- a/platformio.ini +++ b/platformio.ini @@ -4,6 +4,8 @@ test_framework = googletest build_src_filter = -<**/*.c> -<**/*.cpp> +build_unflags = -std=gnu++11 +build_flags = -std=c++17 lib_ignore = interfaces-lib diff --git a/src/main.cpp b/src/main.cpp index 64f9d3199..21810d354 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -61,7 +61,7 @@ InverterInterfaceType rr_inv(&CAN2_txBuffer, ID_MC4_SETPOINTS_COMMAND, 9, 8); SysClock sys_clock; BuzzerController buzzer(BUZZER_ON_INTERVAL); SafetySystem safety_system(&ams_interface, &wd_interface); // Tie ams and wd interface to safety system (by pointers) -PedalsSystem pedals({100, 100, 3000, 3000, 0.1}, {100, 100, 3000, 3000, 0.05}); +PedalsSystem pedals_system({100, 100, 3000, 3000, 0.1}, {100, 100, 3000, 3000, 0.05}); SteeringSystem steering_system(&steering1); // Unify member reference and pointers? tied by reference in this case using DrivetrainSystemType = DrivetrainSystem; auto drivetrain = DrivetrainSystemType({&fl_inv, &fr_inv, &rl_inv, &rr_inv}, INVERTER_ENABLING_TIMEOUT_INTERVAL); // Tie inverter interfaces to drivetrain system (by pointers) @@ -73,7 +73,7 @@ auto drivetrain = DrivetrainSystemType({&fl_inv, &fr_inv, &rl_inv, &rr_inv}, INV TorqueControllerSimple launch_control_mode; TorqueControllerSimple torque_vectoring_mode; */ -TorqueControllerMux torque_controller_mux; // would prob need to tie controllers to mux as well? +TorqueControllerMux torque_controller_mux; /* Declare state machine */ @@ -89,8 +89,8 @@ SysTick_s curr_tick; void update_and_enqueue_all_CAN(); /* External value readings */ void sample_all_external_readings(); -/* Process all readings */ -void process_all_value_readings(); +/* Tick all systems */ +void tick_all_systems(); void setup() { @@ -125,7 +125,7 @@ void loop() { sample_all_external_readings(); /* System process readings prior to ticking state machine */ - process_all_value_readings(); + tick_all_systems(); /* Update and enqueue CAN messages */ update_and_enqueue_all_CAN(); @@ -195,11 +195,29 @@ void sample_all_external_readings() { main_ecu.read_mcu_status(); } -void process_all_value_readings() { - pedals.tick(curr_tick, - ADC1.get().conversions[MCU15_ACCEL1_CHANNEL], - ADC1.get().conversions[MCU15_ACCEL2_CHANNEL], - ADC1.get().conversions[MCU15_BRAKE1_CHANNEL], - ADC1.get().conversions[MCU15_BRAKE2_CHANNEL]); +void tick_all_systems() { + pedals_system.tick( + curr_tick, + ADC1.get().conversions[MCU15_ACCEL1_CHANNEL], + ADC1.get().conversions[MCU15_ACCEL2_CHANNEL], + ADC1.get().conversions[MCU15_BRAKE1_CHANNEL], + ADC1.get().conversions[MCU15_BRAKE2_CHANNEL] + ); + steering_system.tick( + curr_tick, + ADC1.get().conversions[MCU15_STEERING_CHANNEL] + ); + torque_controller_mux.tick( + curr_tick, + (const DrivetrainDynamicReport_s) {}, // TODO: get drivetrain dynamic data + pedals_system.getPedalsSystemData(), + steering_system.getSteeringSystemData(), + ADC2.get().conversions[0], // FL load cell reading. TODO: fix index + ADC3.get().conversions[0], // FR load cell reading. TODO: fix index + (const AnalogConversion_s) {}, // RL load cell reading. TODO: get data from rear load cells + (const AnalogConversion_s) {}, // RR load cell reading. TODO: get data from rear load cells + dashboard.getDialMode(), + dashboard.torqueButtonPressed() + ); } From 39ee0e8dbb1d8303188386944ca9ba3bce5d7c01 Mon Sep 17 00:00:00 2001 From: Dopp-IO <42275088+Dopp-IO@users.noreply.github.com> Date: Wed, 7 Feb 2024 21:57:39 -0500 Subject: [PATCH 14/15] preliminary tcmux test --- lib/systems/include/SysClock.h | 2 +- lib/systems/include/TorqueControllerMux.h | 14 ++++-- lib/systems/src/SysClock.cpp | 35 ++++++++------ lib/systems/src/TorqueControllerMux.cpp | 6 +-- platformio.ini | 2 +- test/controller_mux_test.h | 5 -- test/main.cpp | 2 +- test/torque_controller_mux_test.h | 59 +++++++++++++++++++++++ 8 files changed, 94 insertions(+), 31 deletions(-) delete mode 100644 test/controller_mux_test.h create mode 100644 test/torque_controller_mux_test.h diff --git a/lib/systems/include/SysClock.h b/lib/systems/include/SysClock.h index 934e0e16c..877515ab7 100644 --- a/lib/systems/include/SysClock.h +++ b/lib/systems/include/SysClock.h @@ -22,7 +22,7 @@ struct SysTick_s class SysClock { private: - enum TriggerIndices_s + enum TriggerIndices_e { TRIG_1000 = 0, TRIG_500 = 1, diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 7bdf966b8..6033d6e26 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -12,7 +12,7 @@ const float maxSpeedForModeChange = 5.0; // m/s const float maxTorqueDeltaForModeChange = 0.5; // Nm -enum TorqueLimit_e +enum class TorqueLimit_e { TCMUX_LOW_TORQUE = 0, TCMUX_MID_TORQUE = 1, @@ -33,9 +33,9 @@ class TorqueControllerMux {DialMode_e::ENDURANCE, TC_NO_CONTROLLER}, }; std::unordered_map torqueLimitMap_ = { - {TCMUX_LOW_TORQUE, 10.0}, - {TCMUX_MID_TORQUE, 15.0}, - {TCMUX_FULL_TORQUE, 20.0} + {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0}, + {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0}, + {TorqueLimit_e::TCMUX_FULL_TORQUE, 20.0} }; DrivetrainCommand_s controllerCommands_[static_cast(TC_NUM_CONTROLLERS)]; TorqueControllerNone torqueControllerNone_; @@ -52,7 +52,7 @@ class TorqueControllerMux , torqueControllerSimple_(controllerCommands_[static_cast(TC_SAFE_MODE)]) { muxMode_ = TC_NO_CONTROLLER; - torqueLimit_ = TCMUX_LOW_TORQUE; + torqueLimit_ = TorqueLimit_e::TCMUX_LOW_TORQUE; } // Functions void tick( @@ -71,6 +71,10 @@ class TorqueControllerMux { return drivetrainCommand_; }; + const TorqueLimit_e& getTorqueLimit() + { + return torqueLimit_; + }; }; #endif /* __TORQUECTRLMUX_H__ */ diff --git a/lib/systems/src/SysClock.cpp b/lib/systems/src/SysClock.cpp index 2293f239d..82a4582b0 100644 --- a/lib/systems/src/SysClock.cpp +++ b/lib/systems/src/SysClock.cpp @@ -3,7 +3,10 @@ // Constructor SysClock::SysClock() { - + for (int i = 0; i < static_cast(TriggerIndices_e::NUM_TRIGGERS); i++) + { + triggerTimes[i] = 0; + } } SysTick_s SysClock::tick(unsigned long sysMicros) @@ -12,29 +15,31 @@ SysTick_s SysClock::tick(unsigned long sysMicros) .millis = sysMicros / 1000, .micros = sysMicros, .triggers = { - .trigger1000 = sysMicros > triggerTimes[TRIG_1000], - .trigger500 = sysMicros > triggerTimes[TRIG_500], - .trigger100 = sysMicros > triggerTimes[TRIG_100], - .trigger50 = sysMicros > triggerTimes[TRIG_50], - .trigger10 = sysMicros > triggerTimes[TRIG_10], - .trigger5 = sysMicros > triggerTimes[TRIG_5], - .trigger1 = sysMicros > triggerTimes[TRIG_1] + .trigger1000 = sysMicros > triggerTimes[static_cast(TriggerIndices_e::TRIG_1000)], + .trigger500 = sysMicros > triggerTimes[static_cast(TriggerIndices_e::TRIG_500)], + .trigger100 = sysMicros > triggerTimes[static_cast(TriggerIndices_e::TRIG_100)], + .trigger50 = sysMicros > triggerTimes[static_cast(TriggerIndices_e::TRIG_50)], + .trigger10 = sysMicros > triggerTimes[static_cast(TriggerIndices_e::TRIG_10)], + .trigger5 = sysMicros > triggerTimes[static_cast(TriggerIndices_e::TRIG_5)], + .trigger1 = sysMicros > triggerTimes[static_cast(TriggerIndices_e::TRIG_1)] } }; // Recalculate trigger times if (tickReturn.triggers.trigger1000) - triggerTimes[TRIG_1000] += 1'000; + triggerTimes[TriggerIndices_e::TRIG_1000] += 1'000; if (tickReturn.triggers.trigger500) - triggerTimes[TRIG_500] += 2'000; + triggerTimes[TriggerIndices_e::TRIG_500] += 2'000; if (tickReturn.triggers.trigger100) - triggerTimes[TRIG_100] += 10'000; + triggerTimes[TriggerIndices_e::TRIG_100] += 10'000; if (tickReturn.triggers.trigger50) - triggerTimes[TRIG_50] += 20'000; + triggerTimes[TriggerIndices_e::TRIG_50] += 20'000; if (tickReturn.triggers.trigger10) - triggerTimes[TRIG_10] += 100'000; + triggerTimes[TriggerIndices_e::TRIG_10] += 100'000; if (tickReturn.triggers.trigger5) - triggerTimes[TRIG_5] += 200'000; + triggerTimes[TriggerIndices_e::TRIG_5] += 200'000; if (tickReturn.triggers.trigger1) - triggerTimes[TRIG_1] += 1'000'000; + triggerTimes[TriggerIndices_e::TRIG_1] += 1'000'000; + + return tickReturn; } \ No newline at end of file diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 35bd86b94..a0db51b10 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -15,7 +15,7 @@ void TorqueControllerMux::tick( ) { // Tick all torque controllers - torqueControllerSimple_.tick(tick, pedalsData, torqueLimit_); + torqueControllerSimple_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_]); // Tick torque button logic at 50hz if (tick.triggers.trigger50) @@ -23,12 +23,12 @@ void TorqueControllerMux::tick( // detect high-to-low transition and lock out button presses for DEBOUNCE_MILLIS ms if ( torqueLimitButtonPressed_ - && dashboardTorqueModeButtonPressed + && !dashboardTorqueModeButtonPressed && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS ) { // WOW C++ is ass - torqueLimit_ = static_cast((static_cast(torqueLimit_) + 1) % static_cast(TCMUX_NUM_TORQUE_LIMITS)); + torqueLimit_ = static_cast((static_cast(torqueLimit_) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); torqueLimitButtonPressedTime_ = tick.millis; } torqueLimitButtonPressed_ = dashboardTorqueModeButtonPressed; diff --git a/platformio.ini b/platformio.ini index f6e226371..6024623ed 100644 --- a/platformio.ini +++ b/platformio.ini @@ -5,7 +5,7 @@ build_src_filter = -<**/*.c> -<**/*.cpp> build_unflags = -std=gnu++11 -build_flags = -std=c++17 +build_flags = -std=c++17 -g lib_ignore = interfaces-lib diff --git a/test/controller_mux_test.h b/test/controller_mux_test.h deleted file mode 100644 index 40a8ee3c5..000000000 --- a/test/controller_mux_test.h +++ /dev/null @@ -1,5 +0,0 @@ -#ifndef CONTROLLER_MUX_TEST -#define CONTROLLER_MUX_TEST - - -#endif /* CONTROLLER_MUX_TEST */ diff --git a/test/main.cpp b/test/main.cpp index 2b93feab9..708ea4135 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -2,7 +2,7 @@ #include "state_machine_test.h" #include "pedals_system_test.h" - +#include "torque_controller_mux_test.h" int main(int argc, char **argv) diff --git a/test/torque_controller_mux_test.h b/test/torque_controller_mux_test.h new file mode 100644 index 000000000..551b89656 --- /dev/null +++ b/test/torque_controller_mux_test.h @@ -0,0 +1,59 @@ +#ifndef TORQUE_CONTROLLER_MUX_TEST +#define TORQUE_CONTROLLER_MUX_TEST + +#include +#include +#include +#include +#include +#include +#include "AnalogSensorsInterface.h" +#include "DashboardInterface.h" + +TEST(TorqueControllerMuxTesting, test_torque_button) +{ + const int SERIES_LENGTH = 10; + SysClock clock = SysClock(); + SysTick_s cur_tick; + TorqueControllerMux torque_controller_mux = TorqueControllerMux(); + + bool buttonTimeSeries[SERIES_LENGTH]; // 50hz time series + // Cycle back to low test + buttonTimeSeries[1] = true; + buttonTimeSeries[3] = true; + buttonTimeSeries[5] = true; + // Cycle only on button release test + for (int i = 7; i < SERIES_LENGTH; i++) + buttonTimeSeries[i] = true; + + ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE); + + // Run test + for (int i = 0; i < SERIES_LENGTH; i++) + { + cur_tick = clock.tick(i * 1000 * 20); // 20 ms increments + torque_controller_mux.tick( + cur_tick, + (const DrivetrainDynamicReport_s) {}, + (const PedalsSystemData_s) {}, + (const SteeringSystemData_s) {}, + (const AnalogConversion_s) {}, + (const AnalogConversion_s) {}, + (const AnalogConversion_s) {}, + (const AnalogConversion_s) {}, + DialMode_e::MODE_1, + buttonTimeSeries[i] + ); + // Test conditions + if (i == 2) + ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_MID_TORQUE); + if (i == 4) + ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_FULL_TORQUE); + if (i == 6) + ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE); + if (i == SERIES_LENGTH - 1) + ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE); + } +} + +#endif /* TORQUE_CONTROLLER_MUX_TEST */ From 4b0038af3dc74873b0d09825732fa1b0fa00b86a Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 7 Feb 2024 22:49:30 -0500 Subject: [PATCH 15/15] added drivetrain system testing --- lib/systems/include/DrivetrainSystem.tpp | 4 +- test/drivetrain_system_test.h | 113 +++++++++++++++++++++-- 2 files changed, 110 insertions(+), 7 deletions(-) diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index 9a2403fef..42e90781b 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -13,11 +13,13 @@ bool DrivetrainSystem::handle_inverter_startup(unsigned long curr_ { enable_drivetrain_hv_(curr_time); hv_en_requested_ = true; + return false; } - else if (drivetrain_ready_() && check_drivetrain_quit_dc_on_() && !enable_requested_) + else if (drivetrain_ready_() && check_drivetrain_quit_dc_on_() && !enable_requested_ && hv_en_requested_) { request_enable_(); enable_requested_ = true; + return false; } bool all_ready = ( drivetrain_ready_() && check_drivetrain_quit_dc_on_() && drivetrain_enabled_() ); return all_ready; diff --git a/test/drivetrain_system_test.h b/test/drivetrain_system_test.h index c6064ac14..c5824d480 100644 --- a/test/drivetrain_system_test.h +++ b/test/drivetrain_system_test.h @@ -4,16 +4,22 @@ // TODO @ben // TODO handle startup sequence stuff - +struct InverterCommand +{ + float torque_setpoint_nm; + float speed_setpoint_rpm; +}; class InverterMock { public: int request_enable_hv_count_, request_enable_inverter_count_; - + float torque_setpoint_nm_; + float speed_setpoint_rpm_; InverterMock() { - int request_enable_hv_count_ = 0; - int request_enable_inverter_count_ = 0; + + request_enable_hv_count_ = 0; + request_enable_inverter_count_ = 0; error_ = false; system_ready_ = false; dc_quit_on_ = false; @@ -33,6 +39,7 @@ class InverterMock // stage 2 void request_enable_inverter() { + request_enable_inverter_count_++; quit_inverter_on_ = true; }; @@ -44,15 +51,109 @@ class InverterMock uint16_t dc_bus_voltage() { return voltage_; }; bool dc_quit_on() { return dc_quit_on_; } bool quit_inverter_on() { return quit_inverter_on_; } + void handle_command(const InverterCommand &cmd) + { + torque_setpoint_nm_ = cmd.torque_setpoint_nm; + speed_setpoint_rpm_ = cmd.speed_setpoint_rpm; + }; }; -// TODO TEST(DrivetrainSystemTesting, test_drivetrain_startup) { InverterMock inv_fl, inv_fr, inv_rl, inv_rr; DrivetrainSystem dt({&inv_fl, &inv_fr, &inv_rl, &inv_rr}, 1000); + unsigned long sys_time = 1000; + // inverters are not ready so an inverter startup call shouldnt send any requests to the inverter + // to start the initialization process + EXPECT_FALSE(dt.handle_inverter_startup(sys_time)); + EXPECT_EQ(inv_fl.request_enable_hv_count_, 0); + EXPECT_EQ(inv_fr.request_enable_hv_count_, 0); + EXPECT_EQ(inv_rl.request_enable_hv_count_, 0); + EXPECT_EQ(inv_rr.request_enable_hv_count_, 0); + + inv_fl.system_ready_ = true; + inv_fr.system_ready_ = true; + inv_rl.system_ready_ = true; + inv_rr.system_ready_ = true; + + // this should enable the drivetrain hv + EXPECT_FALSE(dt.handle_inverter_startup(sys_time)); + + EXPECT_TRUE(inv_fl.dc_quit_on_); + EXPECT_TRUE(inv_fr.dc_quit_on_); + EXPECT_TRUE(inv_rl.dc_quit_on_); + EXPECT_TRUE(inv_rr.dc_quit_on_); + + // im just gonna test one from now on for this type of stuff + EXPECT_EQ(inv_rr.request_enable_hv_count_, 1); + EXPECT_EQ(inv_rr.request_enable_inverter_count_, 0); // this should enable the inverters + EXPECT_FALSE(dt.handle_inverter_startup(sys_time)); + + EXPECT_EQ(inv_rr.request_enable_hv_count_, 1); + EXPECT_EQ(inv_rr.request_enable_inverter_count_, 1); + + EXPECT_TRUE(dt.handle_inverter_startup(sys_time)); } -// TODO test startup timeout +TEST(DrivetrainSystemTesting, test_drivetrain_init_timeout) +{ + InverterMock inv_fl, inv_fr, inv_rl, inv_rr; + DrivetrainSystem dt({&inv_fl, &inv_fr, &inv_rl, &inv_rr}, 1000); + unsigned long sys_time = 1000; + // test and make sure that normally the drivetrain doesnt timeout when changing the time + EXPECT_FALSE(dt.handle_inverter_startup(sys_time)); + inv_fl.system_ready_ = true; + inv_fr.system_ready_ = true; + inv_rl.system_ready_ = true; + inv_rr.system_ready_ = true; + sys_time += 10; + // tick to request hv enable + EXPECT_FALSE(dt.handle_inverter_startup(sys_time)); + + sys_time += 10; + // tick to request enable inverters + EXPECT_FALSE(dt.handle_inverter_startup(sys_time)); + + sys_time += 10; + // tick to check inverters status + EXPECT_TRUE(dt.handle_inverter_startup(sys_time)); + EXPECT_FALSE(dt.inverter_init_timeout(sys_time)); + + // test timeout + InverterMock inv_fl2, inv_fr2, inv_rl2, inv_rr2; + DrivetrainSystem dt2({&inv_fl2, &inv_fr2, &inv_rl2, &inv_rr2}, 1000); + unsigned long sys_time2 = 1000; + + EXPECT_FALSE(dt2.handle_inverter_startup(sys_time2)); + // inv_fl.system_ready_ = true; + inv_fr2.system_ready_ = true; + inv_rl2.system_ready_ = true; + inv_rr2.system_ready_ = true; + sys_time2 += 1050; + EXPECT_TRUE(dt.inverter_init_timeout(sys_time2)); +} + +TEST(DrivetrainSystemTesting, test_drivetrain_inverter_comms) +{ + + InverterMock inv_fl, inv_fr, inv_rl, inv_rr; + DrivetrainSystem dt({&inv_fl, &inv_fr, &inv_rl, &inv_rr}, 1000); + dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}}); + + // torque_setpoint_nm_ + // speed_setpoint_rpm_ + + EXPECT_EQ(inv_fl.speed_setpoint_rpm_, 1000.0); + EXPECT_EQ(inv_fl.torque_setpoint_nm_, 2000.0); + + EXPECT_EQ(inv_fr.torque_setpoint_nm_, 2001.0); + EXPECT_EQ(inv_fr.speed_setpoint_rpm_, 1001.0); + + EXPECT_EQ(inv_rl.torque_setpoint_nm_, 2002.0); + EXPECT_EQ(inv_rl.speed_setpoint_rpm_, 1002.0); + + EXPECT_EQ(inv_rr.torque_setpoint_nm_, 2003.0); + EXPECT_EQ(inv_rr.speed_setpoint_rpm_, 1003.0); +} // TODO test commanding of drivetrain to ensure that the data is getting accross correctly #endif /* DRIVETRAIN_SYSTEM_TEST */