diff --git a/firmware/projects/VD_Interface/CMakeLists.txt b/firmware/projects/VD_Interface/CMakeLists.txt new file mode 100644 index 00000000..dd02e5f1 --- /dev/null +++ b/firmware/projects/VD_Interface/CMakeLists.txt @@ -0,0 +1,20 @@ +# Teghveer Singh Ateliey +# January 4, 2025 + +# The target executable 'main' is created in the master CMakeLists. Do not change its name. +# We only need to add the source code files and include directories. + +# include("${CMAKE_SOURCE_DIR}/cmake/cangen.cmake") + +target_sources(main PRIVATE main.cc) + +target_include_directories(main + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${CMAKE_CURRENT_SOURCE_DIR}/generated/can +) + +message("SOURCES: ${SOURCES}") + +# Notice that we don't include any mcal/ subdirectory in this CMake file. +# The master CMakeLists handles platform selection and library linking. \ No newline at end of file diff --git a/firmware/projects/VD_Interface/config.yaml b/firmware/projects/VD_Interface/config.yaml new file mode 100644 index 00000000..db44df6a --- /dev/null +++ b/firmware/projects/VD_Interface/config.yaml @@ -0,0 +1,8 @@ +canGen: + busses: + - name: veh + node: fc + dbcFile: "../veh.dbc" + - name: pt + node: fc + dbcFile: "../pt.dbc" \ No newline at end of file diff --git a/firmware/projects/VD_Interface/inc/app.h b/firmware/projects/VD_Interface/inc/app.h new file mode 100644 index 00000000..cc0fb574 --- /dev/null +++ b/firmware/projects/VD_Interface/inc/app.h @@ -0,0 +1,237 @@ +/// @author Blake Freer +/// @date 2024-02-27 + +#pragma once + +#include +#include +#include +#include + +#include "app.h" +#include "shared/comms/can/can_bus.h" +#include "shared/comms/can/can_msg.h" +#include "shared/periph/adc.h" +#include "shared/periph/can.h" +#include "shared/periph/gpio.h" +#include "shared/util/mappers/linear_map.h" +#include "shared/util/mappers/mapper.h" +#include "shared/util/moving_average.h" +// #include "veh_can_messages.h" + +class AnalogInput { + static constexpr size_t kMovingAverageLength = 20; + +public: + AnalogInput(shared::periph::ADCInput& adc, + shared::util::Mapper* adc_to_position) + : adc_(adc), adc_to_position_(adc_to_position) {} + + double Update() { + uint32_t position = adc_.Read(); + moving_average_.LoadValue(uint16_t(position)); + return GetPosition(); + } + + /** + * @brief Get the position from the last `Update()` call. + */ + inline double GetPosition() { + return adc_to_position_->Evaluate(moving_average_.GetValue()); + } + +private: + shared::periph::ADCInput& adc_; + shared::util::MovingAverage moving_average_; + const shared::util::Mapper* adc_to_position_; +}; + +class Speaker { +public: + Speaker(shared::periph::DigitalOutput& digital_output) + : digital_output_(digital_output) {} + + void Update(bool state) const { + digital_output_.Set(state); + } + +private: + shared::periph::DigitalOutput& digital_output_; +}; + +class BrakeLight { +public: + BrakeLight(shared::periph::DigitalOutput& digital_output) + : digital_output_(digital_output) {} + + void Update(bool state) const { + digital_output_.Set(state); + } + +private: + shared::periph::DigitalOutput& digital_output_; +}; + +class Button { +public: + Button(shared::periph::DigitalInput& digital_input) + : digital_input_(digital_input) {} + + bool Read() const { + return digital_input_.Read(); + } + +private: + shared::periph::DigitalInput& digital_input_; +}; + +/** + * @brief Struct containing all data required for each simulink AMK input + * @note (SAM): Do not edit this struct, even though it will be very similar to + * the raw CAN message. Just copy over the CAN message fields to this struct in + * AMKMotor::UpdateInputs, then these values will be passed to the simulink + * input in main. + */ +struct AMKInput { + bool bReserve; + bool bSystemReady; + bool bError; + bool bWarn; + bool bQuitDcOn; + bool bDcOn; + bool bQuitInverterOn; + bool bInverterOn; + bool bDerating; + int16_t ActualVelocity; + int16_t TorqueCurrent; + int16_t MagnetizingCurrent; + int16_t TempMotor; + int16_t TempInverter; + uint16_t ErrorInfo; + int16_t TempIGBT; +}; + +struct AMKOutput { + uint8_t bInverterOn_tx; + uint8_t bDcOn_tx; + uint8_t bEnable; + uint8_t bErrorReset; + float TargetVelocity; + float TorqueLimitPositiv; + float TorqueLimitNegativ; +}; + +class AMKMotor { +public: + AMKMotor(shared::can::CanBus& can_bus, uint8_t amk_num) + : can_bus_(can_bus), amk_num_(amk_num) {} + + /** + * @brief NOT IMPLEMENTED + * @todo (SAM) construct and send a CAN message from the simulink output + * values in `output` + */ + void Transmit(AMKOutput output) {} + + /** + * @brief Read the motor + * + * @param input + */ + AMKInput UpdateInputs() { + /* (SAM) Fill in this section + // You will need to add an instance variable specifying which motor + // this is, so that you can properly + auto amk_can_input = CAN.GetAMKData(this_amk_number_); + + */ + return AMKInput{ + /* (SAM) populate from the CAN message + .bReserve = amk_can_input.reserve, // sample + .bSystemReady = amk_can_input.system_ready, // sample + .bError = 0, + .bWarn = 0, + .bQuitDcOn = 0, + .bDcOn = 0, + .bQuitInverterOn = 0, + .bInverterOn = 0, + .bDerating = 0, + .ActualVelocity = 0, + .TorqueCurrent = 0, + .MagnetizingCurrent = 0, + .TempMotor = 0, + .TempInverter = 0, + .ErrorInfo = 0, + .TempIGBT = 0, + */ + }; + } + +private: + shared::can::CanBus& can_bus_; + uint8_t amk_num_ = 0; +}; + +struct ContactorInput { + bool Pack_Precharge_Feedback; + bool Pack_Negative_Feedback; + bool Pack_Positive_Feedback; + bool HvilFeedback; + int8_t LowThermValue; + int8_t HighThermValue; + int8_t AvgThermValue; + double PackSOC; +}; + +struct ContactorOutput { + bool prechargeContactorCMD; + bool HVposContactorCMD; + bool HVnegContactorCMD; +}; + +class Contactors { +public: + Contactors(shared::can::CanBus& can_bus) : can_bus_(can_bus) {} + + ContactorInput ReadInput() { + /* (SAM) read from CAN and fill this struct */ + + // auto msg = can_base_.Read(...); + + return ContactorInput{ + // (SAM) populate from `msg` + .Pack_Precharge_Feedback = 0, .Pack_Negative_Feedback = 0, + .Pack_Positive_Feedback = 0, .HvilFeedback = 0, + .LowThermValue = 0, .HighThermValue = 0, + .AvgThermValue = 0, .PackSOC = 0, + }; + } + + // void Transmit(ContactorOutput output) { + // generated::can::ContactorStates contactor_states; + + // contactor_states.pack_negative = output.HVnegContactorCMD; + // contactor_states.pack_positive = output.HVposContactorCMD; + // contactor_states.pack_precharge = output.prechargeContactorCMD; + + // can_bus_.Send(contactor_states); + // } + +private: + shared::can::CanBus& can_bus_; +}; + +class StatusLight { +public: + StatusLight(shared::periph::DigitalOutput& digital_output) + : digital_output_(digital_output) {} + + void Toggle() { + digital_output_.Set(next_value_); + next_value_ = !next_value_; + } + +private: + bool next_value_ = true; + shared::periph::DigitalOutput& digital_output_; +}; \ No newline at end of file diff --git a/firmware/projects/VD_Interface/inc/simp_vd_interface.cc b/firmware/projects/VD_Interface/inc/simp_vd_interface.cc new file mode 100644 index 00000000..d8feae60 --- /dev/null +++ b/firmware/projects/VD_Interface/inc/simp_vd_interface.cc @@ -0,0 +1,49 @@ +/// @author Teghveer Singh Ateliey +/// @date 2024-11-23 + +#include "simp_vd_interface.h" + +using namespace ctrl; + +SimpVdInterface::SimpVdInterface(float target_slip) + : target_slip(target_slip) {} + +VdOutput SimpVdInterface::update(const VdInput& input, int time_ms) { + VdOutput output{ + .lm_torque_limit_positive = 0.0f, + .rm_torque_limit_positive = 0.0f, + .lm_torque_limit_negative = 0.0f, + .rm_torque_limit_negative = 0.0f, + .left_motor_speed_request = 1000, + .right_motor_speed_request = 1000, + }; + + const float pedal_torque_lut_data[][2] = { + {0.0f, 0.0f}, + {100.0f, 100.0f} + }; + + float motor_torque_request = ComputeTorqueRequest(input.driver_torque_request, + input.brake_pedal_postion); + float actual_slip = + CalculateActualSlip(input.wheel_speed_lr, input.wheel_speed_rr, + input.wheel_speed_lf, input.wheel_speed_rf); + float tc_scale_factor = CalculateTCScaleFactor(actual_slip, target_slip, time_ms); + + float tv_factor_left; + float tv_factor_right; + + float steering_angle = input.tv_enable ? input.steering_angle : 0.0f; + std::tie(tv_factor_left, tv_factor_right) = AdjustTorqueVectoring( + steering_angle, CreateTorqueVectoringFactor(steering_angle)); + + constexpr int pedal_torque_lut_length = (sizeof(pedal_torque_lut_data)) / (sizeof(pedal_torque_lut_data[0])); + const shared::util::LookupTable pedal_to_torque{pedal_torque_lut_data}; + + // Running avg calculation done within CalculateMotorTorque + std::tie(output.lm_torque_limit_positive, output.rm_torque_limit_positive) = + CalculateMotorTorque(pedal_to_torque.Evaluate(motor_torque_request * tc_scale_factor), + tv_factor_right, tv_factor_left); + + return output; +} \ No newline at end of file diff --git a/firmware/projects/VD_Interface/inc/simp_vd_interface.h b/firmware/projects/VD_Interface/inc/simp_vd_interface.h new file mode 100644 index 00000000..df375bff --- /dev/null +++ b/firmware/projects/VD_Interface/inc/simp_vd_interface.h @@ -0,0 +1,40 @@ +/// @author Teghveer Singh Ateliey +/// @date 2024-11-23 + +#pragma once + +#include "app.h" +#include "shared/controls/motor_torque.h" +#include "shared/controls/tc_scale_factor.h" +#include "shared/controls/tvFactor.h" +#include "shared/util/mappers/lookup_table.h" + +struct VdInput { + float driver_torque_request; + float brake_pedal_postion; + float steering_angle; + float wheel_speed_lr; + float wheel_speed_rr; + float wheel_speed_lf; + float wheel_speed_rf; + bool tv_enable; +}; + +struct VdOutput { + float lm_torque_limit_positive; + float rm_torque_limit_positive; + float lm_torque_limit_negative; + float rm_torque_limit_negative; + uint16_t left_motor_speed_request; + uint16_t right_motor_speed_request; +}; + +class SimpVdInterface { +public: + SimpVdInterface( + float target_slip = 0.2f); // default target slip is float 0.2 + VdOutput update(const VdInput& input, int time_ms); + +private: + float target_slip; +}; \ No newline at end of file diff --git a/firmware/projects/VD_Interface/inc/simp_vd_interface_test.cc b/firmware/projects/VD_Interface/inc/simp_vd_interface_test.cc new file mode 100644 index 00000000..9ad727c8 --- /dev/null +++ b/firmware/projects/VD_Interface/inc/simp_vd_interface_test.cc @@ -0,0 +1,189 @@ +/// @author Teghveer Singh Ateliey +/// @date 2024-11-23 + +#include "simp_vd_interface.h" + +#include +#include + +void test_1() { + SimpVdInterface simp_vd_int{}; + int time_ms = 0; + + VdOutput output_1 = simp_vd_int.update( + VdInput{ + .driver_torque_request = 0.0f, + .brake_pedal_postion = 0.0f, + .steering_angle = 0.0f, + .wheel_speed_lr = 0.0f, + .wheel_speed_rr = 0.0f, + .wheel_speed_lf = 0.0f, + .wheel_speed_rf = 0.0f, + .tv_enable = true + }, + time_ms); + + VdOutput output_1_expected{ + .lm_torque_limit_positive = 0.0f, + .rm_torque_limit_positive = 0.0f, + .lm_torque_limit_negative = 0.0f, + .rm_torque_limit_negative = 0.0f, + .left_motor_speed_request = 1000, + .right_motor_speed_request = 1000 + }; + + std::cout << "Output 1: (negative torque limits and speed requests are always constant)" << std::endl; + std::cout << "lm_torque_limit_positive: " << output_1.lm_torque_limit_positive << std::endl; + std::cout << "rm_torque_limit_positive: " << output_1.rm_torque_limit_positive << std::endl; + + assert(output_1.lm_torque_limit_positive == output_1_expected.lm_torque_limit_positive); + assert(output_1.lm_torque_limit_positive == output_1_expected.lm_torque_limit_positive); +} + +void test_2() { + SimpVdInterface simp_vd_int{}; + int time_ms = 0; + + VdOutput output_2 = simp_vd_int.update( + VdInput{ + .driver_torque_request = 100.0f, + .brake_pedal_postion = 0.0f, + .steering_angle = 0.0f, + .wheel_speed_lr = 0.0f, + .wheel_speed_rr = 0.0f, + .wheel_speed_lf = 0.0f, + .wheel_speed_rf = 0.0f, + .tv_enable = true + }, + time_ms); + + VdOutput output_2_expected{ + .lm_torque_limit_positive = 100.0f, + .rm_torque_limit_positive = 100.0f, + .lm_torque_limit_negative = 0.0f, + .rm_torque_limit_negative = 0.0f, + .left_motor_speed_request = 1000, + .right_motor_speed_request = 1000 + }; + + std::cout << "Output 1: (negative torque limits and speed requests are always constant)" << std::endl; + std::cout << "lm_torque_limit_positive: " << output_2.lm_torque_limit_positive << std::endl; + std::cout << "rm_torque_limit_positive: " << output_2.rm_torque_limit_positive << std::endl; + + assert(output_2.lm_torque_limit_positive == output_2_expected.lm_torque_limit_positive); + assert(output_2.lm_torque_limit_positive == output_2_expected.lm_torque_limit_positive); +} + +void test_3() { + SimpVdInterface simp_vd_int{}; + int time_ms = 0; + + VdOutput output_3 = simp_vd_int.update( + VdInput{ + .driver_torque_request = 100.0f, + .brake_pedal_postion = 0.0f, + .steering_angle = 25.0f, + .wheel_speed_lr = 0.0f, + .wheel_speed_rr = 0.0f, + .wheel_speed_lf = 0.0f, + .wheel_speed_rf = 0.0f, + .tv_enable = true + }, + time_ms); + + VdOutput output_3_expected{ + .lm_torque_limit_positive = 100.0f, + .rm_torque_limit_positive = 68.3f, + .lm_torque_limit_negative = 0.0f, + .rm_torque_limit_negative = 0.0f, + .left_motor_speed_request = 1000, + .right_motor_speed_request = 1000 + }; + + std::cout << "Output 1: (negative torque limits and speed requests are always constant)" << std::endl; + std::cout << "lm_torque_limit_positive: " << output_3.lm_torque_limit_positive << std::endl; + std::cout << "rm_torque_limit_positive: " << output_3.rm_torque_limit_positive << std::endl; + + assert(output_3.lm_torque_limit_positive == output_3_expected.lm_torque_limit_positive); + assert(output_3.lm_torque_limit_positive == output_3_expected.lm_torque_limit_positive); +} + +void test_4() { + SimpVdInterface simp_vd_int{}; + int time_ms = 0; + + VdOutput output_4 = simp_vd_int.update( + VdInput{ + .driver_torque_request = 50.0f, + .brake_pedal_postion = 0.0f, + .steering_angle = 0.0f, + .wheel_speed_lr = 10.0f, // actual slip 0.5 + .wheel_speed_rr = 10.0f, + .wheel_speed_lf = 15.0f, + .wheel_speed_rf = 15.0f, + .tv_enable = true + }, + time_ms); + + VdOutput output_4_expected{ + .lm_torque_limit_positive = 50.0f, + .rm_torque_limit_positive = 50.0f, + .lm_torque_limit_negative = 0.0f, + .rm_torque_limit_negative = 0.0f, + .left_motor_speed_request = 1000, + .right_motor_speed_request = 1000 + }; + + std::cout << "Output TC: (negative torque limits and speed requests are always constant)" << std::endl; + std::cout << "lm_torque_limit_positive: " << output_4.lm_torque_limit_positive << std::endl; + std::cout << "rm_torque_limit_positive: " << output_4.rm_torque_limit_positive << std::endl; + + assert(output_4.lm_torque_limit_positive == output_4_expected.lm_torque_limit_positive); + assert(output_4.rm_torque_limit_positive == output_4_expected.rm_torque_limit_positive); +} + +void test_5() { + SimpVdInterface simp_vd_int{}; + int time_ms = 50; + + VdOutput output_5 = simp_vd_int.update( + VdInput{ + .driver_torque_request = 50.0f, + .brake_pedal_postion = 0.0f, + .steering_angle = 0.0f, + .wheel_speed_lr = 10.0f, // actual slip 0.5 + .wheel_speed_rr = 10.0f, + .wheel_speed_lf = 15.0f, + .wheel_speed_rf = 15.0f, + .tv_enable = true + }, + time_ms); + + VdOutput output_5_expected{ + .lm_torque_limit_positive = 0.0f, // tc scale factor is 0 + .rm_torque_limit_positive = 0.0f, + .lm_torque_limit_negative = 0.0f, + .rm_torque_limit_negative = 0.0f, + .left_motor_speed_request = 1000, + .right_motor_speed_request = 1000 + }; + + std::cout << "Output TC: (negative torque limits and speed requests are always constant)" << std::endl; + std::cout << "lm_torque_limit_positive: " << output_5.lm_torque_limit_positive << std::endl; + std::cout << "rm_torque_limit_positive: " << output_5.rm_torque_limit_positive << std::endl; + + assert(output_5.lm_torque_limit_positive == output_5_expected.lm_torque_limit_positive); + assert(output_5.rm_torque_limit_positive == output_5_expected.rm_torque_limit_positive); +} + +int start_tests() { + test_1(); + test_2(); + test_3(); + test_4(); + test_5(); + + std::cout << "Testing done!" << std::endl; + + return 0; +} \ No newline at end of file diff --git a/firmware/projects/VD_Interface/main.cc b/firmware/projects/VD_Interface/main.cc new file mode 100644 index 00000000..6ebab1eb --- /dev/null +++ b/firmware/projects/VD_Interface/main.cc @@ -0,0 +1,12 @@ +/// @author Teghveer Singh Ateliey +/// @date 2025-01-04 + +#include "inc/simp_vd_interface_test.cc" +#include "inc/simp_vd_interface.h" +#include "inc/simp_vd_interface.cc" + +int main(void) { + start_tests(); + + return 0; +} \ No newline at end of file diff --git a/firmware/projects/VD_Interface/platforms/cli/CMakeLists.txt b/firmware/projects/VD_Interface/platforms/cli/CMakeLists.txt new file mode 100644 index 00000000..d25c4e45 --- /dev/null +++ b/firmware/projects/VD_Interface/platforms/cli/CMakeLists.txt @@ -0,0 +1,2 @@ +target_sources(bindings PRIVATE bindings.cc) +target_link_libraries(bindings PRIVATE mcal-cli) \ No newline at end of file diff --git a/firmware/projects/VD_Interface/platforms/cli/bindings.cc b/firmware/projects/VD_Interface/platforms/cli/bindings.cc new file mode 100644 index 00000000..560e5344 --- /dev/null +++ b/firmware/projects/VD_Interface/platforms/cli/bindings.cc @@ -0,0 +1,15 @@ +/// @author Teghveer Singh Ateliey +/// @date 2025-01-04 + +#include +#include +#include +#include +#include + +namespace bindings { +void Initialize() { + std::cout << "Initializing CLI..." << std::endl; + +} +} // namespace bindings \ No newline at end of file diff --git a/firmware/projects/VD_Interface/platforms/cli/mcal_conf.cmake b/firmware/projects/VD_Interface/platforms/cli/mcal_conf.cmake new file mode 100644 index 00000000..a30e90e1 --- /dev/null +++ b/firmware/projects/VD_Interface/platforms/cli/mcal_conf.cmake @@ -0,0 +1 @@ +set(MCAL cli) \ No newline at end of file