diff --git a/ETHERNET.md b/ETHERNET.md new file mode 100644 index 000000000..c43db2b89 --- /dev/null +++ b/ETHERNET.md @@ -0,0 +1,17 @@ +```mermaid +flowchart TD + ethernet[ethernet port] --> qnethernet[encoded ethernet data packets] + qnethernet --> port[protobuf union msgs port] + port --> ht_eth[HT ethernet interface] + ht_eth --> union_dec[union msgs splitter] + union_dec --> int1[interface 1 message buffer] + union_dec --> int2[interface 2 message buffer] + union_dec --> intN[interface N message buffer] +``` + +## explanation + +### receiving +Packets stream over ethernet and hit the ethernet port itself. The ethernet library has an internal queue for the UDP packets received. We know that a specific port id (say 4521) all messages will be a protobuf union msg that can contain only one of the types of messages that we will be sending (config control, TCU status, CASE msgs, etc.) and the union decoder method in the ethernet interface itself will handle this. The ethernet shall be able to receive multiple messages in one loop (of a limited number) and in between iterations of the loop the ethernet driver will hold the un-parsed messages in it's queue. + +The union message decoder will handle parsing of all of the messages in the queue and will be able to determine what message the union pb packet holds. If all messages of a specific type are needed, The message decoder will then add the decoded particular protobuf message struct to the message queue in the respective interface that way if multiple messages of the same type appear in one loop iteration they can all be processed by the underlying system / interface. If the interface just needs the latest version of a message each loop, the decoder will just update the message instance in its respective interface. diff --git a/README.md b/README.md index ed87478c5..a0c890c3d 100644 --- a/README.md +++ b/README.md @@ -296,3 +296,63 @@ new MCU code: - interface level: - hytech_can interface - spi interfaces: SPI adcs for load cells, steering input, glv, etc. + + +```mermaid +flowchart LR + subgraph user_inputs[user defined inputs] + simulink + o_params + CAN_data + other_protos + end + subgraph user_interact[user interfaces] + fxglv_live + mcap + database + param_server + + end + subgraph user_code_interact[user embedded code interfaces] + MCU + end + subgraph CASE_lib_repo[CASE lib repo gen] + CASE_lib + params + outputs + end + subgraph HT_params[HT_params repo gen] + param_defaults + nanopb + end + simulink[CASE simulink model] --> CASE_lib + simulink --> params[CASE defined params.json] + simulink --> outputs[CASE_outputs.proto] + params --> param_protos[params.proto] + params --> param_defaults[config defaults header] + param_defaults --> MCU + o_params[other params.json] --> param_protos + outputs --> h_proto + param_protos --> param_server[parameter server] + + h_proto --> data_acq + data_acq --> fxglv_live[live foxglove] + data_acq --> mcap[output mcap files] + CAN_data[CAN message definitions] --> can_protos[CAN messages in hytech.proto] + can_protos --> h_proto + mcap --> database[mcap metadata defined database] + CASE_lib --> MCU + CAN_data --> dbc_h[dbc to C code gen hytech.h for CAN] + param_protos --> h_proto[hytech.proto] + dbc_h --> MCU + h_proto --> nanopb[protobuf msg defs ht_eth.pb.h] + nanopb --> MCU + other_protos[other msgs.proto] + other_protos --> h_proto + + + + + + +``` \ No newline at end of file diff --git a/include/InterfaceParams.h b/include/InterfaceParams.h new file mode 100644 index 000000000..0c55b6e92 --- /dev/null +++ b/include/InterfaceParams.h @@ -0,0 +1,21 @@ +#ifndef INTERFACEPARAMS +#define INTERFACEPARAMS +#include "NativeEthernet.h" + +namespace EthParams +{ + uint8_t default_MCU_MAC_address[6] = + {0x04, 0xe9, 0xe5, 0x10, 0x1f, 0x22}; + + const IPAddress default_MCU_ip(192, 168, 1, 30); + const IPAddress default_TCU_ip(192, 168, 1, 68); + + const uint16_t default_protobuf_send_port = 2001; + const uint16_t default_protobuf_recv_port = 2000; + + const IPAddress default_netmask(255, 255, 255, 0); + const IPAddress default_gateway(192, 168, 0, 1); + + const uint16_t default_buffer_size = 512; +} +#endif \ No newline at end of file diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index dd0cc7752..1ca6f11fb 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -2,6 +2,10 @@ #define __MCU15_H__ #include "PedalsSystem.h" +#ifndef TESTING_SYSTEMS +#include "InterfaceParams.h" +#endif + // pindefs const int ADC1_CS = 34; @@ -26,6 +30,14 @@ const int MCU15_FR_POTS_CHANNEL = 1; const int MCU15_FL_LOADCELL_CHANNEL = 2; const int MCU15_FR_LOADCELL_CHANNEL = 2; +//MCU teensy analog channels +const int MCU15_TEENSY_ADC_CHANNELS = 2; +const int MCU15_THERM_FL = 38; +const int MCU15_THERM_FR = 41; +const int MCU15_THERM_FL_CHANNEL = 0; +const int MCU15_THERM_FR_CHANNEL = 1; + +const int DEFAULT_ANALOG_PINS[MCU15_TEENSY_ADC_CHANNELS] = {MCU15_THERM_FL, MCU15_THERM_FR}; // Time intervals const unsigned long SETUP_PRESENT_ACTION_INTERVAL = 1000; const unsigned long BUZZER_ON_INTERVAL = 2000; @@ -87,9 +99,10 @@ const float LOADCELL_RR_OFFSET = 23.761 / LOADCELL_RR_SCALE; // Steering parameters const float PRIMARY_STEERING_SENSE_OFFSET = 0.0; // units are degrees -const int SECONDARY_STEERING_SENSE_LEFTMOST_BOUND = 812; -const int SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND = 3179; -const int SECONDARY_STEERING_SENSE_CENTER = 1970; -const float STEERING_RANGE_DEGREES = 256.05f; +const int SECONDARY_STEERING_SENSE_LEFTMOST_BOUND = 785; // 794 // 812 // 130 deg +const int SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND = 3087; // 3075 // 3179 // 134 deg +const int SECONDARY_STEERING_SENSE_CENTER = 1945; // 1960 // 1970 +const float STEERING_RANGE_DEGREES = 257.0f; // 253.0f // 256.05f // 134+130-7(slop) +const float STEERING_IIR_ALPHA = 0.7f; // shaves off around 1 deg of max discrepancy #endif /* __MCU15_H__ */ \ No newline at end of file diff --git a/lib/interfaces/include/ParameterInterface.h b/lib/interfaces/include/ParameterInterface.h new file mode 100644 index 000000000..c6b0cac1c --- /dev/null +++ b/lib/interfaces/include/ParameterInterface.h @@ -0,0 +1,46 @@ +#ifndef PARAMETERINTERFACE +#define PARAMETERINTERFACE +#include "MCUStateMachine.h" +#include "ht_eth.pb.h" +#include "default_config.h" + +// yes, i know this is a singleton. im prototyping rn. +// TODO review if I can just give this a pointer to an ethernet port +class ParameterInterface +{ +public: + ParameterInterface(): current_car_state_(CAR_STATE::STARTUP), params_need_sending_(false), config_(DEFAULT_CONFIG) {} + + void update_car_state(const CAR_STATE& state) + { + current_car_state_ = state; + } + void update_config(const config &config) + { + if(static_cast(current_car_state_) < 5 ){ + config_ = config; + } + + } + config get_config() + { + return config_; + } + void set_params_need_sending() + { + params_need_sending_ = true; + } + void reset_params_need_sending() + { + params_need_sending_ = false; + } + bool params_need_sending() { return params_need_sending_; } + +private: + CAR_STATE current_car_state_; + bool params_need_sending_ = false; + config config_; + +}; + +#endif \ No newline at end of file diff --git a/lib/interfaces/include/ProtobufMsgInterface.h b/lib/interfaces/include/ProtobufMsgInterface.h new file mode 100644 index 000000000..ffa4e469d --- /dev/null +++ b/lib/interfaces/include/ProtobufMsgInterface.h @@ -0,0 +1,78 @@ +#ifndef PROTOBUFMSGINTERFACE +#define PROTOBUFMSGINTERFACE + +#include "ht_eth.pb.h" +#include "pb_encode.h" +#include "pb_decode.h" +#include "pb_common.h" +#include "ParameterInterface.h" +#include "circular_buffer.h" +#include "NativeEthernet.h" +#include "MCU_rev15_defs.h" + + +struct ETHInterfaces +{ + ParameterInterface* param_interface; +}; + +using recv_function_t = void (*)(const uint8_t* buffer, size_t packet_size, ETHInterfaces& interfaces); + +// this should be usable with arbitrary functions idk something +void handle_ethernet_socket_receive(EthernetUDP* socket, recv_function_t recv_function, ETHInterfaces& interfaces) +{ + int packet_size = socket->parsePacket(); + if(packet_size > 0) + { + Serial.println("packet size"); + Serial.println(packet_size); + uint8_t buffer[EthParams::default_buffer_size]; + size_t read_bytes = socket->read(buffer, sizeof(buffer)); + socket->read(buffer, UDP_TX_PACKET_MAX_SIZE); + recv_function(buffer, read_bytes, interfaces); + } +} + +template +bool handle_ethernet_socket_send_pb(EthernetUDP* socket, const pb_struct& msg, const pb_msgdesc_t* msg_desc) +{ + socket->beginPacket(EthParams::default_TCU_ip, EthParams::default_protobuf_send_port); + + uint8_t buffer[EthParams::default_buffer_size]; + pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); + if (!pb_encode(&stream, msg_desc, &msg)) { + // You can handle error more explicitly by looking at stream.errmsg + return false; + } + auto message_length = stream.bytes_written; + socket->write(buffer, message_length); + socket->endPacket(); + return true; +} + +// +void recv_pb_stream_union_msg(const uint8_t *buffer, size_t packet_size, ETHInterfaces& interfaces) +{ + pb_istream_t stream = pb_istream_from_buffer(buffer, packet_size); + HT_ETH_Union msg = HT_ETH_Union_init_zero; + if (pb_decode(&stream, HT_ETH_Union_fields, &msg)) + { + Serial.println("decoded!"); + + switch (msg.which_type_union) + { + case HT_ETH_Union_config__tag: + interfaces.param_interface->update_config(msg.type_union.config_); + break; + case HT_ETH_Union_get_config__tag: + interfaces.param_interface->set_params_need_sending(); + break; + default: + break; + } + } +} + + + +#endif \ No newline at end of file diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index fb40d4b0b..c94223e3e 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -29,6 +29,8 @@ struct TelemetryInterfaceReadChannels int current_channel; int current_ref_channel; int glv_sense_channel; + int therm_fl_channel; + int therm_fr_channel; }; class TelemetryInterface @@ -54,6 +56,10 @@ class TelemetryInterface /* Update CAN messages (main loop) */ // Interfaces + void update_front_thermistors_CAN_msg( + const AnalogConversion_s &therm_fl, + const AnalogConversion_s &therm_fr + ); void update_pedal_readings_CAN_msg( float accel_percent, float brake_percent, @@ -125,6 +131,7 @@ class TelemetryInterface const AnalogConversionPacket_s<8> &adc1, const AnalogConversionPacket_s<4> &adc2, const AnalogConversionPacket_s<4> &adc3, + const AnalogConversionPacket_s<2> &mcu_adc, const SteeringEncoderConversion_s &encoder, InvInt_t *fl, InvInt_t *fr, diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index 5c82fc80f..963a83740 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -1,5 +1,4 @@ #include "TelemetryInterface.h" - /* Update CAN messages */ // Main loop // MCP3208 returns structure @@ -58,6 +57,16 @@ void TelemetryInterface::update_analog_readings_CAN_msg(const SteeringEncoderCon enqueue_CAN(mcu_analog_readings_, ID_MCU_ANALOG_READINGS); } +void TelemetryInterface::update_front_thermistors_CAN_msg(const AnalogConversion_s &therm_fl, + const AnalogConversion_s &therm_fr) { + + FRONT_THERMISTORS_t front_thermistors_; + front_thermistors_.thermistor_motor_fl = therm_fl.raw; + front_thermistors_.thermistor_motor_fr = therm_fr.raw; + + enqueue_new_CAN(&front_thermistors_, &Pack_FRONT_THERMISTORS_hytech); +} + void TelemetryInterface::update_drivetrain_rpms_CAN_msg(InvInt_t* fl, InvInt_t* fr, InvInt_t* rl, InvInt_t* rr) { @@ -233,6 +242,7 @@ void TelemetryInterface::enqeue_controller_CAN_msg(const PIDTVTorqueControllerDa void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, const AnalogConversionPacket_s<4> &adc2, const AnalogConversionPacket_s<4> &adc3, + const AnalogConversionPacket_s<2> &mcu_adc, const SteeringEncoderConversion_s &encoder, InvInt_t* fl, InvInt_t* fr, @@ -279,5 +289,7 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, adc1.conversions[channels_.current_ref_channel]); enqeue_controller_CAN_msg(data); + update_front_thermistors_CAN_msg(mcu_adc.conversions[channels_.therm_fl_channel], + mcu_adc.conversions[channels_.therm_fr_channel]); } \ No newline at end of file diff --git a/lib/library.json b/lib/library.json deleted file mode 100644 index 61921be1f..000000000 --- a/lib/library.json +++ /dev/null @@ -1,52 +0,0 @@ -{ - "name": "CASE_lib", - "version": "1.0.0", - "build": { - "flags": [ - "-Istate_machine", - "-Istate_machine/include", - "-Iinterfaces", - "-Iinterfaces/include", - "-Iinterfaces/src", - "-Imock_interfaces", - "-Isystems", - "-Isystems/src", - "-Isystems/include", - "-Ishared_data", - "-Ishared_data/include", - "-ICASE_lib", - "-ICASE_lib/R2023b", - "-ICASE_lib/R2023b/simulink", - "-ICASE_lib/R2023b/simulink/include", - "-ICASE_lib/R2023b/rtw", - "-ICASE_lib/R2023b/rtw/c", - "-ICASE_lib/R2023b/rtw/c/src", - "-ICASE_lib/CASE", - "-ICASE_lib/CASE/HT08_CONTROL_SYSTEM_ert_rtw", - "-ICASE_lib/CASE/slprj", - "-ICASE_lib/CASE/slprj/ert", - "-ICASE_lib/CASE/slprj/ert/BasicVehicleMath", - "-ICASE_lib/CASE/slprj/ert/LAUNCH_CONTROL", - "-ICASE_lib/CASE/slprj/ert/NORMAL_FORCE_TV", - "-ICASE_lib/CASE/slprj/ert/PID_TV", - "-ICASE_lib/CASE/slprj/ert/POWER_LIMIT", - "-ICASE_lib/CASE/slprj/ert/_sharedutils", - "-ILateral Models", - "-ILateral Models/HT08_CONTROL_SYSTEM_ert_rtw", - "-ILateral Models/slprj", - "-ILateral Models/slprj/ert", - "-ILateral Models/slprj/ert/BasicVehicleMath", - "-ILateral Models/slprj/ert/LAUNCH_CONTROL", - "-ILateral Models/slprj/ert/NORMAL_FORCE_TV", - "-ILateral Models/slprj/ert/PID_TV", - "-ILateral Models/slprj/ert/POWER_LIMIT", - "-ILateral Models/slprj/ert/_sharedutils", - "-IR2023b", - "-IR2023b/simulink", - "-IR2023b/simulink/include", - "-IR2023b/rtw", - "-IR2023b/rtw/c", - "-IR2023b/rtw/c/src" - ] - } -} \ No newline at end of file diff --git a/lib/mock_interfaces/Filter_IIR.h b/lib/mock_interfaces/Filter_IIR.h new file mode 100644 index 000000000..2c9c7e69c --- /dev/null +++ b/lib/mock_interfaces/Filter_IIR.h @@ -0,0 +1,69 @@ +/* IIR digital low pass filter */ +#ifndef __FILTER_IIR__ +#define __FILTER_IIR__ + +#include + +#define DEFAULT_ALPHA 0.0 + +template +class Filter_IIR +{ + +public: + /** + * Constructors + */ + Filter_IIR(float alpha, dataType init_val=0) { + set_alpha(alpha); + prev_reading = init_val; + } + Filter_IIR() { + Filter_IIR(DEFAULT_ALPHA); + } + + void set_alpha(float alpha); + dataType get_prev_reading() const {return prev_reading;} + + dataType filtered_result(dataType new_val); + +private: + float alpha; + dataType prev_reading; +}; + +template +void Filter_IIR::set_alpha(float alpha) { + if (alpha > 1.0) { + this->alpha = 1.0; + } + else if (alpha < 0.0) { + this->alpha = 0.0; + } + else + { + this->alpha = alpha; + } +} + +template +dataType Filter_IIR::filtered_result(dataType new_val) { + prev_reading = (1 - alpha) * new_val + alpha * prev_reading; + + return prev_reading; +} + +template +class FilterIIRMulti +{ +protected: + Filter_IIR filter_channels_[N]; +public: + virtual void setAlphas(int channel, float alpha) + { + filter_channels_[channel].set_alpha(alpha); + } +}; + +#endif +#pragma once \ No newline at end of file diff --git a/lib/mock_interfaces/ParameterInterface.h b/lib/mock_interfaces/ParameterInterface.h new file mode 100644 index 000000000..c6b0cac1c --- /dev/null +++ b/lib/mock_interfaces/ParameterInterface.h @@ -0,0 +1,46 @@ +#ifndef PARAMETERINTERFACE +#define PARAMETERINTERFACE +#include "MCUStateMachine.h" +#include "ht_eth.pb.h" +#include "default_config.h" + +// yes, i know this is a singleton. im prototyping rn. +// TODO review if I can just give this a pointer to an ethernet port +class ParameterInterface +{ +public: + ParameterInterface(): current_car_state_(CAR_STATE::STARTUP), params_need_sending_(false), config_(DEFAULT_CONFIG) {} + + void update_car_state(const CAR_STATE& state) + { + current_car_state_ = state; + } + void update_config(const config &config) + { + if(static_cast(current_car_state_) < 5 ){ + config_ = config; + } + + } + config get_config() + { + return config_; + } + void set_params_need_sending() + { + params_need_sending_ = true; + } + void reset_params_need_sending() + { + params_need_sending_ = false; + } + bool params_need_sending() { return params_need_sending_; } + +private: + CAR_STATE current_car_state_; + bool params_need_sending_ = false; + config config_; + +}; + +#endif \ No newline at end of file diff --git a/lib/mock_interfaces/ProtobufMsgInterface.h b/lib/mock_interfaces/ProtobufMsgInterface.h new file mode 100644 index 000000000..75c8989b0 --- /dev/null +++ b/lib/mock_interfaces/ProtobufMsgInterface.h @@ -0,0 +1,3 @@ +#ifndef PROTOBUFMSGINTERFACE +#define PROTOBUFMSGINTERFACE +#endif \ No newline at end of file diff --git a/lib/mock_interfaces/SteeringEncoderInterface.h b/lib/mock_interfaces/SteeringEncoderInterface.h index bef37fc69..09a710834 100644 --- a/lib/mock_interfaces/SteeringEncoderInterface.h +++ b/lib/mock_interfaces/SteeringEncoderInterface.h @@ -20,15 +20,18 @@ struct SteeringEncoderConversion_s class SteeringEncoderInterface { public: +// Mock data + SteeringEncoderConversion_s mockConversion; // Functions - /// @brief Commands the underlying steering sensor to sample and hold the result - virtual void sample(); - /// @brief Calculate steering angle and whether result is in sensor's defined bounds. DOES NOT SAMPLE. - /// @return Calculated steering angle in degrees, upperSteeringStatus_s - virtual SteeringEncoderConversion_s convert(); - /// @brief Set the upper steering sensor's offset. 0 degrees should be centered. - /// @param newOffset - virtual void setOffset(float newOffset); + void sample() + { + return; + }; + SteeringEncoderConversion_s convert() + { + return mockConversion; + }; + void setOffset(float newOffset); }; #endif /* __UPPERSTEERINGSENSOR_H__ */ diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index 8cf556e1a..9207da8d5 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -7,6 +7,8 @@ #include "DrivetrainSystem.h" #include "SteeringSystem.h" #include "MCUStateMachine.h" +#include "ProtobufMsgInterface.h" +#include "ParameterInterface.h" struct CASEConfiguration { @@ -14,7 +16,10 @@ struct CASEConfiguration float yaw_pid_p; float yaw_pid_i; float yaw_pid_d; - float tcs_pid_p; + float tcs_pid_p_lowerBound_front; + float tcs_pid_p_upperBound_front; + float tcs_pid_p_lowerBound_rear; + float tcs_pid_p_upperBound_rear; float tcs_pid_i; float tcs_pid_d; bool useLaunch; @@ -47,6 +52,37 @@ struct CASEConfiguration float DriveTorquePercentFront; float BrakeTorquePercentFront; float MechPowerMaxkW; + float launchLeftRightMaxDiff; + float tcs_pid_lower_rpm_front; + float tcs_pid_upper_rpm_front; + float tcs_pid_lower_rpm_rear; + float tcs_pid_upper_rpm_rear; + float maxNormalLoadBrakeScalingFront; + float tcs_saturation_front; + float tcs_saturation_rear; + float TCSGenLeftRightDiffLowerBound; + float TCSGenLeftRightDiffUpperBound; + float TCSWheelSteerLowerBound; + float TCSWheelSteerUpperBound; + bool useRPM_TCS_GainSchedule; + bool useNL_TCS_GainSchedule; + float TCS_NL_startBoundPerc_FrontAxle; + float TCS_NL_endBoundPerc_FrontAxle; + float TCS_NL_startBoundPerc_RearAxle; + float TCS_NL_endBoundPerc_RearAxle; + bool useNL_TCS_SlipSchedule; + float launchSL_startBound_Front; + float launchSL_endBound_Front; + float launchSL_startBound_Rear; + float launchSL_endBound_Rear; + float TCS_SL_startBound_Front; + float TCS_SL_endBound_Front; + float TCS_SL_startBound_Rear; + float TCS_SL_endBound_Rear; + float TCS_SL_NLPerc_startBound_Front; + float TCS_SL_NLPerc_endBound_Front; + float TCS_SL_NLPerc_startBound_Rear; + float TCS_SL_NLPerc_endBound_Rear; float max_rpm; float max_regen_torque; @@ -68,6 +104,7 @@ class CASESystem message_queue *can_queue, unsigned long controller_send_period_ms, unsigned long vehicle_math_offset_ms, + unsigned long lowest_controller_send_period_ms, CASEConfiguration config) { msg_queue_ = can_queue; @@ -76,10 +113,13 @@ class CASESystem config_ = config; last_controller_pt1_send_time_ = 0; last_controller_pt2_send_time_ = 0; + last_controller_pt3_send_time_ = 0; + last_lowest_priority_controller_send_time_ = 0; controller_send_period_ms_ = controller_send_period_ms; last_vehm_send_time_ = 0; vehicle_math_offset_ms_ = vehicle_math_offset_ms; + lowest_priority_controller_send_period_ms_ = lowest_controller_send_period_ms; } /// @brief function that evaluates the CASE (controller and state estimation) system. updates the internal pstate_ and returns controller result @@ -105,26 +145,77 @@ class CASESystem bool start_button_pressed, uint8_t vn_status); - void update_pid(float yaw_p, float yaw_i, float yaw_d, float tcs_p, float tcs_i, float tcs_d, float brake_p, float brake_i, float brake_d) - { - config_.yaw_pid_p = yaw_p; - config_.yaw_pid_p = yaw_i; - config_.yaw_pid_p = yaw_d; + // void update_pid(float yaw_p, float yaw_i, float yaw_d, float tcs_p, float tcs_i, float tcs_d, float brake_p, float brake_i, float brake_d) + // { + // config_.yaw_pid_p = yaw_p; + // config_.yaw_pid_p = yaw_i; + // config_.yaw_pid_p = yaw_d; - config_.tcs_pid_p = tcs_p; - config_.tcs_pid_i = tcs_i; - config_.tcs_pid_d = tcs_d; + // config_.tcs_pid_p = tcs_p; + // config_.tcs_pid_i = tcs_i; + // config_.tcs_pid_d = tcs_d; + + // config_.yaw_pid_brakes_p = brake_p; + // config_.yaw_pid_brakes_i = brake_i; + // config_.yaw_pid_brakes_d = brake_d; + // } - config_.yaw_pid_brakes_p = brake_p; - config_.yaw_pid_brakes_i = brake_i; - config_.yaw_pid_brakes_d = brake_d; - } float calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_regen_torque, float max_rpm); /// @brief configuration function to determine what CASE is using / turn on and off different features within CASE /// @param config the configuration struct we will be setting - void configure(const CASEConfiguration &config) + + void update_config_from_param_interface(ParameterInterface ¶m_interface_ref) { - config_ = config; + config cfg = param_interface_ref.get_config(); + config_.AbsoluteTorqueLimit = cfg.AbsoluteTorqueLimit; + config_.yaw_pid_p = cfg.yaw_pid_p; + config_.yaw_pid_i = cfg.yaw_pid_i; + config_.yaw_pid_d = cfg.yaw_pid_d; + config_.tcs_pid_p_lowerBound_front = cfg.tcs_pid_p_lowerBound_front; + config_.tcs_pid_p_upperBound_front = cfg.tcs_pid_p_upperBound_front; + config_.tcs_pid_p_lowerBound_rear = cfg.tcs_pid_p_lowerBound_rear; + config_.tcs_pid_p_upperBound_rear = cfg.tcs_pid_p_upperBound_rear; + config_.tcs_pid_i = cfg.tcs_pid_i; + config_.tcs_pid_d = cfg.tcs_pid_d; + config_.useLaunch = cfg.useLaunch; + config_.usePIDTV = cfg.usePIDTV; + config_.useTCSLimitedYawPID = cfg.useTCSLimitedYawPID; + config_.useNormalForce = cfg.useNormalForce; + config_.useTractionControl = cfg.useTractionControl; + config_.usePowerLimit = cfg.usePowerLimit; + config_.usePIDPowerLimit = cfg.usePIDPowerLimit; + config_.useDecoupledYawBrakes = cfg.useDecoupledYawBrakes; + config_.useDiscontinuousYawPIDBrakes = cfg.useDiscontinuousYawPIDBrakes; + config_.tcsSLThreshold = cfg.tcsSLThreshold; + config_.launchSL = cfg.launchSL; + config_.launchDeadZone = cfg.launchDeadZone; + config_.launchVelThreshold = cfg.launchVelThreshold; + config_.tcsVelThreshold = cfg.tcsVelThreshold; + config_.yawPIDMaxDifferential = cfg.yawPIDMaxDifferential; + config_.yawPIDErrorThreshold = cfg.yawPIDErrorThreshold; + config_.yawPIDVelThreshold = cfg.yawPIDVelThreshold; + config_.yawPIDCoastThreshold = cfg.yawPIDCoastThreshold; + config_.yaw_pid_brakes_p = cfg.yaw_pid_brakes_p; + config_.yaw_pid_brakes_i = cfg.yaw_pid_brakes_i; + config_.yaw_pid_brakes_d = cfg.yaw_pid_brakes_d; + config_.decoupledYawPIDBrakesMaxDIfference = cfg.decoupledYawPIDBrakesMaxDIfference; + config_.discontinuousBrakesPercentThreshold = cfg.discontinuousBrakesPercentThreshold; + config_.TorqueMode = cfg.TorqueMode; + config_.RegenLimit = cfg.RegenLimit; + config_.useNoRegen5kph = cfg.useNoRegen5kph; + config_.useTorqueBias = cfg.useTorqueBias; + config_.DriveTorquePercentFront = cfg.DriveTorquePercentFront; + config_.BrakeTorquePercentFront = cfg.BrakeTorquePercentFront; + config_.MechPowerMaxkW = cfg.MechPowerMaxkW; + config_.launchLeftRightMaxDiff = cfg.launchLeftRightMaxDiff; + config_.tcs_pid_lower_rpm_front = cfg.tcs_pid_lower_rpm_front; + config_.tcs_pid_upper_rpm_front = cfg.tcs_pid_upper_rpm_front; + config_.tcs_pid_lower_rpm_rear = cfg.tcs_pid_lower_rpm_rear; + config_.tcs_pid_upper_rpm_rear = cfg.tcs_pid_upper_rpm_rear; + config_.maxNormalLoadBrakeScalingFront = cfg.maxNormalLoadBrakeScalingFront; + config_.max_rpm = cfg.max_rpm; + config_.max_regen_torque = cfg.max_regen_torque; + config_.max_torque = cfg.max_torque; } float get_rpm_setpoint(float final_torque) { @@ -143,7 +234,7 @@ class CASESystem message_queue *msg_queue_; HT08_CASE case_; - unsigned long vn_active_start_time_, last_eval_time_, vehicle_math_offset_ms_, last_controller_pt1_send_time_, last_controller_pt2_send_time_, last_controller_pt3_send_time_, last_vehm_send_time_, controller_send_period_ms_; + unsigned long vn_active_start_time_, last_eval_time_, vehicle_math_offset_ms_, last_controller_pt1_send_time_, last_controller_pt2_send_time_, last_controller_pt3_send_time_, last_vehm_send_time_, controller_send_period_ms_, lowest_priority_controller_send_period_ms_, last_lowest_priority_controller_send_time_; }; #include "CASESystem.tpp" diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 663c9cfa5..38f914700 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -15,13 +15,16 @@ DrivetrainCommand_s CASESystem::evaluate( { HT08_CASE::ExtU_HT08_CASE_T in; - // in. as defined in HT08_CASE.h + // in. as defined in HT08_CASE.h, ExtU_HT08_CASE_T in.SteeringWheelAngleDeg = steering_data.angle; in.TorqueAverageNm = calculate_torque_request(pedals_data, config_.max_regen_torque, config_.max_rpm); in.YawRaterads = vn_data.angular_rates.z; + // FAKE + // in.YawRaterads = 2.5; + // REAL in.Vx_B = vn_data.velocity_x; @@ -42,8 +45,8 @@ DrivetrainCommand_s CASESystem::evaluate( in.MotorOmegaRRrpm = drivetrain_data.measuredSpeeds[3]; // FAKE, 566.273 rpm = 1 m/s - // in.MotorOmegaFLrpm = 566.27330024 * 1.36; - // in.MotorOmegaFRrpm = 566.27330024 * 1.36; + // in.MotorOmegaFLrpm = 566.27330024 * 5; + // in.MotorOmegaFRrpm = 566.27330024 * 10; // in.MotorOmegaRRrpm = 566.27330024 * 1.36; // in.MotorOmegaRLrpm = 566.27330024 * 1.36; @@ -69,16 +72,26 @@ DrivetrainCommand_s CASESystem::evaluate( in.useTractionControl = config_.useTractionControl; - in.TCS_SLThreshold = config_.tcsSLThreshold; - in.LaunchSL = config_.launchSL; + in.TCS_SL_Targets[0] = config_.TCS_SL_startBound_Front; + in.TCS_SL_Targets[1] = config_.TCS_SL_endBound_Front; + in.TCS_SL_Targets[2] = config_.TCS_SL_startBound_Rear; + in.TCS_SL_Targets[3] = config_.TCS_SL_endBound_Rear; + + in.launchSL_Targets[0] = config_.launchSL_startBound_Front; + in.launchSL_Targets[1] = config_.launchSL_endBound_Front; + in.launchSL_Targets[2] = config_.launchSL_startBound_Rear; + in.launchSL_Targets[3] = config_.launchSL_endBound_Rear; + in.LaunchDeadZone = config_.launchDeadZone; - in.TCSPIDConfig[0] = config_.tcs_pid_p; - in.TCSPIDConfig[1] = config_.tcs_pid_i; - in.TCSPIDConfig[2] = config_.tcs_pid_d; + in.TCSPIDConfig[0] = config_.tcs_pid_p_lowerBound_front; + in.TCSPIDConfig[1] = config_.tcs_pid_p_upperBound_front; + in.TCSPIDConfig[2] = config_.tcs_pid_p_lowerBound_rear; + in.TCSPIDConfig[3] = config_.tcs_pid_p_upperBound_rear; + in.TCSPIDConfig[4] = config_.tcs_pid_i; + in.TCSPIDConfig[5] = config_.tcs_pid_d; in.LaunchVelThreshold = config_.launchVelThreshold; - in.TCSVelThreshold = config_.tcsVelThreshold; in.YawPIDErrorThreshold = config_.yawPIDErrorThreshold; in.YawPIDVelThreshold = config_.yawPIDVelThreshold; @@ -114,6 +127,43 @@ DrivetrainCommand_s CASESystem::evaluate( in.MechPowerMaxkW = config_.MechPowerMaxkW; + in.launchLeftRightMaxDiff = config_.launchLeftRightMaxDiff; + + in.TCS_PID_Motor_RPM_Schedule[0] = config_.tcs_pid_lower_rpm_front; + in.TCS_PID_Motor_RPM_Schedule[1] = config_.tcs_pid_upper_rpm_front; + in.TCS_PID_Motor_RPM_Schedule[2] = config_.tcs_pid_lower_rpm_rear; + in.TCS_PID_Motor_RPM_Schedule[3] = config_.tcs_pid_upper_rpm_rear; + + in.maxNormalLoadBrakeScalingFront = config_.maxNormalLoadBrakeScalingFront; + + in.TCS_Saturation_Front = config_.tcs_saturation_front; + + in.TCS_Saturation_Rear = config_.tcs_saturation_rear; + + in.TCSGenLeftRightDiffLowerBound = config_.TCSGenLeftRightDiffLowerBound; + + in.TCSGenLeftRightDiffUpperBound = config_.TCSGenLeftRightDiffUpperBound; + + in.TCSWheelSteerLowerBound = config_.TCSWheelSteerLowerBound; + + in.TCSWheelSteerUpperBound = config_.TCSWheelSteerUpperBound; + + in.useRPM_TCS_GainSchedule = config_.useRPM_TCS_GainSchedule; + + in.useNL_TCS_GainSchedule = config_.useNL_TCS_GainSchedule; + + in.TCS_PID_NL_Schedule[0] = config_.TCS_NL_startBoundPerc_FrontAxle; + in.TCS_PID_NL_Schedule[1] = config_.TCS_NL_endBoundPerc_FrontAxle; + in.TCS_PID_NL_Schedule[2] = config_.TCS_NL_startBoundPerc_RearAxle; + in.TCS_PID_NL_Schedule[3] = config_.TCS_NL_endBoundPerc_RearAxle; + + in.TCS_SL_Targets_NLSchedule[0] = config_.TCS_SL_NLPerc_startBound_Front; + in.TCS_SL_Targets_NLSchedule[1] = config_.TCS_SL_NLPerc_endBound_Front; + in.TCS_SL_Targets_NLSchedule[2] = config_.TCS_SL_NLPerc_startBound_Rear; + in.TCS_SL_Targets_NLSchedule[3] = config_.TCS_SL_NLPerc_endBound_Rear; + + in.useNL_TCS_SlipSchedule = config_.useNL_TCS_SlipSchedule; + if ((vn_active_start_time_ == 0) && (vn_status >= 2)) { vn_active_start_time_ = tick.millis; @@ -140,11 +190,11 @@ DrivetrainCommand_s CASESystem::evaluate( if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_)) { - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl); last_controller_pt1_send_time_ = tick.millis; } @@ -158,6 +208,7 @@ DrivetrainCommand_s CASESystem::evaluate( enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pn); last_controller_pt2_send_time_ = tick.millis; } @@ -186,10 +237,24 @@ DrivetrainCommand_s CASESystem::evaluate( enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_kin_desired_); enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_beta_deg); enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_lin_ve); - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co); + last_vehm_send_time_ = tick.millis; } + if ((tick.millis - last_lowest_priority_controller_send_time_) >= (lowest_priority_controller_send_period_ms_)) + { + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_yaw_pi); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sa); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_di); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_rp); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_nl); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tc_pna); + + last_lowest_priority_controller_send_time_ = tick.millis; + } + DrivetrainCommand_s command; command.torqueSetpoints[0] = res.FinalTorqueFL; diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index afd664819..c2fa7f7c5 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -3,14 +3,17 @@ #include "SteeringEncoderInterface.h" #include "AnalogSensorsInterface.h" +#include "Filter_IIR.h" #include "SysClock.h" // Digital Encoder = Primary Sensor // Analog Encoder = Secondary Sensor // Definitions // TODO: evalaute reasonable thresholds for agreement -#define STEERING_DIVERGENCE_ERROR_THRESHOLD (14.0) // Steering sensors can disagree by 5 degrees before output is considered erroneous -#define STEERING_DIVERGENCE_WARN_THRESHOLD (8.0) // Warning condition will be raised when steering sensors diverge 2.5 degrees +#define STEERING_DIVERGENCE_ERROR_THRESHOLD (16.0) // Steering sensors can disagree by x degrees before output is considered erroneous +#define STEERING_DIVERGENCE_WARN_THRESHOLD (8.0) // Warning condition will be raised when steering sensors diverge x degrees +#define NUM_SENSORS 2 +#define DEFAULT_STEERING_ALPHA (0.0) // Enums enum class SteeringSystemStatus_e @@ -40,11 +43,32 @@ class SteeringSystem SteeringEncoderInterface *primarySensor_; SteeringEncoderConversion_s primaryConversion_; SteeringSystemData_s steeringData_; + + /** + * Utility digital IIR filters + * 0 : primary sensor filter + * 1 : secondary sensor filter + */ + Filter_IIR steeringFilters_[NUM_SENSORS]; + float filteredAnglePrimary_; + float filteredAngleSecondary_; public: SteeringSystem(SteeringEncoderInterface *primarySensor) - : primarySensor_(primarySensor) + : SteeringSystem(primarySensor, DEFAULT_STEERING_ALPHA) + {} + + SteeringSystem(SteeringEncoderInterface *primarySensor, float filterAlpha) + : SteeringSystem(primarySensor, filterAlpha, filterAlpha) {} + SteeringSystem(SteeringEncoderInterface *primarySensor, float filterAlphaPrimary, float filterAlphaSecondary) + : primarySensor_(primarySensor) + { + steeringFilters_[0] = Filter_IIR(filterAlphaPrimary); + steeringFilters_[1] = Filter_IIR(filterAlphaSecondary); + + } + /// @brief Computes steering angle and status of the steering system. /// @param secondaryAngle The computed steering angle as reported by the secondary steering sensor. /// @return SteeringSystemOutput_s contains steering angle and SteeringSystemStatus_e diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index e5dcc4fba..18ed91e28 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -15,7 +15,7 @@ #include "accel_lookup.h" #include "TorqueControllersData.h" -#include "PID_TV.h" + /* CONTROLLER CONSTANTS */ diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 72727354a..32439736e 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -12,15 +12,19 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake) primarySensor_->sample(); primaryConversion_ = primarySensor_->convert(); + // Filter both sensor angle readings + filteredAnglePrimary_ = steeringFilters_[0].filtered_result(primaryConversion_.angle); + filteredAngleSecondary_ = steeringFilters_[1].filtered_result(intake.secondaryConversion.conversion); + // Both sensors are nominal and agree if ( (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL) && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD) - && (std::abs(primaryConversion_.angle - intake.secondaryConversion.conversion) < STEERING_DIVERGENCE_WARN_THRESHOLD) + && (std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_WARN_THRESHOLD) ) { steeringData_ = { - .angle = primaryConversion_.angle, + .angle = filteredAnglePrimary_, .status = SteeringSystemStatus_e::STEERING_SYSTEM_NOMINAL }; } @@ -28,12 +32,12 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake) // Sensors disagree by STEERING_DIVERGENCE_WARN_THRESHOLD degrees and less than STEERING_DIVERGENCE_ERROR_THRESHOLD degrees else if ( (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL) - || (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED) - || ((std::abs(primaryConversion_.angle - intake.secondaryConversion.conversion) >= STEERING_DIVERGENCE_WARN_THRESHOLD) && (std::abs(primaryConversion_.angle - intake.secondaryConversion.conversion) < STEERING_DIVERGENCE_ERROR_THRESHOLD)) + || ((primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL) && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED)) + || ((std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) >= STEERING_DIVERGENCE_WARN_THRESHOLD) && (std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_ERROR_THRESHOLD)) ) { steeringData_ = { - .angle = primaryConversion_.angle, + .angle = filteredAnglePrimary_, .status = SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL }; } @@ -44,7 +48,7 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake) ) { steeringData_ = { - .angle = intake.secondaryConversion.conversion, + .angle = filteredAngleSecondary_, .status = SteeringSystemStatus_e::STEERING_SYSTEM_DEGRADED }; } @@ -57,5 +61,11 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake) .status = SteeringSystemStatus_e::STEERING_SYSTEM_ERROR }; } + + // For possible bottom sensor recalibration + // TODO: Remove me once done! + // Serial.print("Secondary sensor raw: "); + // Serial.println(intake.secondaryConversion.raw); + // Serial.println(); } } diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index f246e0e62..96b8b705f 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -71,11 +71,11 @@ void TorqueControllerMux::tick( // Check if targeted controller is ready to be selected bool controllerNotReadyPreventsModeChange = (controllerOutputs_[static_cast(dialModeMap_[dashboardDialMode])].ready == false); - if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange || controllerNotReadyPreventsModeChange)) - { - muxMode_ = dialModeMap_[dashboardDialMode]; - cur_dial_mode_ = dashboardDialMode; - } + // if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange || controllerNotReadyPreventsModeChange)) + // { + muxMode_ = dialModeMap_[dashboardDialMode]; + cur_dial_mode_ = dashboardDialMode; + // } } // Check if the current controller is ready. If it has faulted, revert to safe mode @@ -88,10 +88,13 @@ void TorqueControllerMux::tick( drivetrainCommand_ = controllerOutputs_[static_cast(muxMode_)].command; - // apply torque limit before power limit to not power limit + // Apply setpoints value limits + // Safety checks for CASE: CASE handles regen, torque, and power limit internally applyRegenLimit(&drivetrainCommand_, &drivetrainData); + // Apply torque limit before power limit to not power limit applyTorqueLimit(&drivetrainCommand_); applyPowerLimit(&drivetrainCommand_, &drivetrainData); + // Uniformly apply speed limit to all controller modes applyPosSpeedLimit(&drivetrainCommand_); } } @@ -99,6 +102,12 @@ void TorqueControllerMux::tick( /* Apply limit to make sure that regenerative braking is not applied when wheelspeed is below 5kph on all wheels. + + FSAE rules: + EV.3.3.3 The powertrain must not regenerate energy when vehicle speed is between 0 and 5 km/hr + Assumption: + Assuming there won't be a scenario where there are positive and negative setpoints simultaneously + AND vehicle speed is < 5km/h */ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s *command, const DrivetrainDynamicReport_s *drivetrain) { @@ -110,13 +119,13 @@ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s *command, const Dr for (int i = 0; i < NUM_MOTORS; i++) { - #ifdef ARDUINO_TEENSY41 +#ifdef ARDUINO_TEENSY41 maxWheelSpeed = std::max(maxWheelSpeed, abs(drivetrain->measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); allWheelsRegen &= (command->speeds_rpm[i] < abs(drivetrain->measuredSpeeds[i]) || command->speeds_rpm[i] == 0); - #else +#else maxWheelSpeed = std::max(maxWheelSpeed, std::abs(drivetrain->measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); allWheelsRegen &= (command->speeds_rpm[i] < std::abs(drivetrain->measuredSpeeds[i]) || command->speeds_rpm[i] == 0); - #endif +#endif } // begin limiting regen at noRegenLimitKPH and completely limit regen at fullRegenLimitKPH @@ -145,11 +154,11 @@ void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s *command, const Dr // calculate current mechanical power for (int i = 0; i < NUM_MOTORS; i++) { - // get the total magnitude of torque from all 4 wheels - #ifdef ARDUINO_TEENSY41 // screw arduino.h macros +// get the total magnitude of torque from all 4 wheels +#ifdef ARDUINO_TEENSY41 // screw arduino.h macros net_torque_mag += abs(command->torqueSetpoints[i]); net_power += abs(command->torqueSetpoints[i] * (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); - #else +#else // sum up net torque net_torque_mag += std::abs(command->torqueSetpoints[i]); // calculate P = T*w for each wheel and sum together @@ -171,15 +180,14 @@ void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s *command, const Dr // based on the torque percent and max power limit, get the max power each wheel can use float power_per_corner = (torque_percent * MAX_POWER_LIMIT); - // power / omega (motor rad/s) to get torque per wheel - #ifdef ARDUINO_TEENSY41 +// power / omega (motor rad/s) to get torque per wheel +#ifdef ARDUINO_TEENSY41 command->torqueSetpoints[i] = abs(power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); - #else +#else command->torqueSetpoints[i] = std::abs(power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); - #endif +#endif command->torqueSetpoints[i] = std::max(0.0f, std::min(command->torqueSetpoints[i], getMaxTorque())); - - } + } } } @@ -218,7 +226,9 @@ void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s *command) } } -/* Apply limit such that wheelspeed never goes negative */ +/** + * Apply limit such that wheelspeed never goes negative + */ void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s *command) { for (int i = 0; i < NUM_MOTORS; i++) diff --git a/platformio.ini b/platformio.ini index e070b9035..5d9237b66 100644 --- a/platformio.ini +++ b/platformio.ini @@ -11,9 +11,8 @@ lib_ignore = test_ignore= test_interfaces* lib_deps= - git+ssh://git@github.com/hytech-racing/CASE_lib.git#v39 + git+ssh://git@github.com/hytech-racing/CASE_lib.git#v45 https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 - [env:teensy41] ; Testing variables @@ -30,7 +29,9 @@ build_src_filter = -<**/*.cpp> + build_unflags = -std=gnu++11 -build_flags = -std=c++17 +build_flags = + -std=c++17 + -D TEENSY_OPT_SMALLEST_CODE check_tool = clangtidy check_src_filters = + @@ -41,17 +42,21 @@ check_src_filters = platform = teensy board = teensy41 framework = arduino +monitor_speed = 115200 upload_protocol = teensy-cli extra_scripts = pre:extra_script.py lib_deps = SPI - https://github.com/hytech-racing/shared_firmware_interfaces.git#cf47a18 + Nanopb + https://github.com/vjmuzik/NativeEthernet.git + https://github.com/hytech-racing/HT_params/releases/download/2024-05-07T06_59_33/ht_eth_pb_lib.tar.gz + https://github.com/hytech-racing/shared_firmware_interfaces.git#feature/parameterize-filter-class https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 https://github.com/RCMast3r/spi_libs#2214fee https://github.com/tonton81/FlexCAN_T4#b928bcb https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/80/can_lib.tar.gz - git+ssh://git@github.com/hytech-racing/CASE_lib.git#v39 + https://github.com/hytech-racing/HT_CAN/releases/download/97/can_lib.tar.gz + git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 [env:test_can_on_teensy] lib_ignore = diff --git a/src/main.cpp b/src/main.cpp index 776eca31a..8f2357000 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,14 +1,17 @@ /* Include files */ /* System Includes*/ #include - +#include "ParameterInterface.h" /* Libraries */ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" #include "MCU_rev15_defs.h" +// #include "NativeEthernet.h" // /* Interfaces */ + #include "HytechCANInterface.h" +#include "Teensy_ADC.h" #include "MCP_ADC.h" #include "ORBIS_BR10.h" #include "MCUInterface.h" @@ -50,7 +53,9 @@ const TelemetryInterfaceReadChannels telem_read_channels = { .analog_steering_channel = MCU15_STEERING_CHANNEL, .current_channel = MCU15_CUR_POS_SENSE_CHANNEL, .current_ref_channel = MCU15_CUR_NEG_SENSE_CHANNEL, - .glv_sense_channel = MCU15_GLV_SENSE_CHANNEL}; + .glv_sense_channel = MCU15_GLV_SENSE_CHANNEL, + .therm_fl_channel = MCU15_THERM_FL_CHANNEL, + .therm_fr_channel = MCU15_THERM_FR_CHANNEL}; const PedalsParams accel_params = { .min_pedal_1 = ACCEL1_PEDAL_MIN, @@ -85,6 +90,9 @@ const PedalsParams brake_params = { DATA SOURCES */ +EthernetUDP protobuf_send_socket; +EthernetUDP protobuf_recv_socket; + /* Two CAN lines on Main ECU rev15 */ FlexCAN_T4 INV_CAN; // Inverter CAN (now both are on same line) FlexCAN_T4 TELEM_CAN; // telemetry CAN (basically everything except inverters) @@ -97,12 +105,14 @@ using CircularBufferType = CANBufferType; MCP_ADC<8> a1 = MCP_ADC<8>(ADC1_CS); MCP_ADC<4> a2 = MCP_ADC<4>(ADC2_CS, 1000000); // 1M baud needed for 04s MCP_ADC<4> a3 = MCP_ADC<4>(ADC3_CS, 1000000); - +Teensy_ADC<2> mcu_adc = Teensy_ADC<2>(DEFAULT_ANALOG_PINS); OrbisBR10 steering1(&Serial5); // /* // INTERFACES // */ +ParameterInterface param_interface; +ETHInterfaces ethernet_interfaces = {¶m_interface}; VNInterface vn_interface(&CAN3_txBuffer); DashboardInterface dashboard(&CAN3_txBuffer); AMSInterface ams_interface(8); @@ -132,7 +142,7 @@ struct inverters // */ SysClock sys_clock; -SteeringSystem steering_system(&steering1); +SteeringSystem steering_system(&steering1, STEERING_IIR_ALPHA); BuzzerController buzzer(BUZZER_ON_INTERVAL); SafetySystem safety_system(&ams_interface, &wd_interface); @@ -144,70 +154,83 @@ TorqueControllerMux torque_controller_mux(1.0, 0.4); // TODO ensure that case uses max regen torque, right now its not CASEConfiguration case_config = { // Following used for generated code - .AbsoluteTorqueLimit = AMK_MAX_TORQUE, // N-m - .yaw_pid_p = 1.33369, + .AbsoluteTorqueLimit = 21.42, // N-m, Torque limit used for yaw pid torque split overflow + .yaw_pid_p = 1.5, .yaw_pid_i = 0.25, .yaw_pid_d = 0.0, - .tcs_pid_p = 40.0, + .tcs_pid_p_lowerBound_front = 55.0, // if tcs_pid_p_lowerBound_front > tcs_pid_p_upperBound_front, inverse relationship, no error + .tcs_pid_p_upperBound_front = 45.0, + .tcs_pid_p_lowerBound_rear = 32.0, + .tcs_pid_p_upperBound_rear = 45.0, .tcs_pid_i = 0.0, .tcs_pid_d = 0.0, .useLaunch = false, .usePIDTV = true, .useTCSLimitedYawPID = true, - .useNormalForce = false, + .useNormalForce = true, .useTractionControl = true, .usePowerLimit = true, .usePIDPowerLimit = false, .useDecoupledYawBrakes = true, - .useDiscontinuousYawPIDBrakes = false, - .tcsSLThreshold = 0.2, - .launchSL = 0.2, - .launchDeadZone = 20, // N-m - .launchVelThreshold = 0.75, // m/s - .tcsVelThreshold = 2.5, // m/s - .yawPIDMaxDifferential = 10, // N-m - .yawPIDErrorThreshold = 0.1, // rad/s - .yawPIDVelThreshold = 1, // m/s - .yawPIDCoastThreshold = 2.5, // m/s + .useDiscontinuousYawPIDBrakes = true, + .launchDeadZone = 20.0, // N-m + .launchVelThreshold = 0.15, // m/s + .tcsVelThreshold = 1.5, // m/s + .yawPIDMaxDifferential = 10.0, // N-m + .yawPIDErrorThreshold = 0.1, // rad/s + .yawPIDVelThreshold = 0.35, // m/s + .yawPIDCoastThreshold = 2.5, // m/s .yaw_pid_brakes_p = 0.25, - .yaw_pid_brakes_i = 0, - .yaw_pid_brakes_d = 0, + .yaw_pid_brakes_i = 0.0, + .yaw_pid_brakes_d = 0.0, .decoupledYawPIDBrakesMaxDIfference = 2, // N-m - .discontinuousBrakesPercentThreshold = 0.4, - .TorqueMode = AMK_MAX_TORQUE, // N-m - // .TorqueMode = 2, // N-m - .RegenLimit = -10.0, // N-m + .discontinuousBrakesPercentThreshold = 0.7, + .TorqueMode = 21.42, // N-m + .RegenLimit = -15.0, // N-m .useNoRegen5kph = true, .useTorqueBias = true, - .DriveTorquePercentFront = 0.5, - .BrakeTorquePercentFront = 0.6, - .MechPowerMaxkW = 63, // kW + .DriveTorquePercentFront = 0.5, // DON'T TOUCH UNTIL LOAD CELL ADHERES TO DRIVE BIAS + .BrakeTorquePercentFront = 0.7, + .MechPowerMaxkW = 63.0, // kW + .launchLeftRightMaxDiff = 2.0, // N-m + .tcs_pid_lower_rpm_front = 0.0, // RPM + .tcs_pid_upper_rpm_front = 5000.0, // RPM + .tcs_pid_lower_rpm_rear = 0.0, // RPM + .tcs_pid_upper_rpm_rear = 5000.0, // RPM + .maxNormalLoadBrakeScalingFront = 1.25, + .tcs_saturation_front = 20, + .tcs_saturation_rear = 20, + .TCSGenLeftRightDiffLowerBound = 2, // N-m + .TCSGenLeftRightDiffUpperBound = 20, // N-m + .TCSWheelSteerLowerBound = 2, // Deg + .TCSWheelSteerUpperBound = 25, // Deg + .useRPM_TCS_GainSchedule = false, // If both are false, then P values defaults to lower bound per axle + .useNL_TCS_GainSchedule = true, + .TCS_NL_startBoundPerc_FrontAxle = 0.5, + .TCS_NL_endBoundPerc_FrontAxle = 0.4, + .TCS_NL_startBoundPerc_RearAxle = 0.5, + .TCS_NL_endBoundPerc_RearAxle = 0.6, + .useNL_TCS_SlipSchedule = true, + .launchSL_startBound_Front = 0.25, + .launchSL_endBound_Front = 0.15, + .launchSL_startBound_Rear = 0.3, + .launchSL_endBound_Rear = 0.4, + .TCS_SL_startBound_Front = 0.25, + .TCS_SL_endBound_Front = 0.15, + .TCS_SL_startBound_Rear = 0.3, + .TCS_SL_endBound_Rear = 0.4, + .TCS_SL_NLPerc_startBound_Front = 0.5, + .TCS_SL_NLPerc_endBound_Front = 0.4, + .TCS_SL_NLPerc_startBound_Rear = 0.5, + .TCS_SL_NLPerc_endBound_Rear = 0.6, // Following used for calculate_torque_request in CASESystem.tpp - .max_rpm = AMK_MAX_RPM, - .max_regen_torque = AMK_MAX_TORQUE, - .max_torque = AMK_MAX_TORQUE, + .max_rpm = 20000, + .max_regen_torque = 21.42, + .max_torque = 21.42, }; -// Torque limit used for yaw pid torque split overflow -// Yaw PID P -// Yaw PID I -// Yaw PID D -// TCS PID P -// TCS PID I -// TCS PID D -// Use launch -// Use PID TV -// Use normal force TV -// Use Traction Control (TCS) -// Use power limit -// Use PID power limit -// TCS activation threshold -// TCS launch SL target -// TCS launch torque deadzone (N-m) -// Max motor rpm -// Max regen torque -// Max torque -CASESystem case_system(&CAN3_txBuffer, 100, 70, case_config); + +CASESystem case_system(&CAN3_txBuffer, 100, 70, 550, case_config); /* Declare state machine */ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); @@ -231,20 +254,31 @@ void tick_all_systems(const SysTick_s ¤t_system_tick); /* Reset inverters */ void drivetrain_reset(); +void handle_ethernet_interface_comms(); + /* SETUP */ void setup() { - // initialize CAN communication init_all_CAN_devices(); + // Ethernet.begin(EthParams::default_MCU_MAC_address, EthParams::default_MCU_ip); + // protobuf_send_socket.begin(EthParams::default_protobuf_send_port); + // protobuf_recv_socket.begin(EthParams::default_protobuf_recv_port); + + /* Do this to send message VVV */ + // protobuf_socket.beginPacket(EthParams::default_TCU_ip, EthParams::default_protobuf_port); + // protobuf_socket.write(buf, len); + // protobuf_socker.endPacket(); + SPI.begin(); a1.init(); a2.init(); a3.init(); + mcu_adc.init(); a1.setChannelScale(MCU15_ACCEL1_CHANNEL, (1.0 / (float)(ACCEL1_PEDAL_MAX - ACCEL1_PEDAL_MIN))); a1.setChannelScale(MCU15_ACCEL2_CHANNEL, (1.0 / (float)(ACCEL2_PEDAL_MAX - ACCEL2_PEDAL_MIN))); @@ -264,6 +298,8 @@ void setup() a2.setChannelOffset(MCU15_FL_LOADCELL_CHANNEL, LOADCELL_FL_OFFSET /*Todo*/); a3.setChannelOffset(MCU15_FR_LOADCELL_CHANNEL, LOADCELL_FR_OFFSET /*Todo*/); + mcu_adc.setAlphas(MCU15_THERM_FL, 0.95); + mcu_adc.setAlphas(MCU15_THERM_FR, 0.95); // get latest tick from sys clock SysTick_s curr_tick = sys_clock.tick(micros()); @@ -282,7 +318,6 @@ void setup() /* Init Systems */ - safety_system.init(); // Drivetrain set all inverters disabled @@ -296,10 +331,11 @@ void setup() void loop() { - // Serial.println("test"); // get latest tick from sys clock SysTick_s curr_tick = sys_clock.tick(micros()); + // handle_ethernet_interface_comms(); + // process received CAN messages process_ring_buffer(CAN2_rxBuffer, CAN_receive_interfaces, curr_tick.millis); process_ring_buffer(CAN3_rxBuffer, CAN_receive_interfaces, curr_tick.millis); @@ -319,6 +355,9 @@ void loop() // tick state machine fsm.tick_state_machine(curr_tick.millis); + // give the state of the car to the param interface + param_interface.update_car_state(fsm.get_state()); + // tick safety system safety_system.software_shutdown(curr_tick); @@ -326,10 +365,13 @@ void loop() send_all_CAN_msgs(CAN2_txBuffer, &INV_CAN); send_all_CAN_msgs(CAN3_txBuffer, &TELEM_CAN); + // Basic debug prints if (curr_tick.triggers.trigger5) { Serial.print("Steering system reported angle (deg): "); Serial.println(steering_system.getSteeringSystemData().angle); + Serial.print("Steering system status: "); + Serial.println(static_cast(steering_system.getSteeringSystemData().status)); Serial.print("Primary sensor angle: "); Serial.println(steering1.convert().angle); Serial.print("Secondary sensor angle: "); @@ -409,6 +451,7 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) a1.get(), a2.get(), a3.get(), + mcu_adc.get(), steering1.convert(), &inv.fl, &inv.fr, @@ -437,6 +480,7 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) a1.tick(); a2.tick(); a3.tick(); + mcu_adc.tick(); load_cell_interface.tick( (LoadCellInterfaceTick_s){ .FLConversion = a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL], @@ -457,15 +501,6 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) { // tick pedals system - // Serial.println("accel1"); - // Serial.println(a1.get().conversions[MCU15_ACCEL1_CHANNEL].raw); - // Serial.println("accel2"); - // Serial.println(a1.get().conversions[MCU15_ACCEL2_CHANNEL].raw); - // Serial.println("brake1"); - // Serial.println(a1.get().conversions[MCU15_BRAKE1_CHANNEL].raw); - // Serial.println("brake2"); - // Serial.println(a1.get().conversions[MCU15_BRAKE2_CHANNEL].raw); - pedals_system.tick( current_system_tick, a1.get().conversions[MCU15_ACCEL1_CHANNEL], @@ -499,6 +534,8 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) dashboard.startButtonPressed(), 3); + // case_system.update_config_from_param_interface(param_interface); + torque_controller_mux.tick( current_system_tick, drivetrain.get_dynamic_data(), @@ -510,3 +547,24 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) vn_interface.get_vn_struct(), controller_output); } + +void handle_ethernet_interface_comms() +{ + // function that will handle receiving and distributing of all messages to all ethernet interfaces + // via the union message. this is a little bit cursed ngl. + // TODO un fuck this and make it more sane + // Serial.println("bruh"); + handle_ethernet_socket_receive(&protobuf_recv_socket, &recv_pb_stream_union_msg, ethernet_interfaces); + + // this is just kinda here i know. + if (param_interface.params_need_sending()) + { + // Serial.println("handling ethernet"); + auto config = param_interface.get_config(); + if (!handle_ethernet_socket_send_pb(&protobuf_send_socket, config, config_fields)) + { + // TODO this means that something bad has happend + } + param_interface.reset_params_need_sending(); + } +} \ No newline at end of file diff --git a/src/main_ethernet_test.cpp b/src/main_ethernet_test.cpp new file mode 100644 index 000000000..9c92421fa --- /dev/null +++ b/src/main_ethernet_test.cpp @@ -0,0 +1,43 @@ +#include + + +#include "ProtobufMsgInterface.h" +#include "MCU_rev15_defs.h" + +EthernetUDP protobuf_send_socket; +EthernetUDP protobuf_recv_socket; +CAR_STATE state = CAR_STATE::STARTUP; +ParameterInterface params(state); +ETHInterfaces ethernet_interfaces = {¶ms}; + +void init_ethernet_device() +{ + Ethernet.begin(EthParams::default_MCU_MAC_address, EthParams::default_MCU_ip); + protobuf_send_socket.begin(EthParams::default_protobuf_send_port); + protobuf_recv_socket.begin(EthParams::default_protobuf_recv_port); +} + +void test_ethernet() +{ + + handle_ethernet_socket_receive(&protobuf_recv_socket, &recv_pb_stream_union_msg, ethernet_interfaces); + if(params.params_need_sending()) + { + auto config = params.get_config(); + if(!handle_ethernet_socket_send_pb(&protobuf_send_socket, config, config_fields)){ + } + params.reset_params_need_sending(); + } + +} + +void setup() +{ + init_ethernet_device(); +} + +void loop() +{ + test_ethernet(); + // Serial.println("loopin"); +} \ No newline at end of file diff --git a/test/test_interfaces/run_embedded_tests.cpp b/test/test_interfaces/run_embedded_tests.cpp index 083fd0eaa..5eb8cc1f7 100644 --- a/test/test_interfaces/run_embedded_tests.cpp +++ b/test/test_interfaces/run_embedded_tests.cpp @@ -1,15 +1,16 @@ #include -#include - -#include "AMS_interface_test.h" -#include "dashboard_interface_test.h" -#include "Watchdog_interface_test.h" +// #include +// #include "AMS_interface_test.h" +// #include "dashboard_interface_test.h" +// #include "Watchdog_interface_test.h" +// #include "test_ethernet.h" // #include "Telemetry_interface_test.h" void setUp(void) { // init_can_interface(); + init_ethernet_device(); } void tearDown(void) @@ -21,12 +22,13 @@ int runUnityTests(void) { UNITY_BEGIN(); /* TEST DASHBOARD */ - RUN_TEST(test_dashboard_unpacking_can_message); - RUN_TEST(test_dashboard_circular_buffer); + // RUN_TEST(test_ethernet); + // RUN_TEST(test_dashboard_unpacking_can_message); + // RUN_TEST(test_dashboard_circular_buffer); /* TEST AMS */ - RUN_TEST(test_AMS_heartbeat); + // RUN_TEST(test_AMS_heartbeat); /* TEST WATCHDOG */ - RUN_TEST(test_watchdog_kick); + // RUN_TEST(test_watchdog_kick); // testing can interface // RUN_TEST(test_can_interface_send) // RUN_TEST(test_can_interface_send_and_receive_raw) @@ -38,7 +40,7 @@ int runUnityTests(void) void setup() { - delay(500); + delay(200); runUnityTests(); } diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index 4ac9fd06a..8038eb36d 100644 --- a/test/test_systems/main.cpp +++ b/test/test_systems/main.cpp @@ -1,16 +1,17 @@ #include #include "state_machine_test.h" -#include "pedals_system_test.h" +// #include "pedals_system_test.h" #include "torque_controller_mux_test.h" #include "drivetrain_system_test.h" #include "safety_system_test.h" -#include "test_CASE.h" +// #include "test_CASE.h" +// #include "param_system_test.h" +#include "steering_system_test.h" int main(int argc, char **argv) { - testing::InitGoogleMock(&argc, argv); if (RUN_ALL_TESTS()) ; diff --git a/test/test_systems/param_system_test.h b/test/test_systems/param_system_test.h new file mode 100644 index 000000000..7e7c30b03 --- /dev/null +++ b/test/test_systems/param_system_test.h @@ -0,0 +1,20 @@ +#ifndef PARAMS_SYSTEM_TEST +#define PARAMS_SYSTEM_TEST + +#include "ParameterSystem.h" + +// TODO may wanna do this with a fixed param lib, but for now the regular +// param lib should be fine +TEST(ParamSystemTest, test_set_params) { + int val = 20; + set_parameter(Parameters::CASE_TORQUE_MAXInstance, val); + EXPECT_EQ(Parameters::CASE_TORQUE_MAXInstance.get(), val); +} + +TEST(ParamSystemTest, test_set_get_params) { + int val = 20; + set_parameter(Parameters::CASE_TORQUE_MAXInstance, val); + EXPECT_EQ(get_parameter(Parameters::CASE_TORQUE_MAXInstance), val); +} + +#endif \ No newline at end of file diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h new file mode 100644 index 000000000..7c907f8d2 --- /dev/null +++ b/test/test_systems/steering_system_test.h @@ -0,0 +1,258 @@ +#ifndef STEERING_SYSTEM_TEST +#define STEERING_SYSTEM_TEST +#include +#include +#include "SteeringSystem.h" + +#define PRIMARY_ALPHA (0.7) // parameter used on car +#define SECONDARY_ALPHA (0.7) +#define WARN_DIVERGENCE_OFFSET (0.6) +#define ERR_DIVERGENCE_OFFSET (0.6) +#define WARN_FILTER_LATENCY (8) +#define ERR_FILTER_LATENCY (9) + +TEST(SteeringSystemTesting, test_steering_nominal) +{ + float angles_to_test[5] = {-120.0f, -60.0f, 0.0f, 60.0f, 120.0f}; + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; + + // Sweep through a few angles where the sensors agree perfectly and check the system is nominal + for (float angle: angles_to_test) + { + primarySensor.mockConversion.angle = angle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = angle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } + } + ); + + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == mockAngleFilters[0].filtered_result(angle)); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_NOMINAL); + } +} + +TEST(SteeringSystemTesting, test_steering_primary_is_marginal) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; + float angle = 0.0f; + + primarySensor.mockConversion.angle = angle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = angle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == mockAngleFilters[0].filtered_result(angle)); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL); +} + +TEST(SteeringSystemTesting, test_steering_secondary_is_marginal) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; + float angle = 0.0f; + + primarySensor.mockConversion.angle = angle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = angle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == mockAngleFilters[0].filtered_result(angle)); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL); +} + +TEST(SteeringSystemTesting, test_steering_divergence_warning) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; + float mockPrimaryFilteredAngle; + float mockSecondaryFilteredAngle; + + float primaryAngle = 0.0f; + // Pend divergence to sensor readings + // - IIR filter eq.: filtered_val = (1-alpha)*new_val + alpha*prev_val + // - when divergence set exactly to system threshold + // - divergence magnitude gets shaved off, will not trigger above threshold + // - pend offset to divergence to trip threshold + float secondaryAngle = primaryAngle + WARN_DIVERGENCE_OFFSET + STEERING_DIVERGENCE_WARN_THRESHOLD; // edge discrepancy + + // Latency due to filtering + // - required steps of filtering for divergence to rise up to threshold with offset pended + // - e.g. when alpha = 0.7, pending offset = 0.6, it takes a minimal of 8 steps of filtering to exceed warn threshold + for (int i = 0; i < WARN_FILTER_LATENCY; i++) // edge filter steps to align output + { + mockPrimaryFilteredAngle = mockAngleFilters[0].filtered_result(primaryAngle); + mockSecondaryFilteredAngle = mockAngleFilters[1].filtered_result(secondaryAngle); + + primarySensor.mockConversion.angle = primaryAngle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = secondaryAngle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } + } + ); + } + + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == mockPrimaryFilteredAngle); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL); +} + +TEST(SteeringSystemTesting, test_steering_divergence_error) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; + float mockPrimaryFilteredAngle; + float mockSecondaryFilteredAngle; + + float primaryAngle = 0.0f; + // Pend divergence to sensor readings + // - IIR filter eq.: filtered_val = (1-alpha)*new_val + alpha*prev_val + // - when divergence set exactly to system threshold + // - divergence magnitude gets shaved off, will not trigger above threshold + // - pend offset to divergence to trip threshold + float secondaryAngle = primaryAngle + ERR_DIVERGENCE_OFFSET + STEERING_DIVERGENCE_ERROR_THRESHOLD; // edge discrepancy + + // Latency due to filtering + // - required steps of filtering for divergence to rise up to threshold with offset pended + // - e.g. when alpha = 0.7, pending offset = 0.6, it takes a minimal of 9 steps of filtering to exceed error threshold + for (int i = 0; i < ERR_FILTER_LATENCY; i++) // edge filter steps to align output + { + mockPrimaryFilteredAngle = mockAngleFilters[0].filtered_result(primaryAngle); + mockSecondaryFilteredAngle = mockAngleFilters[1].filtered_result(secondaryAngle); + + primarySensor.mockConversion.angle = primaryAngle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = secondaryAngle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } + } + ); + } + + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == 0.0f); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_ERROR); +} + +TEST(SteeringSystemTesting, test_steering_primary_is_missing) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + // Filter refernce values for evaluation + Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; + float angle = 100.0f; + + primarySensor.mockConversion.angle = 0.0f; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_ERROR; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = angle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == mockAngleFilters[1].filtered_result(angle)); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_DEGRADED); +} + +TEST(SteeringSystemTesting, test_steering_primary_is_missing_and_secondary_is_marginal) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + float angle = 100.0f; + + primarySensor.mockConversion.angle = 0.0f; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_ERROR; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = angle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == 0.0f); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_ERROR); +} + +#endif \ No newline at end of file diff --git a/test/test_systems/test_CASE.h b/test/test_systems/test_CASE.h index aa2c86d65..50fb5c448 100644 --- a/test/test_systems/test_CASE.h +++ b/test/test_systems/test_CASE.h @@ -19,11 +19,15 @@ TEST(CASESystemTesting, test_vn_start_time) FakeMessageQueue faked_msg_q; CASEConfiguration case_config = { - .AbsoluteTorqueLimit = AMK_MAX_TORQUE, // N-m - .yaw_pid_p = 1.33369, + // Following used for generated code + .AbsoluteTorqueLimit = AMK_MAX_TORQUE, // N-m, Torque limit used for yaw pid torque split overflow + .yaw_pid_p = 1.369, .yaw_pid_i = 0.25, .yaw_pid_d = 0.0, - .tcs_pid_p = 40.0, + .tcs_pid_p_lowerBound_front = 35.0, // if tcs_pid_p_lowerBound_front > tcs_pid_p_upperBound_front, inverse relationship, no error + .tcs_pid_p_upperBound_front = 55.0, + .tcs_pid_p_lowerBound_rear = 28.0, + .tcs_pid_p_upperBound_rear = 35.0, .tcs_pid_i = 0.0, .tcs_pid_d = 0.0, .useLaunch = false, @@ -31,30 +35,41 @@ TEST(CASESystemTesting, test_vn_start_time) .useTCSLimitedYawPID = true, .useNormalForce = false, .useTractionControl = true, - .usePowerLimit = false, + .usePowerLimit = true, .usePIDPowerLimit = false, - .useDecoupledYawBrakes = false, - .useDiscontinuousYawPIDBrakes = true, + .useDecoupledYawBrakes = true, + .useDiscontinuousYawPIDBrakes = false, .tcsSLThreshold = 0.2, .launchSL = 0.2, - .launchDeadZone = 20, // N-m - .launchVelThreshold = 0.75, // m/s - .tcsVelThreshold = 2.5, // m/s - .yawPIDMaxDifferential = 10, // N-m - .yawPIDErrorThreshold = 0.1, // rad/s - .yawPIDVelThreshold = 1, // m/s - .yawPIDCoastThreshold = 2.5, // m/s + .launchDeadZone = 20.0, // N-m + .launchVelThreshold = 0.75, // m/s + .tcsVelThreshold = 2.5, // m/s + .yawPIDMaxDifferential = 10.0, // N-m + .yawPIDErrorThreshold = 0.1, // rad/s + .yawPIDVelThreshold = 1.0, // m/s + .yawPIDCoastThreshold = 2.5, // m/s .yaw_pid_brakes_p = 0.25, - .yaw_pid_brakes_i = 0, - .yaw_pid_brakes_d = 0, + .yaw_pid_brakes_i = 0.0, + .yaw_pid_brakes_d = 0.0, .decoupledYawPIDBrakesMaxDIfference = 2, // N-m .discontinuousBrakesPercentThreshold = 0.4, .TorqueMode = AMK_MAX_TORQUE, // N-m .RegenLimit = -10.0, // N-m + .useNoRegen5kph = true, + .useTorqueBias = true, + .DriveTorquePercentFront = 0.5, + .BrakeTorquePercentFront = 0.6, + .MechPowerMaxkW = 63.0, // kW + .launchLeftRightMaxDiff = 2.0, // N-m + .tcs_pid_lower_rpm_front = 0.0, // RPM + .tcs_pid_upper_rpm_front = 5000.0, // RPM + .tcs_pid_lower_rpm_rear = 0.0, // RPM + .tcs_pid_upper_rpm_rear = 5000.0, // RPM + // Following used for calculate_torque_request in CASESystem.tpp .max_rpm = AMK_MAX_RPM, .max_regen_torque = AMK_MAX_TORQUE, .max_torque = AMK_MAX_TORQUE, }; - CASESystem case_system(&faked_msg_q, 100, 70, case_config); + CASESystem case_system(&faked_msg_q, 100, 70, 550, case_config); }