diff --git a/lib/interfaces/README.md b/lib/interfaces/README.md index 2c9b606..c1c2778 100644 --- a/lib/interfaces/README.md +++ b/lib/interfaces/README.md @@ -22,7 +22,6 @@ below is the description of the control messages that will be configured within - Control Input Message (will be attempted to be sent every 5ms, monitored by the inverter to be expected at least every 20ms) - `int16_t speed_setpoint` - - __NOTE: active only when in speed control mode__ - AKA (`AMK_TargetVelocity`) / Special Signal index 6 / AKA 16-bit version of SERCOS parameter ID36 (unknown why this is 16 bit vs the 32 bit SERCOS parameter in manual) - the RPM setpoint for the inverter in units of RPM. - `int16_t positive_torque_limit` @@ -33,10 +32,6 @@ below is the description of the control messages that will be configured within - `int16_t negative_torque_limit` - AKA (`AMK_TorqueLimitNegativ`) / SERCOS parameter ID83 / Special Signal index 14 - `positive_torque_limit` with a negative sign. - - `int16_t torque_setpoint` - - __NOTE: active only when in torque control mode.__ - - AKA SERCOS parameter ID80 / Special Signal index 17 - - the torque setpoint for the inverter in units of percentage as described in the `positive_torque_limit` message member above - Control parameter message (will be sent intermitently, not monitored by the inverter to be expected at any regular period) - `uint16_t speed_control_kp` diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index c51c1c2..b9a8f59 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -2,6 +2,13 @@ #define INVERTERINTERFACE_H #include +#include "FlexCAN_T4.h" +#include "MessageQueueDefine.h" + +#include +#include "DrivetrainSystem.h" +#include + namespace HTUnits { using celcius = float; @@ -12,10 +19,56 @@ namespace HTUnits using volts = float; }; -// for the most part these are mirrors of the lower-level CAN struct data, except with already fully float-ized data +struct InverterParams_s +{ + float MINIMUM_HV_VOLTAGE; +}; + +/** + * Struct containing id info for this specific inverter interface + */ +struct InverterIds_s +{ + uint32_t inv_control_word_id; + uint32_t inv_control_input_id; + uint32_t inv_control_parameter_id; +}; + +/** + * Drivetrain system accessible structs for + * requesting change of state + */ +struct InverterControlWord_s +{ + bool inverter_enable : 1; + bool hv_enable : 1; + bool driver_enable : 1; + bool remove_error : 1; +}; + +struct InverterControlInput_s +{ + int16_t speed_rpm_setpoint; + int16_t positive_torque_limit; + int16_t negative_torque_limit; +}; + +struct InverterControlParams_s +{ + uint16_t speed_control_kp; + uint16_t speed_control_ki; + uint16_t speed_control_kd; +}; +/** + * For the most part these are mirrors of the lower-level CAN struct data, + * except with already fully float-ized data +**/ struct InverterStatus_s { + bool hv_present : 1; + bool connected : 1; + bool new_data : 1; bool system_ready : 1; bool error : 1; bool warning : 1; @@ -30,6 +83,7 @@ struct InverterStatus_s struct InverterTemps_s { + bool new_data : 1; HTUnits::celcius motor_temp; HTUnits::celcius inverter_temp; HTUnits::celcius igbt_temp; @@ -37,19 +91,22 @@ struct InverterTemps_s struct InverterPower_s { + bool new_data : 1; HTUnits::watts active_power; HTUnits::var reactive_power; }; struct MotorMechanics_s { + bool new_data : 1; HTUnits::watts actual_power; HTUnits::torque_nm actual_torque; HTUnits::speed_rpm actual_speed; }; -struct InverterControlParams_s +struct InverterControlFeedback_s { + bool new_data : 1; uint16_t speed_control_kp; uint16_t speed_control_ki; uint16_t speed_control_kd; @@ -61,16 +118,75 @@ struct InverterFeedbackData_s InverterTemps_s temps; InverterPower_s power; MotorMechanics_s motor_mechanics; - InverterControlParams_s control_params; + InverterControlFeedback_s control_feedback; }; -// struct InverterMotorControl_s -// { - -// } - +/** + * Inverter interface + */ class InverterInterface { + public: + + InverterInterface( + CANBufferType *msg_output_queue, + uint32_t inv_control_word_id, + uint32_t inv_control_input_id, + uint32_t inv_control_params_id, + InverterParams_s inverter_params) : msg_queue_(msg_output_queue), _inverter_params(inverter_params) + { + inverter_ids.inv_control_word_id = inv_control_word_id; + inverter_ids.inv_control_parameter_id = inv_control_params_id; + inverter_ids.inv_control_input_id = inv_control_input_id; + } + + // TODO un-public these (they are public for testing) + + /* receiving callbacks */ + void receive_INV_STATUS(CAN_message_t &can_msg); + + void receive_INV_TEMPS(CAN_message_t &can_msg); + + void receive_INV_DYNAMICS(CAN_message_t &can_msg); + + void receive_INV_POWER(CAN_message_t &can_msg); + + void receive_INV_FEEDBACK(CAN_message_t &can_msg); + + /* Sending */ + void send_INV_SETPOINT_COMMAND(); + + void send_INV_CONTROL_WORD(); + + void send_INV_CONTROL_PARAMS(); + + /* Inverter Functs */ + void set_speed(float desired_rpm, float torque_limit_nm); + + void set_idle(); + + void set_inverter_control_word(InverterControlWord_s control_word); + + InverterStatus_s get_inverter_status(); + + private: + + InverterIds_s inverter_ids; + InverterControlInput_s _inverter_control_inputs; + InverterControlWord_s _inverter_control_word; + InverterControlParams_s _inverter_control_params; + InverterFeedbackData_s _feedback_data; + InverterParams_s _inverter_params; + + /* Getters */ + InverterStatus_s get_status(); + InverterTemps_s get_temps(); + InverterPower_s get_power(); + MotorMechanics_s get_motor_mechanics(); + InverterControlFeedback_s get_control_params(); + + CANBufferType *msg_queue_; }; -#endif // __INVERTERINTERFACE_H__ \ No newline at end of file +#endif // __INVERTERINTERFACE_H__ + diff --git a/lib/interfaces/include/MessageQueueDefine.h b/lib/interfaces/include/MessageQueueDefine.h new file mode 100644 index 0000000..8c07056 --- /dev/null +++ b/lib/interfaces/include/MessageQueueDefine.h @@ -0,0 +1,8 @@ +#ifndef MESAGE_QUEUE_DEFINE_H +#define MESAGE_QUEUE_DEFINE_H + +#include "FlexCAN_T4.h" + +using CANBufferType = Circular_Buffer; + +#endif \ No newline at end of file diff --git a/lib/interfaces/library.json b/lib/interfaces/library.json new file mode 100644 index 0000000..52e9b53 --- /dev/null +++ b/lib/interfaces/library.json @@ -0,0 +1,13 @@ +{ + "name": "interfaces-lib", + "version": "1.0.0", + "license": "MIT", + "dependencies": { + "can_lib": "*", + "shared_data": "*", + "CASE_lib": "*", + "nanopb": "*" + }, + "frameworks": "*", + "platforms": "*" + } \ No newline at end of file diff --git a/lib/interfaces/src/InverterInterface.cpp b/lib/interfaces/src/InverterInterface.cpp new file mode 100644 index 0000000..1dd8eb2 --- /dev/null +++ b/lib/interfaces/src/InverterInterface.cpp @@ -0,0 +1,185 @@ +#include + +/** + * Getters for the data + */ + +InverterStatus_s InverterInterface::get_status() { + _feedback_data.status.new_data = false; + return _feedback_data.status; +} + +InverterTemps_s InverterInterface::get_temps() { + _feedback_data.temps.new_data = false; + return _feedback_data.temps; +} + +InverterPower_s InverterInterface::get_power() { + _feedback_data.power.new_data = false; + return _feedback_data.power; +} + +MotorMechanics_s InverterInterface::get_motor_mechanics() { + _feedback_data.motor_mechanics.new_data = false; + return _feedback_data.motor_mechanics; +} + + +InverterControlFeedback_s InverterInterface::get_control_params() { + _feedback_data.control_feedback.new_data = false; + return _feedback_data.control_feedback; +} + + +/** + * receiving CAN messages + */ + +void InverterInterface::receive_INV_STATUS(CAN_message_t &can_msg) +{ + // Unpack the message + INV1_STATUS_t unpacked_msg; + Unpack_INV1_STATUS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + + // Update inverter interface with new data + _feedback_data.status.connected = true; // Will set to true once first CAN message has been received + _feedback_data.status.system_ready = unpacked_msg.system_ready; + _feedback_data.status.error = unpacked_msg.error; + _feedback_data.status.warning = unpacked_msg.warning; + _feedback_data.status.quit_dc_on = unpacked_msg.quit_dc_on; + _feedback_data.status.dc_on = unpacked_msg.dc_on; + _feedback_data.status.quit_inverter_on = unpacked_msg.quit_inverter_on; + _feedback_data.status.derating_on = unpacked_msg.derating_on; + _feedback_data.status.dc_bus_voltage = unpacked_msg.dc_bus_voltage; + _feedback_data.status.diagnostic_number = unpacked_msg.diagnostic_number; + _feedback_data.status.hv_present = _feedback_data.status.dc_bus_voltage > _inverter_params.MINIMUM_HV_VOLTAGE; +} + +void InverterInterface::receive_INV_TEMPS(CAN_message_t &can_msg) +{ + + // Unpack the message + INV1_TEMPS_t unpacked_msg; + Unpack_INV1_TEMPS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + + // Update inverter interface with new data + _feedback_data.temps.igbt_temp = HYTECH_igbt_temp_ro_fromS(unpacked_msg.igbt_temp_ro); + _feedback_data.temps.inverter_temp = HYTECH_inverter_temp_ro_fromS(unpacked_msg.inverter_temp_ro); + _feedback_data.temps.motor_temp = HYTECH_motor_temp_ro_fromS(unpacked_msg.motor_temp_ro); + _feedback_data.temps.new_data = true; + +} + +void InverterInterface::receive_INV_DYNAMICS(CAN_message_t &can_msg) +{ + + // Unpack the message + INV1_DYNAMICS_t unpacked_msg; + Unpack_INV1_DYNAMICS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + + // Update inverter interface with new data + _feedback_data.motor_mechanics.actual_power = unpacked_msg.actual_power_w; + _feedback_data.motor_mechanics.actual_torque = unpacked_msg.actual_torque_nm; + _feedback_data.motor_mechanics.actual_speed = unpacked_msg.actual_speed_rpm; + _feedback_data.motor_mechanics.new_data = true; + +} + +void InverterInterface::receive_INV_POWER(CAN_message_t &can_msg) +{ + // Unpack the message + INV1_POWER_t unpacked_msg; + Unpack_INV1_POWER_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + + // Update inverter interface with new data + _feedback_data.power.active_power = unpacked_msg.active_power_w; + _feedback_data.power.reactive_power = unpacked_msg.reactive_power_var; + _feedback_data.power.new_data = true; + +} + +void InverterInterface::receive_INV_FEEDBACK(CAN_message_t &can_msg) +{ + // Unpack the message + INV1_FEEDBACK_t unpacked_msg; + Unpack_INV1_FEEDBACK_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + + // Update inverter interface with new data + _feedback_data.control_feedback.speed_control_kp = unpacked_msg.speed_control_kp; + _feedback_data.control_feedback.speed_control_ki = unpacked_msg.speed_control_ki; + _feedback_data.control_feedback.speed_control_kd = unpacked_msg.speed_control_kd; + _feedback_data.control_feedback.new_data = true; + +} + +/** + * Sending CAN messages + */ + +void InverterInterface::send_INV_SETPOINT_COMMAND() +{ + INV1_CONTROL_INPUT_t msg_out; + + msg_out.speed_setpoint_rpm = _inverter_control_inputs.speed_rpm_setpoint; + msg_out.positive_torque_limit_ro = _inverter_control_inputs.positive_torque_limit; + msg_out.negative_torque_limit_ro = _inverter_control_inputs.negative_torque_limit; + + CAN_util::enqueue_msg(&msg_out, &Pack_INV1_CONTROL_INPUT_hytech, inverter_ids.inv_control_input_id); +} + +void InverterInterface::send_INV_CONTROL_WORD() +{ + INV1_CONTROL_WORD_t msg_out; + + msg_out.driver_enable = _inverter_control_word.driver_enable; + msg_out.hv_enable = _inverter_control_word.hv_enable; + msg_out.inverter_enable = _inverter_control_word.inverter_enable; + msg_out.remove_error = _inverter_control_word.remove_error; + + CAN_util::enqueue_msg(&msg_out, &Pack_INV1_CONTROL_WORD_hytech, inverter_ids.inv_control_word_id); +} + +void InverterInterface::send_INV_CONTROL_PARAMS() +{ + INV1_CONTROL_PARAMETER_t msg_out; + + msg_out.speed_control_kd = _inverter_control_params.speed_control_kp; + msg_out.speed_control_ki = _inverter_control_params.speed_control_ki; + msg_out.speed_control_kd = _inverter_control_params.speed_control_kd; + + CAN_util::enqueue_msg(&msg_out, &Pack_INV1_CONTROL_PARAMETER_hytech, inverter_ids.inv_control_parameter_id); +} + +/** + * Methods for use as inverter functs + */ + +void InverterInterface::set_speed(float desired_rpm, float torque_limit_nm) +{ + _inverter_control_inputs.speed_rpm_setpoint = desired_rpm; + + float converted_torque = std::abs(torque_limit_nm * (1000/9.8)); + + _inverter_control_inputs.positive_torque_limit = converted_torque; + _inverter_control_inputs.negative_torque_limit = -converted_torque; +} + +void InverterInterface::set_idle() +{ + _inverter_control_inputs.negative_torque_limit = 0; + _inverter_control_inputs.positive_torque_limit = 0; + _inverter_control_inputs.speed_rpm_setpoint = 0; +} + +void InverterInterface::set_inverter_control_word(InverterControlWord_s control_word) +{ + _inverter_control_word.driver_enable = control_word.driver_enable; + _inverter_control_word.hv_enable = control_word.hv_enable; + _inverter_control_word.inverter_enable = control_word.inverter_enable; + _inverter_control_word.remove_error = control_word.remove_error; +} + +InverterStatus_s InverterInterface::get_inverter_status() +{ + return _feedback_data.status; +} \ No newline at end of file diff --git a/lib/state_machine/include/VehicleStateMachine.h b/lib/state_machine/include/VehicleStateMachine.h index fd6db34..6ad3ac5 100644 --- a/lib/state_machine/include/VehicleStateMachine.h +++ b/lib/state_machine/include/VehicleStateMachine.h @@ -1,83 +1,62 @@ -#ifndef VEHICLE_STATE_MACHINE -#define VEHICLE_STATE_MACHINE +#ifndef __VEHICLE_STATE_MACHINE__ +#define __VEHICLE_STATE_MACHINE__ -/* From local systems library */ -#include "DrivetrainSystem.h" -#include "BuzzerController.h" +#include -/** - * Enum representing possible states for the vehicle's state machine. - * - * STARTUP - Initial state. Vehicle never stays in STARTUP for more than 1 tick. - * TRACTIVE_SYSTEM_NOT_ACTIVE - Car stays here until the Drivetrain reports that the inverters' voltage - * is above the HV threshold. - * TRACTICE_SYSTEM_ACTIVE - HV is on, car is waiting for brake + start button (to start car) - * ENABLING_INVERTERS - While in this state, the car calls the drivetrain's enable command. If - * successful, (usually nearly-immediate), then car goes into WRTDS. - * WAITING_READY_TO_DRIVE_SOUND - When entering state, buzzer is activated. After BuzzerController says - * enough time has passed, we enter the next state. - * READY_TO_DRIVE - While in this state, pedal inputs command the drivetrain. - */ -enum class CAR_STATE -{ - STARTUP = 0, - TRACTIVE_SYSTEM_NOT_ACTIVE = 1, - TRACTIVE_SYSTEM_ACTIVE = 2, - ENABLING_INVERTERS = 3, - WAITING_READY_TO_DRIVE_SOUND = 4, - READY_TO_DRIVE = 5 +enum class VEHICLE_STATE { + TRACTIVE_SYSTEM_NOT_ACTIVE = 1, + TRACTIVE_SYSTEM_ACTIVE = 2, + WANTING_READY_TO_DRIVE = 3, + READY_TO_DRIVE = 4 }; -/** - * This singleton class represents the vehicle's state machine as we enable HV, enable the inverters, - * wait for the start button to be pressed, and enter ready-to-drive mode. Aside from getters, this - * class only has one public function, tick_state_machine(). In order to run its update logic, the - * VehicleStateMachine uses instance data directly from other singleton classes (Buzzer, Pedals, etc) - * and from the global structs (VCRInterfaceData_s, VCRSystemData_s, etc.). - */ class VehicleStateMachine { -public: - VehicleStateMachine(DrivetrainSystem & drivetrain_system) : - _current_state(CAR_STATE::STARTUP), - _drivetrain(drivetrain_system), - _buzzer(BuzzerController::getInstance()) {}; - - - /** - * This tick() function handles all the update logic for traversing states, and calls the functions - * of other classes as necessary. - * @pre Other systems are updated properly - * @pre All relevant data exists in the data structs (VCRInterfaceData, VCRSystemData, etc.) - * @param current_millis The system time, in millis. Passed in by the scheduler. - * @param system_data A reference to the global system data struct. - */ - void tick_state_machine(unsigned long current_millis, const VCRSystemData_s &system_data); - - CAR_STATE get_state() { return _current_state; } - -private: - - void set_state_(CAR_STATE new_state, unsigned long curr_time); - - /** - * The function run upon the entry of the car into a new state. - * @param new_state The state in which we are entering. - */ - void handle_entry_logic_(CAR_STATE new_state, unsigned long curr_millis); - - /** - * The function run upon the exit of a state. - * @param prev_state the state in which we are leaving. - */ - void handle_exit_logic_(CAR_STATE prev_state, unsigned long curr_millis); - - CAR_STATE _current_state; + public: + VehicleStateMachine( + etl::delegate hv_over_threshold, + etl::delegate start_button_pressed, + etl::delegate brake_pressed, + etl::delegate drivetrain_error_ocurred, + etl::delegate drivetrain_ready, + etl::delegate handle_drivetrain_init, + etl::delegate command_drivetrain + ) : + _hv_over_threshold(hv_over_threshold), + _start_button_pressed(start_button_pressed), + _brake_pressed(brake_pressed), + _drivetrain_error_ocurred(drivetrain_error_ocurred), + _drivetrain_ready(drivetrain_ready), + _handle_drivetrain_init(handle_drivetrain_init), + _command_drivetrain(command_drivetrain) + { + _current_state = VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE; + + } + + void tick_state_machine(unsigned long curr_time_millis); + + VEHICLE_STATE get_state() { return _current_state; } + + private: + + void _set_state(VEHICLE_STATE new_state, unsigned long current_time_millis); + + void _handle_entry_logic(VEHICLE_STATE prev_state, unsigned long current_time_millis); + + void _handle_exit_logic(VEHICLE_STATE new_state, unsigned long current_time_millis); + + VEHICLE_STATE _current_state; + + // Lambdas neccesary for state machine to work + etl::delegate _hv_over_threshold; + etl::delegate _start_button_pressed; + etl::delegate _brake_pressed; + etl::delegate _drivetrain_error_ocurred; + etl::delegate _drivetrain_ready; + etl::delegate _handle_drivetrain_init; + etl::delegate _command_drivetrain; // Shouldn't need to pass anything; logic will be handled in the lambda - /* System references to show dependence on systems library */ - DrivetrainSystem &_drivetrain; //TODO: Make this InverterInterface instead of uint32_t - BuzzerController &_buzzer; - // AMSSystem &_ams_system; }; -#endif /* VEHICLE_STATE_MACHINE */ +#endif \ No newline at end of file diff --git a/lib/state_machine/src/VehicleStateMachine.cpp b/lib/state_machine/src/VehicleStateMachine.cpp index d182629..2764159 100644 --- a/lib/state_machine/src/VehicleStateMachine.cpp +++ b/lib/state_machine/src/VehicleStateMachine.cpp @@ -4,192 +4,116 @@ /* Local includes */ #include "VehicleStateMachine.h" -void VehicleStateMachine::tick_state_machine(unsigned long current_millis, const VCRSystemData_s &system_data) +void VehicleStateMachine::tick_state_machine(unsigned long current_millis) { switch (_current_state) { - - case CAR_STATE::STARTUP: - { - hal_println("In startup state"); - set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - break; - } - - case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: - { - // if TS is above HV threshold, move to Tractive System Active - // _drivetrain.disable_no_pins(); - // if (_drivetrain.hv_over_threshold_on_drivetrain()) - // { - // set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); - // } - break; - } - - case CAR_STATE::TRACTIVE_SYSTEM_ACTIVE: - { - if (_buzzer.buzzer_is_active(current_millis)) + case VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: { - _buzzer.deactivate(); + if (_hv_over_threshold()) + { + _set_state(VEHICLE_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); + break; + } + break; } - // _drivetrain.disable_no_pins(); - - // if (!_drivetrain.hv_over_threshold_on_drivetrain()) - // { - // set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - // break; - // } - - if (system_data.dash_input_state.start_btn_is_pressed && system_data.pedals_system_data.brake_is_pressed) + case VEHICLE_STATE::TRACTIVE_SYSTEM_ACTIVE: { - set_state_(CAR_STATE::ENABLING_INVERTERS, current_millis); + if (!_hv_over_threshold()) + { + _set_state(VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + break; + } + + if (_start_button_pressed() && _brake_pressed()) + { + _set_state(VEHICLE_STATE::WANTING_READY_TO_DRIVE, current_millis); + break; + } break; } - break; - } - - case CAR_STATE::ENABLING_INVERTERS: - { - // TODO: replace old drivetrain function handling with new interaction paradigm - // If HV is not active, go to TRACTIVE_SYSTEM_NOT_ACTIVE - // if (_drivetrain.hv_over_threshold_on_drivetrain()) - // { - // set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - // break; - // } - - // If motor controllers have error, but HV still active - // if (_drivetrain.drivetrain_error_occured()) - // { - // set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); - // } - - // if (_drivetrain.handle_inverter_startup(current_millis)) - // { - // set_state_(CAR_STATE::WAITING_READY_TO_DRIVE_SOUND, current_millis); - // break; - // } - break; - } - - case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: - { - - // If HV is no longer active, return to TRACTIVE_SYSTEM_NOT_ACTIVE - // if (_drivetrain.hv_over_threshold_on_drivetrain()) - // { - // set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - // break; - // } - - // _drivetrain.command_drivetrain_no_torque(); // While waiting for RTD sound to complete, always command 0 torque - - // If the ready-to-drive sound is done playing, move to ready to drive mode - if (!_buzzer.buzzer_is_active(current_millis)) + case VEHICLE_STATE::WANTING_READY_TO_DRIVE: { - set_state_(CAR_STATE::READY_TO_DRIVE, current_millis); - } - break; - - } - - case CAR_STATE::READY_TO_DRIVE: - { - - // If HV is no longer active, return to TRACTIVE_SYSTEM_NOT_ACTIVE - - // TODO make this real: check to see if the state - if(_drivetrain.get_state() == DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT) - { - set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + if (!_hv_over_threshold) + { + _set_state(VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + break; + } + + if (_drivetrain_ready()) + { + _set_state(VEHICLE_STATE::READY_TO_DRIVE, current_millis); + break; + } break; } - - bool drivetrain_in_driveable_mode = ((_drivetrain.get_state() == DrivetrainState_e::ENABLED_SPEED_MODE) || (_drivetrain.get_state() == DrivetrainState_e::ENABLED_TORQUE_MODE)); - if (!drivetrain_in_driveable_mode) + + case VEHICLE_STATE::READY_TO_DRIVE: { - hal_println("Drivetrain error occurred while in READY_TO_DRIVE"); - set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); + if (!_hv_over_threshold) + { + _set_state(VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + break; + } + + if (_drivetrain_error_ocurred) + { + _set_state(VEHICLE_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); + break; + } + + _command_drivetrain(); break; } - if (/* _ams_system.ams_ok() && */ !system_data.pedals_system_data.implausibility_has_exceeded_max_duration) + default: { - // TODO: Fix with all references to singleton classes - // TODO: need to also handle request to mode switch via drivetrain init (?) - // _drivetrain.command_drivetrain(controller_mux_->getDrivetrainCommand(dashboard_->getDialMode(), dashboard_->getTorqueLimitMode(), current_car_state)); - DrivetrainTorqueCommand_s example_cmd = {}; // will need to do a variant check from the controller mux to see what type it is (torque / speed) - - (void)_drivetrain.evaluate_drivetrain(example_cmd); - - } - else - { - // If software is not OK or some implausibility has exceeded max duration, command 0 torque (but stay in RTD mode) - - // TODO: make this check to see exactly what drive mode the drivetrain is in and send its associated empty command. - DrivetrainTorqueCommand_s example_cmd = {}; // will need to do a variant check from the controller mux to see what type it is (torque / speed) - (void)_drivetrain.evaluate_drivetrain(example_cmd); + break; } - break; - } - } } -void VehicleStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_millis) +void VehicleStateMachine::_set_state(VEHICLE_STATE new_state, unsigned long curr_millis) { - handle_exit_logic_(_current_state, curr_millis); + _handle_entry_logic(_current_state, curr_millis); _current_state = new_state; - handle_entry_logic_(new_state, curr_millis); + _handle_exit_logic(new_state, curr_millis); } -void VehicleStateMachine::handle_exit_logic_(CAR_STATE prev_state, unsigned long curr_millis) +void VehicleStateMachine::_handle_exit_logic(VEHICLE_STATE prev_state, unsigned long curr_millis) { - switch (get_state()) + switch (prev_state) { - case CAR_STATE::STARTUP: - break; - case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: - break; - case CAR_STATE::TRACTIVE_SYSTEM_ACTIVE: - break; - case CAR_STATE::ENABLING_INVERTERS: - break; - case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: - break; - case CAR_STATE::READY_TO_DRIVE: - break; - default: - break; - } + case VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: + break; + case VEHICLE_STATE::TRACTIVE_SYSTEM_ACTIVE: + break; + case VEHICLE_STATE::WANTING_READY_TO_DRIVE: + break; + case VEHICLE_STATE::READY_TO_DRIVE: + break; + default: + break; + } } -void VehicleStateMachine::handle_entry_logic_(CAR_STATE new_state, unsigned long curr_millis) +void VehicleStateMachine::_handle_entry_logic(VEHICLE_STATE new_state, unsigned long curr_millis) { switch (new_state) - { - case CAR_STATE::STARTUP: - break; - case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: - break; - case CAR_STATE::TRACTIVE_SYSTEM_ACTIVE: - break; - case CAR_STATE::ENABLING_INVERTERS: - break; - case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: - { - _buzzer.activate(curr_millis); - break; - } - case CAR_STATE::READY_TO_DRIVE: - break; - default: - break; - } + { + case VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: + break; + case VEHICLE_STATE::TRACTIVE_SYSTEM_ACTIVE: + break; + case VEHICLE_STATE::WANTING_READY_TO_DRIVE: + break; + case VEHICLE_STATE::READY_TO_DRIVE: + break; + default: + break; + } } diff --git a/lib/systems/README.md b/lib/systems/README.md index e41d3e8..38b99f4 100644 --- a/lib/systems/README.md +++ b/lib/systems/README.md @@ -43,13 +43,10 @@ stateDiagram-v2 ready : INVERTERS_READY hv_en : INVERTERS_HV_ENABLED en : INVERTERS_ENABLED - en_sp_mode : ENABLING_INVERTERS_SPEED_MODE - en_torq_mode : ENABLING_INVERTERS_TORQUE_MODE - speed_mode : ENABLED_SPEED_MODE - torq_mode : ENABLED_TORQUE_MODE + drive_mode : ENABLED_DRIVE_MODE err: ERROR clear_err: CLEARING_ERRORS - note2: note that that note shown is that any state other than NOT_CONNECTED can go to the ERROR state if in any of the states any inverter has an error flag present + note2: note that any state other than NOT_CONNECTED can go to the ERROR state if in any of the states any inverter has an error flag present note right of err during this state all setpoints will be set to 0 and the inverter control word will have the inverter_enable flag set to false @@ -59,22 +56,10 @@ stateDiagram-v2 during this state all setpoints will be set to 0, (inverter_enable: false, hv_enable: true, driver_enable: true, remove_error: true) end note - note right of speed_mode + note right of drive_mode during this mode the drivetrain is free to receive speed commands and car will move end note - note right of torq_mode - during this mode the drivetrain is free to receive torque commands and car will move - end note - - note right of en_sp_mode - during this mode the VCR sets the enable torque mode GPIO pin to be off and waits for the inverter's GPIO for signaling that the secondary control is not in control - end note - - note right of en_torq_mode - same as the ENABLING_SPEED_MODE, however the VCR is turning on its GPIO pin and waits for the inverter's GPIO to be signaled on - end note - nc --> not_en: received data from all inverters nc --> not_en_hv: received data from all inverters AND the inverters have a voltage above HV level @@ -89,16 +74,32 @@ stateDiagram-v2 hv_en --> en: on set of all inverters enabled flags en --> hv_en: on inverter enabled flag off - en --> en_sp_mode: on command request of speed mode - en_sp_mode --> speed_mode: on verification of inverters GPIO of speed mode set - en --> en_torq_mode: on command request of torque mode - en_torq_mode --> torq_mode: on verification of inverters GPIO of torque mode set - torq_mode --> err: on error during operation - speed_mode --> err: on error during operation - torq_mode --> en_sp_mode: on request of speed mode AND drivetrain is not active - speed_mode --> en_torq_mode: on request of torque mode AND drivetrain is not active - speed_mode --> err: incorrect user command type OR any inverter error - torq_mode --> err: incorrect user command type OR any inverter error + en --> drive_mode: on command request of drive mode + drive_mode --> err: on error during operation + drive_mode --> err: incorrect user command type OR any inverter error err --> clear_err: on user request of error reset clear_err --> not_en_hv: on successful reset of errors (either internally to the drivetrain system or of the inverters themselves) +``` + +``` +--- +title: Vehicle State Machine +--- +stateDiagram-v2 + + tsna : TRACTIVE_SYSTEM_NOT_ACTIVE + tsa : TRACTIVE_SYSTEM_ACTIVE + wrtd : WANTING_READY_TO_DRIVE + rtd : READY_TO_DRIVE + + tsna --> tsa : If hv is over threshold + + tsa --> tsna : If hv is under threshold + tsa --> wrtd : If the brake and start button are pressed + + wrtd --> tsna : If hv is under threshold + wrtd --> rtd : If the drivetrain system is in the ready state + + rtd --> tsna : If hv is under threshold + rtd --> tsa : If a drivetrain error occurs ``` \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 6f901bc..f10823d 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -9,6 +9,7 @@ #include "SharedFirmwareTypes.h" #include "SysClock.h" +#include "InverterInterface.h" // requirements: @@ -24,8 +25,6 @@ // - [ ] be able to reset drivetrain // - [ ] - - // TODO move these into the shared types after finishing the system enum class DrivetrainState_e { @@ -35,11 +34,8 @@ enum class DrivetrainState_e INVERTERS_READY = 3, INVERTERS_HV_ENABLED = 4, INVERTERS_ENABLED = 5, - ENABLING_INVERTERS_SPEED_MODE = 6, - ENABLING_INVERTERS_TORQUE_MODE = 7, - ENABLED_SPEED_MODE = 8, - ENABLED_TORQUE_MODE = 9, - ERROR = 10 + ENABLED_DRIVE_MODE = 6, + ERROR = 7 }; enum class DrivetrainCmdResponse_e @@ -49,47 +45,6 @@ enum class DrivetrainCmdResponse_e COMMAND_INVALID = 2 }; -struct DrivetrainSpeedCommand_s -{ - veh_vec desired_speed_rpm; - veh_vec torque_limit_nm; -}; - -struct DrivetrainTorqueCommand_s -{ - veh_vec desired_torque_nm; -}; - -enum class DrivetrainModeRequest_e -{ - UNINITIALIZED = 0, - INIT_SPEED_MODE = 1, - INIT_TORQUE_MODE =2 -}; - -struct DrivetrainInit_s -{ - DrivetrainModeRequest_e init_drivetrain; -}; - -struct InverterStatus_s -{ - float dc_bus_voltage; - float torque_nm; - float speed_rpm; - float mech_power_w; - float inverter_temp_c; - float motor_temp_c; - float igbt_temp_c; - uint16_t error_status_id; - bool inverter_ready : 1; - bool quit_dc : 1; - bool quit_inverter : 1; - bool error_present : 1; - bool connected : 1; - bool hv_present : 1; -}; - struct DrivetrainStatus_s { bool all_inverters_connected; @@ -102,30 +57,21 @@ struct DrivetrainResetError_s bool reset_errors; // true: reset the errors present on inverters, false: dont }; -// output pin of micro, read by inverters -struct DrivetrainOutputPins_s +enum DrivetrainModeRequest_e { - bool torque_mode_pin_state : 1; -}; - -// the pin set by the inverters themselves ("input": pin being read by micro) -struct DrivetrainInputPins_s -{ - bool torque_mode_enabled_pin_state : 1; + UNINITIALIZED = 0, + INIT_DRIVE_MODE = 1 }; -struct InverterControlWord_s +struct DrivetrainInit_s { - bool inverter_enable; - bool hv_enable; - bool driver_enable; - bool remove_error; + DrivetrainModeRequest_e init_drivetrain; }; class DrivetrainSystem { public: - using CmdVariant = etl::variant; + using CmdVariant = etl::variant; DrivetrainSystem() = delete; DrivetrainStatus_s evaluate_drivetrain(CmdVariant cmd); @@ -134,10 +80,10 @@ class DrivetrainSystem struct InverterFuncts { std::function set_speed; - std::function set_torque; std::function set_idle; std::function set_inverter_control_word; std::function get_status; + std::function get_motor_mechanics; }; DrivetrainSystem(veh_vec inverter_interfaces); @@ -146,17 +92,13 @@ class DrivetrainSystem bool _drivetrain_active(float max_active_rpm); void _set_state(DrivetrainState_e state); - void _handle_exit_logic(DrivetrainState_e prev_state); - void _handle_entry_logic(DrivetrainState_e new_state); void _set_drivetrain_disabled(); void _set_drivetrain_keepalive_idle(); void _set_enable_drivetrain_hv(); void _set_enable_drivetrain(); void _set_drivetrain_error_reset(); - - void _set_drivetrain_speed_command(DrivetrainSpeedCommand_s cmd); - void _set_drivetrain_torque_command(DrivetrainTorqueCommand_s cmd); + void _set_drivetrain_command(DrivetrainCommand_s cmd); DrivetrainState_e _evaluate_state_machine(CmdVariant cmd); @@ -170,11 +112,7 @@ class DrivetrainSystem std::function _check_inverter_error_flag; std::function _check_inverter_hv_present_flag; std::function _check_inverter_hv_not_present_flag; - std::function _check_inverter_enabled; - - std::function _set_gpio_state; - std::function _get_gpio_state; - + std::function _check_inverter_enabled; }; #endif /* DRIVETRAINSYSTEM */ \ No newline at end of file diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index b3ce969..3740749 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -4,14 +4,14 @@ DrivetrainSystem::DrivetrainSystem( veh_vec inverter_interfaces) - : _inverter_interfaces(inverter_interfaces), _state(DrivetrainState_e::NOT_CONNECTED), - _check_inverter_ready_flag([](const InverterStatus_s & status) -> bool {return status.inverter_ready;}), + : _inverter_interfaces(inverter_interfaces), _state(DrivetrainState_e::NOT_CONNECTED), + _check_inverter_ready_flag([](const InverterStatus_s & status) -> bool {return status.system_ready;}), _check_inverter_connected_flag([](const InverterStatus_s & status) -> bool {return status.connected;}), - _check_inverter_quit_dc_flag([](const InverterStatus_s & status) -> bool {return status.quit_dc;}), - _check_inverter_error_flag([](const InverterStatus_s & status) -> bool {return status.error_present;}), - _check_inverter_hv_present_flag([](const InverterStatus_s & status) -> bool {return status.hv_present;}), - _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return !status.hv_present;}), - _check_inverter_enabled([](const InverterStatus_s & status) -> bool {return !status.quit_inverter;}) { }; + _check_inverter_quit_dc_flag([](const InverterStatus_s & status) -> bool {return status.quit_dc_on;}), + _check_inverter_error_flag([](const InverterStatus_s & status) -> bool {return status.error;}), + _check_inverter_hv_present_flag([](const InverterStatus_s & status) -> bool {return status.dc_on;}), + _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return status.hv_present;}), // TODO fix this + _check_inverter_enabled([](const InverterStatus_s & status) -> bool {return !status.quit_inverter_on;}) { }; DrivetrainState_e DrivetrainSystem::get_state() @@ -63,7 +63,6 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm { _set_state(DrivetrainState_e::NOT_ENABLED_HV_PRESENT); } - _set_drivetrain_disabled(); // TODO dont know if this should be sent here, but it shouldn't hurt break; } @@ -151,10 +150,10 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm _set_drivetrain_disabled(); _set_state(DrivetrainState_e::ERROR); } - else if(requesting_init && hv_enabled && inverters_ready && hv_enabled && !inverters_enabled) + else if(requesting_init && hv_enabled && inverters_ready && !inverters_enabled) { _set_enable_drivetrain(); // should be done on entry of this state - } else if(hv_enabled && inverters_ready && hv_enabled && inverters_enabled) + } else if(hv_enabled && inverters_ready && inverters_enabled) { _set_enable_drivetrain(); _set_state(DrivetrainState_e::INVERTERS_ENABLED); @@ -175,10 +174,8 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm bool inverter_error_present = false; inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool requesting_speed_mode = false; - requesting_speed_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); - bool requesting_torque_mode = false; - requesting_torque_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); + bool requesting_drive = false; + requesting_drive = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_DRIVE_MODE); // for now i wont worry about checking whether or not the inverters are still enabled. // bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); @@ -188,63 +185,15 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm _set_drivetrain_disabled(); _set_state(DrivetrainState_e::ERROR); } - else if(requesting_speed_mode) + else if(requesting_drive) { - // on transition the pin will be written to the correct state (for speed mode it should be off) - _set_enable_drivetrain(); // this is just being verbose here, underlying on the inverter's interfaces all maintain - _set_state(DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE); - } else if(requesting_torque_mode) - { - // on transition the pin will be written to the correct state (for torq mode it should be on) - _set_enable_drivetrain(); - _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); - } - - break; - } - - case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: - { - bool inverter_error_present = false; - inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool inverters_in_speed_mode = false; - inverters_in_speed_mode = (!_get_gpio_state().torque_mode_enabled_pin_state); - - if(inverter_error_present) - { - _set_drivetrain_disabled(); - _set_state(DrivetrainState_e::ERROR); - } else if(inverters_in_speed_mode) - { - _set_enable_drivetrain(); - _set_state(DrivetrainState_e::ENABLED_SPEED_MODE); - } + _set_state(DrivetrainState_e::ENABLED_DRIVE_MODE); + } break; } - case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: - { - - bool inverter_error_present = false; - inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool inverters_in_torque_mode = false; - inverters_in_torque_mode = _get_gpio_state().torque_mode_enabled_pin_state; - - if(inverter_error_present) - { - _set_drivetrain_disabled(); - _set_state(DrivetrainState_e::ERROR); - } else if(inverters_in_torque_mode) - { - _set_enable_drivetrain(); - _set_state(DrivetrainState_e::ENABLED_TORQUE_MODE); - } - - break; - } - - case DrivetrainState_e::ENABLED_SPEED_MODE: + case DrivetrainState_e::ENABLED_DRIVE_MODE: { if(!(_check_inverter_flags(_check_inverter_hv_present_flag))) { @@ -252,14 +201,10 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm break; } - // TODO may need to verify that the gpio state is correct while in this state (GPIO torque mode low) bool inverter_error_present = false; - inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - // now finally in this mode and the ENABLED_TORQUE_MODE can we command the drivetrain - bool user_requesting_speed_command = false; - user_requesting_speed_command = etl::holds_alternative(cmd); - bool user_requesting_torque_mode = false; - user_requesting_torque_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + + bool valid_drivetrain_command = etl::holds_alternative(cmd); if(inverter_error_present) { @@ -267,41 +212,13 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm _set_state(DrivetrainState_e::ERROR); break; } - else if(user_requesting_speed_command) - { - _set_drivetrain_speed_command(etl::get(cmd)); - - } else if(user_requesting_torque_mode && !_drivetrain_active(_active_rpm_level)) - { - _set_drivetrain_keepalive_idle(); - _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); - } - break; - } - case DrivetrainState_e::ENABLED_TORQUE_MODE: - { - // TODO may need to verify that the gpio state is correct while in this state (GPIO torque mode high) - bool inverter_error_present = false; - inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - - bool user_requesting_torque_command = etl::holds_alternative(cmd); - bool user_requesting_speed_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); - - if(inverter_error_present) - { - _set_drivetrain_disabled(); - _set_state(DrivetrainState_e::ERROR); - } - else if(user_requesting_torque_command) - { - _set_drivetrain_torque_command(etl::get(cmd)); - } else if(user_requesting_speed_mode && !_drivetrain_active(_active_rpm_level)) - { - _set_drivetrain_keepalive_idle(); - _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); + else if (valid_drivetrain_command) { + DrivetrainCommand_s drivetrain_command = etl::get(cmd); + _set_drivetrain_command(drivetrain_command); } break; } + case DrivetrainState_e::ERROR: { bool user_requesting_error_reset = etl::holds_alternative(cmd) && (etl::get(cmd).reset_errors); @@ -326,82 +243,9 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm void DrivetrainSystem::_set_state(DrivetrainState_e new_state) { - _handle_exit_logic(_state); _state = new_state; - _handle_entry_logic(new_state); } -void DrivetrainSystem::_handle_exit_logic(DrivetrainState_e prev_state) -{ - switch (prev_state) - { - case DrivetrainState_e::NOT_CONNECTED: - break; - case DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT: - break; - case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: - break; - case DrivetrainState_e::INVERTERS_READY: - break; - case DrivetrainState_e::INVERTERS_HV_ENABLED: - break; - case DrivetrainState_e::INVERTERS_ENABLED: - break; - case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: - break; - case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: - break; - case DrivetrainState_e::ENABLED_SPEED_MODE: - break; - case DrivetrainState_e::ENABLED_TORQUE_MODE: - break; - case DrivetrainState_e::ERROR: - break; - default: - break; - } -} - -void DrivetrainSystem::_handle_entry_logic(DrivetrainState_e new_state) -{ - switch (new_state) - { - case DrivetrainState_e::NOT_CONNECTED: - break; - case DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT: - break; - case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: - break; - case DrivetrainState_e::INVERTERS_READY: - break; - case DrivetrainState_e::INVERTERS_HV_ENABLED: - break; - case DrivetrainState_e::INVERTERS_ENABLED: - break; - - case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: - { - // set the torque mode pin state to false (aka: keep the inverter in the main control mode which is speed mode) - _set_gpio_state({false}); - break; - } - - case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: - { - _set_gpio_state({true}); - break; - } - - case DrivetrainState_e::ENABLED_SPEED_MODE: - break; - case DrivetrainState_e::ENABLED_TORQUE_MODE: - break; - case DrivetrainState_e::ERROR: - break; - default: - break; - } -} // returns false if any of the inverters fail the flag check. bool DrivetrainSystem::_check_inverter_flags(std::function flag_check_func) { @@ -480,20 +324,12 @@ void DrivetrainSystem::_set_drivetrain_error_reset() func.set_idle(); } } -void DrivetrainSystem::_set_drivetrain_speed_command(DrivetrainSpeedCommand_s cmd) -{ - _inverter_interfaces.FL.set_speed(cmd.desired_speed_rpm.FL, cmd.torque_limit_nm.FL); - _inverter_interfaces.FR.set_speed(cmd.desired_speed_rpm.FR, cmd.torque_limit_nm.FR); - _inverter_interfaces.RL.set_speed(cmd.desired_speed_rpm.RL, cmd.torque_limit_nm.RL); - _inverter_interfaces.RR.set_speed(cmd.desired_speed_rpm.RR, cmd.torque_limit_nm.RR); -} - -void DrivetrainSystem::_set_drivetrain_torque_command(DrivetrainTorqueCommand_s cmd) +void DrivetrainSystem::_set_drivetrain_command(DrivetrainCommand_s cmd) { - _inverter_interfaces.FL.set_torque(cmd.desired_torque_nm.FL); - _inverter_interfaces.FR.set_torque(cmd.desired_torque_nm.FR); - _inverter_interfaces.RL.set_torque(cmd.desired_torque_nm.RL); - _inverter_interfaces.RR.set_torque(cmd.desired_torque_nm.RR); + _inverter_interfaces.FL.set_speed(cmd.desired_speeds.FL, cmd.torque_limits.FL); + _inverter_interfaces.FR.set_speed(cmd.desired_speeds.FR, cmd.torque_limits.FR); + _inverter_interfaces.RL.set_speed(cmd.desired_speeds.RL, cmd.torque_limits.RL); + _inverter_interfaces.RR.set_speed(cmd.desired_speeds.RR, cmd.torque_limits.RR); } bool DrivetrainSystem::_drivetrain_active(float max_active_rpm) @@ -502,8 +338,8 @@ bool DrivetrainSystem::_drivetrain_active(float max_active_rpm) auto funcs_arr = _inverter_interfaces.as_array(); for(const auto & func : funcs_arr) { - auto inv_status = func.get_status(); - if(inv_status.speed_rpm >= max_active_rpm) + auto motor_mechanics = func.get_motor_mechanics(); + if(motor_mechanics.actual_speed >= max_active_rpm) { return true; } diff --git a/platformio.ini b/platformio.ini index a811d7c..a10a734 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,14 +8,19 @@ lib_deps_shared = https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 - https://github.com/hytech-racing/shared_firmware_types.git + https://github.com/hytech-racing/shared_firmware_types.git#9f4f017e5fd2a359d1123df37efef017ed06222e https://github.com/ssilverman/QNEthernet#v0.26.0 https://github.com/hytech-racing/HT_SCHED Embedded Template Library@^20.39.4 + ; Teensy41 Environment. This environment is the primary environment for uploading code to the car. ; * Build to verify the on-car software. ; * UPLOAD to compile and upload on-car software. ; * TEST is not yet configured. + +[platformio] +default_envs = teensy41 + [env:teensy41] check_tool = clangtidy check_flags = @@ -38,7 +43,10 @@ test_ignore = test_systems lib_deps = ${common.lib_deps_shared} - https://github.com/hytech-racing/shared_firmware_interfaces.git + https://github.com/tonton81/FlexCAN_T4 + https://github.com/hytech-racing/shared_firmware_interfaces.git#4d7d617d63720f5fef586f1d95ecd979c1f8a075 + https://github.com/hytech-racing/HT_CAN/releases/download/141/can_lib.tar.gz + ; Test Systems Environment. This is only for compiling and uploading the hardware-abstracted code. ; * BUILD to verify the SYSTEMS compile. @@ -64,4 +72,4 @@ test_ignore= test_interfaces* lib_deps = google/googletest@^1.15.2 - ${common.lib_deps_shared} \ No newline at end of file + ${common.lib_deps_shared} diff --git a/src/main.cpp b/src/main.cpp index f2665b1..75f0054 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,7 +3,6 @@ #endif - /* From shared_firmware_types libdep */ #include "SharedFirmwareTypes.h" @@ -19,29 +18,25 @@ #include "VCR_Tasks.h" #include "TorqueControllerMux.hpp" +#include +#include +#include /* Scheduler setup */ HT_SCHED::Scheduler& scheduler = HT_SCHED::Scheduler::getInstance(); - - -/* Ethernet message sockets */ // TODO: Move this into its own interface -qindesign::network::EthernetUDP protobuf_send_socket; -qindesign::network::EthernetUDP protobuf_recv_socket; - - - -void setup() { - scheduler.setTimingFunction(micros); - - scheduler.schedule(tick_state_machine_task); - scheduler.schedule(read_adc0_task); - scheduler.schedule(read_adc1_task); - scheduler.schedule(update_buzzer_controller_task); - scheduler.schedule(kick_watchdog_task); +void setup(void) +{ + while (!Serial) + { + // wait for Arduino Serial Monitor to be ready + } + init_can_interface(); } +bool ran_test = false; +void loop() +{ -void loop() { - scheduler.run(); -} \ No newline at end of file + +}