From f13e030c3bf6b27aab68c036f5bb0eabbe5847ed Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 5 Feb 2024 17:48:19 -0500 Subject: [PATCH] building in the teensy environment --- lib/interfaces/src/DashboardInterface.cpp | 40 +++---- lib/state_machine/include/MCUStateMachine.h | 5 +- lib/state_machine/include/MCUStateMachine.tpp | 40 +++---- lib/systems/include/PedalsSystem.h | 2 + src/main.cpp | 101 +++--------------- 5 files changed, 57 insertions(+), 131 deletions(-) diff --git a/lib/interfaces/src/DashboardInterface.cpp b/lib/interfaces/src/DashboardInterface.cpp index 70deebc40..5cba1cdd2 100644 --- a/lib/interfaces/src/DashboardInterface.cpp +++ b/lib/interfaces/src/DashboardInterface.cpp @@ -4,20 +4,20 @@ void DashboardInterface::read(const CAN_message_t &can_msg) { DASHBOARD_STATE_t msg; - Unpack_DASHBOARD_STATE_ht_can(&msg, can_msg.buf, NULL); + Unpack_DASHBOARD_STATE_hytech(&msg, can_msg.buf, NULL); _data.dial_mode = static_cast(msg.dial_state); _data.ssok = msg.ssok_above_threshold; _data.shutdown = msg.shutdown_h_above_threshold; - _data.button.start = msg.start_btn; - _data.button.mark = msg.mark_btn; - _data.button.mode = msg.mode_btn; - _data.button.mc_cycle = msg.mc_cycle_btn; - _data.button.launch_ctrl = msg.launch_ctrl_btn; - _data.button.torque_mode = msg.torque_mode_btn; - _data.button.led_dimmer = msg.led_dimmer_btn; + // _data.button.start = msg.start_btn; + // _data.button.mark = msg.mark_btn; + // _data.button.mode = msg.mode_btn; + // _data.button.mc_cycle = msg.mc_cycle_btn; + // _data.button.launch_ctrl = msg.launch_ctrl_btn; + // _data.button.torque_mode = msg.torque_mode_btn; + // _data.button.led_dimmer = msg.led_dimmer_btn; _data.buzzer_state = msg.drive_buzzer; @@ -30,18 +30,18 @@ void DashboardInterface::write() msg.drive_buzzer = _data.buzzer_cmd; // TODO: use logic as to not write data for LEDs that have not changed - msg.bots_led = _data.LED[DashLED_e::BOTS_LED]; - msg.launch_control_led = _data.LED[DashLED_e::LAUNCH_CONTROL_LED]; - msg.mode_led = _data.LED[DashLED_e::MODE_LED]; - msg.mech_brake_led = _data.LED[DashLED_e::MECH_BRAKE_LED]; - msg.cockpit_brb_led = _data.LED[DashLED_e::COCKPIT_BRB_LED]; - msg.inertia_led = _data.LED[DashLED_e::INERTIA_LED]; - msg.glv_led = _data.LED[DashLED_e::GLV_LED]; - msg.crit_charge_led = _data.LED[DashLED_e::CRIT_CHARGE_LED]; - msg.start_led = _data.LED[DashLED_e::START_LED]; - msg.mc_error_led = _data.LED[DashLED_e::MC_ERROR_LED]; - msg.imd_led = _data.LED[DashLED_e::IMD_LED]; - msg.ams_led = _data.LED[DashLED_e::AMS_LED]; + // msg.bots_led = _data.LED[DashLED_e::BOTS_LED]; + // msg.launch_control_led = _data.LED[DashLED_e::LAUNCH_CONTROL_LED]; + // msg.mode_led = _data.LED[DashLED_e::MODE_LED]; + // msg.mech_brake_led = _data.LED[DashLED_e::MECH_BRAKE_LED]; + // msg.cockpit_brb_led = _data.LED[DashLED_e::COCKPIT_BRB_LED]; + // msg.inertia_led = _data.LED[DashLED_e::INERTIA_LED]; + // msg.glv_led = _data.LED[DashLED_e::GLV_LED]; + // msg.crit_charge_led = _data.LED[DashLED_e::CRIT_CHARGE_LED]; + // msg.start_led = _data.LED[DashLED_e::START_LED]; + // msg.mc_error_led = _data.LED[DashLED_e::MC_ERROR_LED]; + // msg.imd_led = _data.LED[DashLED_e::IMD_LED]; + // msg.ams_led = _data.LED[DashLED_e::AMS_LED]; CAN_message_t can_msg; can_msg.id = Pack_DASHBOARD_MCU_STATE_hytech(&msg, can_msg.buf, &can_msg.len, NULL); diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index 35d32a433..e78052cd1 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -39,7 +39,8 @@ class MCUStateMachine /// @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 /// @param current_millis the current millis() call - void tick_state_machine(const SysTick_s &tick); + // void tick_state_machine(const SysTick_s &tick); + void tick_state_machine(unsigned long cm); CAR_STATE get_state() { return current_state_; } private: @@ -62,7 +63,7 @@ class MCUStateMachine /// @brief drivers within state machine DashboardInterface *dashboard_; AMSInterface *bms_; - IMDInterface *imd_; + // IMDInterface *imd_; TorqueControllerMux* controller_mux_; diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index becc3e5a1..1709da4a5 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -22,17 +22,17 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::TRACTIVE_SYSTEM_ACTIVE: { // TODO migrate to new pedals system - PedalsDriverInterface data; - if (!drivetrain_->hv_over_threshold_on_drivetrain()) - { - set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - break; - } - if (dashboard_->start_button_pressed() && pedals_->mech_brake_active(data)) - { - set_state_(CAR_STATE::ENABLING_INVERTERS, current_millis); - break; - } + PedalsSystemData_s data; + // if (!drivetrain_->hv_over_threshold_on_drivetrain()) + // { + // set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + // break; + // } + // if (dashboard_->start_button_pressed() && pedals_->mech_brake_active(data)) + // { + // set_state_(CAR_STATE::ENABLING_INVERTERS, current_millis); + // break; + // } break; } @@ -118,14 +118,14 @@ void MCUStateMachine::tick_state_machine(unsigned long curren 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); + // 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 if ( - bms_->ok_high() && - imd_->ok_high() && + // bms_->ok_high() && + // imd_->ok_high() && !pedals_->max_duration_of_implausibility_exceeded(current_millis)) { // drivetrain_->command_drivetrain(controller_mux_->get_drivetrain_input(pedals_data, dashboard_data)); @@ -134,11 +134,11 @@ void MCUStateMachine::tick_state_machine(unsigned long curren { drivetrain_->command_drivetrain_no_torque(); hal_println("not calculating torque"); - hal_printf("no brake implausibility: %d\n", pedals_data.brakeImplausible); - hal_printf("no accel implausibility: %d\n", pedals_data.accelImplausible); - hal_printf("bms heartbeat: %d\n", bms_->heartbeat_check(current_millis)); - hal_printf("get bms ok high: %d\n", bms_->ok_high()); - hal_printf("get imd ok high: %d\n", imd_->ok_high()); + // hal_printf("no brake implausibility: %d\n", pedals_data.brakeImplausible); + // hal_printf("no accel implausibility: %d\n", pedals_data.accelImplausible); + // hal_printf("bms heartbeat: %d\n", bms_->heartbeat_check(current_millis)); + // hal_printf("get bms ok high: %d\n", bms_->ok_high()); + // hal_printf("get imd ok high: %d\n", imd_->ok_high()); } break; diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h index aa9105a7b..d6ea839cd 100644 --- a/lib/systems/include/PedalsSystem.h +++ b/lib/systems/include/PedalsSystem.h @@ -78,6 +78,8 @@ class PedalsSystem } // Functions + + bool max_duration_of_implausibility_exceeded(unsigned long t); void tick( const SysTick_s &sysClock, const AnalogConversion_s &accel1, diff --git a/src/main.cpp b/src/main.cpp index dbd02cf9b..2a2d62a8e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -46,7 +46,7 @@ MCP3208 ADC3(ADC3_CS); OrbisBR10 steering1(STEERING_SERIAL); /* Declare interfaces */ -DashboardInterface dashboard; +// DashboardInterface dashboard; AMSInterface ams_interface(SOFTWARE_OK); WatchdogInterface wd_interface(WATCHDOG_INPUT); MCUInterface main_ecu(&CAN3_txBuffer); @@ -61,29 +61,24 @@ 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; -SteeringSystem steering_system(steering1); // Unify member reference and pointers? tied by reference in this case +// 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) // Hypothetical controllers, need more implementation details -TorqueControllerSimple simple_mode; -TorqueControllerSimple normal_force_mode; -TorqueControllerSimple endurance_derating_mode; -TorqueControllerSimple launch_control_mode; -TorqueControllerSimple torque_vectoring_mode; +// TorqueControllerSimple simple_mode; +// TorqueControllerSimple normal_force_mode; +// TorqueControllerSimple endurance_derating_mode; +// TorqueControllerSimple launch_control_mode; +// TorqueControllerSimple torque_vectoring_mode; TorqueControllerMux torque_controller_mux; // would prob need to tie controllers to mux as well? /* Declare state machine */ -MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard); // need more implemetation details. associated interfaces and systems tied by pointers +// MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard); // need more implemetation details. associated interfaces and systems tied by pointers /* Global instantiations */ SysTick_s curr_tick; /* Function declarations */ /* CAN functions */ -void init_all_CAN(); -void dispatch_all_CAN(); -void dispatch_inv_CAN(); -void dispatch_telem_CAN(); -void update_and_enqueue_all_CAN(); /* External value readings */ void sample_all_external_readings(); /* Process all readings */ @@ -94,11 +89,8 @@ void setup() { /* Tick system clock */ curr_tick = sys_clock.tick(micros()); - /* Initialize CAN communication */ - init_all_CAN(); - /* Initialize interface */ - main_ecu.init(); // pin mode, initial shutdown circuit readings, + // main_ecu.init(); // pin mode, initial shutdown circuit readings, wd_interface.init(curr_tick); // pin mode, initialize wd kick time ams_interface.init(curr_tick); // pin mode, initialize last heartbeat time @@ -121,9 +113,6 @@ void loop() { /* Tick system clock */ curr_tick = sys_clock.tick(micros()); - /* Interfaaces retrieve values */ - // Distribute incoming CAN messages - dispatch_all_CAN(); // Sensors sample and cache sample_all_external_readings(); @@ -131,14 +120,14 @@ void loop() { process_all_value_readings(); /* Update and enqueue CAN messages */ - update_and_enqueue_all_CAN(); + // update_and_enqueue_all_CAN(); /* Inverter procedure before entering state machine */ // Drivetrain check if inverters have error // Drivetrain reset inverters /* Tick state machine */ - fsm.tick_state_machine(curr_tick); + // fsm.tick_state_machine(curr_tick); /* Tick safety system */ safety_system.software_shutdown(curr_tick); @@ -166,72 +155,6 @@ void init_all_CAN() { delay(500); } -void dispatch_all_CAN() { - dispatch_inv_CAN(); - dispatch_telem_CAN(); -} - -void dispatch_inv_CAN() { - while (CAN2_rxBuffer.available()) - { - CAN_message_t recvd_msg; - uint8_t buf[sizeof(CAN_message_t)]; - CAN2_rxBuffer.pop_front(buf, sizeof(CAN_message_t)); - memmove(&recvd_msg, buf, sizeof(recvd_msg)); - switch (recvd_msg.id) - { - // Motor status - case ID_MC1_STATUS: fl_inv.receive_status_msg(recvd_msg); break; - case ID_MC2_STATUS: fr_inv.receive_status_msg(recvd_msg); break; - case ID_MC3_STATUS: rl_inv.receive_status_msg(recvd_msg); break; - case ID_MC3_STATUS: rr_inv.receive_status_msg(recvd_msg); break; - // Motor temperature - case ID_MC1_TEMPS: fl_inv.receive_temp_msg(recvd_msg); break; - case ID_MC2_TEMPS: fr_inv.receive_temp_msg(recvd_msg); break; - case ID_MC3_TEMPS: rl_inv.receive_temp_msg(recvd_msg); break; - case ID_MC4_TEMPS: rr_inv.receive_temp_msg(recvd_msg); break; - // Motor energy - case ID_MC1_ENERGY: fl_inv.receive_energy_msg(recvd_msg); break; - case ID_MC2_ENERGY: fr_inv.receive_energy_msg(recvd_msg); break; - case ID_MC3_ENERGY: rl_inv.receive_energy_msg(recvd_msg); break; - case ID_MC4_ENERGY: rr_inv.receive_energy_msg(recvd_msg); break; - default: break; - } - } -} - -void dispatch_telem_CAN() { - while (CAN3_rxBuffer.available()) - { - CAN_message_t recvd_msg; - uint8_t buf[sizeof(CAN_message_t)]; - CAN3_rxBuffer.pop_front(buf, sizeof(CAN_message_t)); - memmove(&recvd_msg, buf, sizeof(recvd_msg)); - switch (recvd_msg.id) - { - // AMS CAN - case ID_BMS_COULOMB_COUNTS: - // ams_interface.retrieve_coulomb_count_CAN(recvd_msg); - break; - case ID_BMS_STATUS: - ams_interface.retrieve_status_CAN(recvd_msg, curr_tick); - break; - case ID_BMS_TEMPERATURES: - ams_interface.retrieve_temp_CAN(recvd_msg); - break; - case ID_BMS_VOLTAGES: - ams_interface.retrieve_voltage_CAN(recvd_msg); - break; - // Dashboard status - case ID_DASHBOARD_STATUS: - dashboard.read(recvd_msg); - break; - default: - break; - } - } -} - void update_and_enqueue_all_CAN() { // Drivetrain system // probably here as well @@ -261,7 +184,7 @@ void sample_all_external_readings() { // Tick steering system // steering1.tick(); // Read shutdown circuits - main_ecu.read_mcu_status(); + // main_ecu.read_mcu_status(); } void process_all_value_readings() {