diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index f7fe0fb56..4b85b5048 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -37,13 +37,17 @@ class InverterInterface void request_enable_inverter(); void command_no_torque(); void handle_command(const InverterCommand &command); + void command_reset(); bool inverter_system_ready() { return system_ready_; } - + bool error() + { + return error_; + } bool dc_quit_on() { return quit_dc_on_; @@ -63,6 +67,7 @@ class InverterInterface void write_cmd_msg_to_queue_(const MC_setpoints_command &msg); int16_t speed_; uint16_t dc_bus_voltage_; + bool error_; bool quit_dc_on_; bool quit_inverter_on_; bool system_ready_; diff --git a/lib/interfaces/include/InverterInterface.tpp b/lib/interfaces/include/InverterInterface.tpp index 0c21ef9e4..b7a5af48c 100644 --- a/lib/interfaces/include/InverterInterface.tpp +++ b/lib/interfaces/include/InverterInterface.tpp @@ -16,7 +16,7 @@ template void InverterInterface::request_enable_hv() { - MC_setpoints_command mc_setpoints_command; + MC_setpoints_command mc_setpoints_command{}; mc_setpoints_command.set_hv_enable(true); write_cmd_msg_to_queue_(mc_setpoints_command); } @@ -24,20 +24,19 @@ void InverterInterface::request_enable_hv() template void InverterInterface::request_enable_inverter() { - MC_setpoints_command mc_setpoints_command; + MC_setpoints_command mc_setpoints_command{}; mc_setpoints_command.set_speed_setpoint(0); mc_setpoints_command.set_pos_torque_limit(0); mc_setpoints_command.set_neg_torque_limit(0); mc_setpoints_command.set_driver_enable(true); mc_setpoints_command.set_inverter_enable(true); - write_cmd_msg_to_queue_(mc_setpoints_command); } template void InverterInterface::command_no_torque() { - MC_setpoints_command mc_setpoints_command; + MC_setpoints_command mc_setpoints_command{}; mc_setpoints_command.set_speed_setpoint(0); mc_setpoints_command.set_pos_torque_limit(0); mc_setpoints_command.set_neg_torque_limit(0); @@ -48,7 +47,7 @@ void InverterInterface::command_no_torque() template void InverterInterface::handle_command(const InverterCommand &command) { - MC_setpoints_command mc_setpoints_command; + MC_setpoints_command mc_setpoints_command{}; // TODO handle the correct conversion to the over the wire data from real-world data type mc_setpoints_command.set_speed_setpoint(command.speed_setpoint_rpm); @@ -65,15 +64,24 @@ void InverterInterface::handle_command(const InverterCommand &com write_cmd_msg_to_queue_(mc_setpoints_command); } + +template +void InverterInterface::command_reset() +{ + MC_setpoints_command mc_setpoints_command{}; + mc_setpoints_command.set_remove_error(true); + write_cmd_msg_to_queue_(mc_setpoints_command); +} + template void InverterInterface::receive_status_msg(CAN_message_t &msg) { MC_status mc_status(&msg.buf[0]); - system_ready_ = mc_status.get_system_ready(); quit_dc_on_ = mc_status.get_quit_dc_on(); quit_inverter_on_ = mc_status.get_quit_inverter_on(); speed_ = mc_status.get_speed(); + error_ = mc_status.get_error(); } // TODO fill this in with the correct receiving diff --git a/lib/interfaces/include/SteeringEncoderInterface.h b/lib/interfaces/include/SteeringEncoderInterface.h index 49a43a672..602f98f02 100644 --- a/lib/interfaces/include/SteeringEncoderInterface.h +++ b/lib/interfaces/include/SteeringEncoderInterface.h @@ -1,7 +1,6 @@ #ifndef __UPPERSTEERINGSENSOR_H__ #define __UPPERSTEERINGSENSOR_H__ -#include "AnalogSensorsInterface.h" #include enum SteeringEncoderStatus_e diff --git a/lib/interfaces/include/WatchdogInterface.h b/lib/interfaces/include/WatchdogInterface.h index 38ad4c2dd..7c30a99d9 100644 --- a/lib/interfaces/include/WatchdogInterface.h +++ b/lib/interfaces/include/WatchdogInterface.h @@ -5,7 +5,7 @@ #include "HyTech_CAN.h" #include "SysClock.h" -const unsigned long WATCHDOG_KICK_INTERVAL = 7 // milliseconds +const unsigned long WATCHDOG_KICK_INTERVAL = 7; // milliseconds class WatchdogInterface { diff --git a/lib/interfaces/library.json b/lib/interfaces/library.json new file mode 100644 index 000000000..d7a40d1ea --- /dev/null +++ b/lib/interfaces/library.json @@ -0,0 +1,7 @@ +{ + "name": "interfaces-lib", + "version": "1.0.0", + "license": "MIT", + "frameworks": "*", + "platforms": "*" + } \ No newline at end of file diff --git a/lib/interfaces/src/DashboardInterface.cpp b/lib/interfaces/src/DashboardInterface.cpp index f090396de..6b2063804 100644 --- a/lib/interfaces/src/DashboardInterface.cpp +++ b/lib/interfaces/src/DashboardInterface.cpp @@ -70,4 +70,5 @@ bool DashboardInterface::safetySystemOK() {return _data.ssok;} bool DashboardInterface::shutdownHAboveThreshold() {return _data.shutdown;} void DashboardInterface::soundBuzzer(bool state) {_data.buzzer_cmd = state;} -bool DashboardInterface::checkBuzzer() {return _data.buzzer_state;} \ No newline at end of file +bool DashboardInterface::checkBuzzer() {return _data.buzzer_state;} + diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 3ba78554d..9e62be225 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -1,11 +1,9 @@ #ifndef DRIVETRAINSYSTEM #define DRIVETRAINSYSTEM -#include "InverterSystem.h" -#include "InverterInterface.h" #include "Utility.h" #include - +#include "stdint.h" struct DrivetrainCommand_s { float speeds[NUM_MOTORS]; @@ -58,6 +56,7 @@ class DrivetrainSystem bool hv_over_threshold_on_drivetrain(); void disable(); bool drivetrain_error_occured(); + void reset_drivetrain(); void command_drivetrain(const DrivetrainCommand_s& data); private: diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index e7cba2beb..6afc33d50 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -37,13 +37,35 @@ void DrivetrainSystem::command_drivetrain_no_torque() } template -void DrivetrainSystem::command_drivetrain(const DrivetrainCommand& data) +bool DrivetrainSystem::drivetrain_error_occured() { + for (auto inv_pointer : inverters_) + { + if(inv_pointer->error()) + { + return true; + } + } + return false; +} - 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); +template +void DrivetrainSystem::reset_drivetrain() +{ + for (auto inv_pointer : inverters_) + { + inv_pointer->command_reset(); + } +} + +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); } @@ -96,4 +118,4 @@ bool DrivetrainSystem::drivetrain_enabled() } } return true; -} \ No newline at end of file +} diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index e8b8c05b6..99efbb07e 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -2,10 +2,10 @@ #define __TORQUECTRLMUX_H__ #include -#include -#include -#include -#include +#include "TorqueControllers.h" +#include "DrivetrainSystem.h" +#include "PedalsSystem.h" +#include "SteeringSystem.h' #include "DashboardInterface.h" #include "AnalogSensorsInterface.h" diff --git a/lib/systems/library.json b/lib/systems/library.json new file mode 100644 index 000000000..3e92d85e5 --- /dev/null +++ b/lib/systems/library.json @@ -0,0 +1,10 @@ +{ + "name": "systems-lib", + "version": "1.0.0", + "license": "MIT", + "dependencies": { + "interfaces-lib": "*" + }, + "frameworks": "*", + "platforms": "*" + } \ No newline at end of file diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index eb6691d91..51765f32e 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -1,4 +1,4 @@ -#include +#include "SteeringSystem.h" void SteeringSystem::tick(const SysTick_s &tick, const AnalogConversion_s &secondaryConversion) { diff --git a/platformio.ini b/platformio.ini index 98d292385..146de664e 100644 --- a/platformio.ini +++ b/platformio.ini @@ -10,7 +10,7 @@ build_src_filter = ; refactoring build_src_filter = -<**/*.cpp> - + + + build_unflags = -std=gnu++11 build_flags = -std=c++17 platform = teensy diff --git a/src/old_main.cpp b/src/old_main.cpp new file mode 100644 index 000000000..8f74d7f3b --- /dev/null +++ b/src/old_main.cpp @@ -0,0 +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" + +// 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; + + + 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; + + + 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()); + + 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()); + } + +}