diff --git a/.gitmodules b/.gitmodules index 29b32a3..a55c37c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -10,4 +10,4 @@ path = lib/CANlib url = https://github.com/MITMotorsports/CANlib.git branch = master - ignore = dirty + ignore = dirty diff --git a/can_spec_my18.yml b/can_spec_my18.yml index a3d7853..928b09b 100644 --- a/can_spec_my18.yml +++ b/can_spec_my18.yml @@ -26,6 +26,28 @@ boards: - MCCommand - VCUControlsParams - VCUControlsMonitoring + - VCUSpeedCntrlKpTimes1000 + - VCUSpeedCntrlKiTimes1000 + - VCUSpeedCntrlKdTimes1000 + - VCUSpeedCntrlIWindupMax + - VCUSpeedCntrlIWindupMin + - VCUSpeedCntrlMinOutputValue + - VCUSpeedCntrlMaxOutputValue + - VCUSpeedCntrlMinInputValue + - VCUSpeedCntrlMaxInputValue + - VCUSpeedCntrlErrorUpdateTimeout + - VCUSpeedCntrlDt + - VCUSpeedCntrlEnabled + - VCUSpeedCntrlOutOfInputRangeThrottled + - VCUSpeedCntrlOutOfOutputRangeThrottled + - VCUSpeedCntrlErrorUpdateTimedOut + - VCUSpeedCntrlRPMSetpoint + - VCUSpeedCntrlCommandedTorque + - VCUSpeedCntrlRPMError + - VCUSpeedCntrlLastRPMError + - VCUSpeedCntrlDerivRPMError + - VCUSpeedCntrlRPMErrorAccumulated + - VCUSpeedCntrlLastErrorUpdateTimestamp subscribe: can0: - FrontCanNodeBrakeThrottle @@ -695,10 +717,10 @@ buses: c_type: uint16_t launch_ctrl_slip_ratio: # Ranges from 105 to 115 position: 48 - length: 8 + length: 9 c_type: uint16_t using_launch_ctrl: - position: 56 + position: 57 length: 1 c_type: bool VCUControlsParams: @@ -869,6 +891,231 @@ buses: position: 48 length: 16 c_type: uint16_t + VCUSpeedCntrlKpTimes1000: + can_id: 0x223 + period: 200ms + is_big_endian: true + segments: + kp_times_1000: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlKiTimes1000: + can_id: 0x225 + period: 200ms + is_big_endian: true + segments: + ki_times_1000: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlKdTimes1000: + can_id: 0x227 + period: 200ms + is_big_endian: true + segments: + kd_times_1000: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlIWindupMax: + can_id: 0x229 + period: 200ms + is_big_endian: true + segments: + i_windup_max: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlIWindupMin: + can_id: 0x22B + period: 200ms + is_big_endian: true + segments: + i_windup_min: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlMinOutputValue: + can_id: 0x22D + period: 200ms + is_big_endian: true + segments: + min_output_value: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlMaxOutputValue: + can_id: 0x22F + period: 200ms + is_big_endian: true + segments: + max_output_value: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlMinInputValue: + can_id: 0x231 + period: 200ms + is_big_endian: true + segments: + min_input_value: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlMaxInputValue: + can_id: 0x233 + period: 200ms + is_big_endian: true + segments: + max_input_value: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlErrorUpdateTimeout: + can_id: 0x235 + period: 200ms + is_big_endian: true + segments: + error_update_timeout: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlDt: + can_id: 0x237 + period: 200ms + is_big_endian: true + segments: + dt: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlEnabled: + can_id: 0x239 + period: 200ms + is_big_endian: true + segments: + enabled: + position: 0 + length: 1 + c_type: bool + VCUSpeedCntrlOutOfInputRangeThrottled: + can_id: 0x23B + period: 200ms + is_big_endian: true + segments: + out_of_input_range_throttled: + position: 0 + length: 1 + c_type: bool + VCUSpeedCntrlOutOfOutputRangeThrottled: + can_id: 0x23D + period: 200ms + is_big_endian: true + segments: + out_of_output_range_throttled: + position: 0 + length: 1 + c_type: bool + VCUSpeedCntrlErrorUpdateTimedOut: + can_id: 0x23F + period: 200ms + is_big_endian: true + segments: + error_update_timed_out: + position: 0 + length: 1 + c_type: bool + VCUSpeedCntrlRPMSetpoint: + can_id: 0x241 + period: 200ms + is_big_endian: true + segments: + rpm_setpoint: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlCommandedTorque: + can_id: 0x243 + period: 200ms + is_big_endian: true + segments: + commanded_torque: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlRPMError: + can_id: 0x245 + period: 200ms + is_big_endian: true + segments: + rpm_error: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlLastRPMError: + can_id: 0x247 + period: 200ms + is_big_endian: true + segments: + last_rpm_error: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlDerivRPMError: + can_id: 0x249 + period: 200ms + is_big_endian: true + segments: + deriv_rpm_error: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlRPMErrorAccumulated: + can_id: 0x24B + period: 200ms + is_big_endian: true + segments: + rpm_error_accumulated: + position: 0 + length: 32 + c_type: int32_t + VCUSpeedCntrlLastErrorUpdateTimestamp: + can_id: 0x24D + period: 200ms + is_big_endian: true + segments: + last_error_update_timestamp: + position: 0 + length: 32 + c_type: uint32_t + DashSpeedCntrlKpTimes1000: + can_id: 0x224 + period: 200ms + is_big_endian: true + segments: + kp_times_1000: + position: 0 + length: 32 + c_type: int32_t + DashSpeedCntrlKiTimes1000: + can_id: 0x226 + period: 200ms + is_big_endian: true + segments: + ki_times_1000: + position: 0 + length: 32 + c_type: int32_t + DashSpeedCntrlRPMSetpoint: + can_id: 0x242 + period: 200ms + is_big_endian: true + segments: + rpm_setpoint: + position: 0 + length: 32 + c_type: int32_t MCCommand: can_id: 0x0C0 # From manual period: 9ms # MC needs a message at 2 Hz or higher diff --git a/src/dashboard/inc/carstats.h b/src/dashboard/inc/carstats.h index 629e4df..280ba8f 100644 --- a/src/dashboard/inc/carstats.h +++ b/src/dashboard/inc/carstats.h @@ -61,6 +61,9 @@ typedef struct { can0_DashRequestLC_T lc_controls; can0_VCUControlsParams_T vcu_controls; can0_VCUControlsParamsLC_T vcu_lc_controls; + can0_DashSpeedCntrlKpTimes1000_T kp; + can0_DashSpeedCntrlKiTimes1000_T ki; + can0_DashSpeedCntrlRPMSetpoint_T rpm_setpoint; bool vcu_controls_received; bool vcu_lc_controls_received; diff --git a/src/dashboard/inc/page_manager.h b/src/dashboard/inc/page_manager.h index 3420519..bf37015 100644 --- a/src/dashboard/inc/page_manager.h +++ b/src/dashboard/inc/page_manager.h @@ -10,6 +10,7 @@ typedef enum { DASH_PAGE_TRACTION, DASH_PAGE_REGEN, DASH_PAGE_LAUNCH_CONTROL, + DASH_LC_PID_DEBUG, DASH_PAGE_TEMP_LIM, DASH_PAGE_VOLT_LIM, DASH_PAGE_FAULT, diff --git a/src/dashboard/src/dispatch.c b/src/dashboard/src/dispatch.c index 13e5312..ff944f8 100644 --- a/src/dashboard/src/dispatch.c +++ b/src/dashboard/src/dispatch.c @@ -108,6 +108,10 @@ void dispatch_init() { carstats.vcu_controls_received = false; carstats.vcu_lc_controls_received = false; + carstats.rpm_setpoint.rpm_setpoint = 0; + carstats.ki.ki_times_1000 = 1000; + carstats.kp.kp_times_1000 = 0; + init_button_state(&carstats.buttons.left); init_button_state(&carstats.buttons.right); init_button_state(&carstats.buttons.A); @@ -177,6 +181,9 @@ void send_dash_controls(void) { LIMIT(can0_DashRequest_period); handle_can_error(can0_DashRequest_Write(&carstats.controls)); handle_can_error(can0_DashRequestLC_Write(&carstats.lc_controls)); + handle_can_error(can0_DashSpeedCntrlKpTimes1000_Write(&carstats.kp)); + handle_can_error(can0_DashSpeedCntrlKiTimes1000_Write(&carstats.ki)); + handle_can_error(can0_DashSpeedCntrlRPMSetpoint_Write(&carstats.rpm_setpoint)); } void vcu_controls_update(void) { diff --git a/src/dashboard/src/page_manager.c b/src/dashboard/src/page_manager.c index 7e8e18c..208ea54 100644 --- a/src/dashboard/src/page_manager.c +++ b/src/dashboard/src/page_manager.c @@ -52,6 +52,7 @@ void draw_regen_page(page_manager_t *pm, NHD_US2066_OLED *oled); void draw_launch_control_page(page_manager_t *pm, NHD_US2066_OLED *oled); void draw_traction_page(page_manager_t *pm, NHD_US2066_OLED *oled); void draw_fault_page(page_manager_t *pm, NHD_US2066_OLED *oled); +void draw_lc_pid_page(page_manager_t *pm, NHD_US2066_OLED *oled); void page_manager_update(page_manager_t *pm, NHD_US2066_OLED *oled) { // Make sure that active aero is disable if we are not on the critical page @@ -81,6 +82,9 @@ void page_manager_update(page_manager_t *pm, NHD_US2066_OLED *oled) { case DASH_PAGE_DEBUG: draw_debug_page(pm, oled); break; + case DASH_LC_PID_DEBUG: + draw_lc_pid_page(pm, oled); + break; default: break; } @@ -516,7 +520,7 @@ void draw_launch_control_page(page_manager_t *pm, NHD_US2066_OLED *oled) { oled_rprint_pad(oled, "SR ", 4); - if (stats->lc_controls.launch_ctrl_slip_ratio != 255) { + if (stats->lc_controls.launch_ctrl_slip_ratio != -1) { oled_print_num(oled, stats->lc_controls.launch_ctrl_slip_ratio); } else { oled_print(oled, DATA_UNKNOWN); @@ -524,7 +528,7 @@ void draw_launch_control_page(page_manager_t *pm, NHD_US2066_OLED *oled) { oled_clearline(oled, 2); oled_set_pos(oled, 2, 0); - + oled_print(oled, "TORQ "); if (stats->lc_controls.speeding_up_torque != 65535) { oled_print_num(oled, stats->lc_controls.speeding_up_torque); @@ -730,3 +734,65 @@ void draw_traction_page(page_manager_t *pm, NHD_US2066_OLED *oled) { oled_print(oled, DATA_UNKNOWN); } } + + +void draw_lc_pid_page(page_manager_t *pm, NHD_US2066_OLED *oled) { + + carstats_t *stats = pm->stats; + static int var_toggled = 0; + + // Process contextual actions + if (stats->buttons.B.rising_edge) var_toggled++; + var_toggled = LOOPOVER(var_toggled, 0, 2); + + if (stats->buttons.left.action == BUTTON_ACTION_TAP) { + switch (var_toggled) { + case 0: + stats->kp.kp_times_1000 -= 50; + break; + case 1: + stats->ki.ki_times_1000 -= 1; + break; + case 2: + stats->rpm_setpoint.rpm_setpoint -= 100; + break; + } + } + + if (stats->buttons.right.action == BUTTON_ACTION_TAP) { + switch (var_toggled) { + case 0: + stats->kp.kp_times_1000 += 50; + break; + case 1: + stats->ki.ki_times_1000 += 1; + break; + case 2: + stats->rpm_setpoint.rpm_setpoint += 100; + break; + } + } + + stats->kp.kp_times_1000 = LOOPOVER(stats->kp.kp_times_1000, 1000, 2000); + stats->ki.ki_times_1000 = LOOPOVER(stats->ki.ki_times_1000, 0, 25); + stats->rpm_setpoint.rpm_setpoint = LOOPOVER(stats->rpm_setpoint.rpm_setpoint, 0, 2500); + + oled_clearline(oled, 0); + oled_set_pos(oled, 0, 1); + oled_print(oled, "P: "); + oled_print_num(oled, stats->kp.kp_times_1000); + + oled_clearline(oled, 1); + oled_set_pos(oled, 1, 1); + oled_print(oled, "I: "); + oled_print_num(oled, stats->ki.ki_times_1000); + + oled_clearline(oled, 2); + oled_set_pos(oled, 2, 1); + oled_print(oled, "RPM Setpoint: "); + oled_print_num(oled, stats->rpm_setpoint.rpm_setpoint); + + + oled_set_pos(oled, var_toggled, 0); + oled_print(oled, ">"); +} diff --git a/src/vcu/inc/can_handles.h b/src/vcu/inc/can_handles.h index 7eff318..e370282 100644 --- a/src/vcu/inc/can_handles.h +++ b/src/vcu/inc/can_handles.h @@ -4,6 +4,8 @@ #include "stdio.h" #include "stm32f2xx_hal.h" #include "CANlib.h" +#include +#include #include "state.h" #include "fault.h" @@ -41,4 +43,27 @@ void sendSpeedCmdMsg(int16_t speed, int16_t torque_limit); void sendMotorOffCmdMsg(void); void send_mc_fault_clear(void); +void sendVCUSpeedCntrlKpTimes1000Msg(void); +void sendVCUSpeedCntrlKiTimes1000Msg(void); +void sendVCUSpeedCntrlKdTimes1000Msg(void); +void sendVCUSpeedCntrlIWindupMaxMsg(void); +void sendVCUSpeedCntrlIWindupMinMsg(void); +void sendVCUSpeedCntrlMinOutputValueMsg(void); +void sendVCUSpeedCntrlMaxOutputValueMsg(void); +void sendVCUSpeedCntrlMinInputValueMsg(void); +void sendVCUSpeedCntrlMaxInputValueMsg(void); +void sendVCUSpeedCntrlErrorUpdateTimeoutMsg(void); +void sendVCUSpeedCntrlDtMsg(void); +void sendVCUSpeedCntrlEnabledMsg(void); +void sendVCUSpeedCntrlOutOfInputRangeThrottledMsg(void); +void sendVCUSpeedCntrlOutOfOutputRangeThrottledMsg(void); +void sendVCUSpeedCntrlErrorUpdateTimedOutMsg(void); +void sendVCUSpeedCntrlRPMSetpointMsg(void); +void sendVCUSpeedCntrlCommandedTorqueMsg(void); +void sendVCUSpeedCntrlRPMErrorMsg(void); +void sendVCUSpeedCntrlLastRPMErrorMsg(void); +void sendVCUSpeedCntrlDerivRPMErrorMsg(void); +void sendVCUSpeedCntrlRPMErrorAccumulatedMsg(void); +void sendVCUSpeedCntrlLastErrorUpdateTimestampMsg(void); + #endif // ifndef __CAN_HANDLES_H diff --git a/src/vcu/inc/controls.h b/src/vcu/inc/controls.h index fd3c3dd..8f4c898 100644 --- a/src/vcu/inc/controls.h +++ b/src/vcu/inc/controls.h @@ -9,6 +9,10 @@ #include "fault_pedalbox.h" #include "fault_brakes.h" +// Internal Speed Controller settings for launch control +#define MAX_INPUT_SPEED 3200 +#define SPEED_CONTROLLER_UPDATE_PERIOD_MS 1 + // TODO: Extract MAX_TORQUE from RMS EEPROM (over CAN?) #define MAX_TORQUE 2400 // unit: dNm #define MAX_ACCEL_VAL 1000 @@ -61,10 +65,14 @@ typedef enum { extern can0_VCUControlsParams_T control_settings; extern can0_VCUControlsParamsLC_T lc_settings; +extern can0_DashSpeedCntrlRPMSetpoint_T rpm_setpoint; // INTERACTION FUNCTIONS void enable_controls(void); void disable_controls(void); +void lock_lc_state(void); +void lc_rtd_callback(void); +bool get_lc_state_locked_until_next_rtd(void); bool get_controls_enabled(void); void execute_controls(void); void set_lc_zero_torque(void); diff --git a/src/vcu/inc/speed_controller.h b/src/vcu/inc/speed_controller.h new file mode 100644 index 0000000..e961316 --- /dev/null +++ b/src/vcu/inc/speed_controller.h @@ -0,0 +1,78 @@ +#ifndef __SPEED_CONTROLLER +#define __SPEED_CONTROLLER + +#include "stm32f2xx_hal.h" +#include +#include + +#include "can_handles.h" + +extern can0_VCUSpeedCntrlKpTimes1000_T kp_times_1000_frame; +extern can0_VCUSpeedCntrlKiTimes1000_T ki_times_1000_frame; +extern can0_VCUSpeedCntrlKdTimes1000_T kd_times_1000_frame; +extern can0_VCUSpeedCntrlIWindupMax_T i_windup_max_frame; +extern can0_VCUSpeedCntrlIWindupMin_T i_windup_min_frame; +extern can0_VCUSpeedCntrlMinOutputValue_T min_output_value_frame; +extern can0_VCUSpeedCntrlMaxOutputValue_T max_output_value_frame; +extern can0_VCUSpeedCntrlMinInputValue_T min_input_value_frame; +extern can0_VCUSpeedCntrlMaxInputValue_T max_input_value_frame; +extern can0_VCUSpeedCntrlErrorUpdateTimeout_T error_update_timeout_frame; +extern can0_VCUSpeedCntrlDt_T dt_frame; +extern can0_VCUSpeedCntrlEnabled_T enabled_frame; +extern can0_VCUSpeedCntrlOutOfInputRangeThrottled_T out_of_input_range_throttled_frame; +extern can0_VCUSpeedCntrlOutOfOutputRangeThrottled_T out_of_output_range_throttled_frame; +extern can0_VCUSpeedCntrlErrorUpdateTimedOut_T error_update_timed_out_frame; +extern can0_VCUSpeedCntrlRPMSetpoint_T rpm_setpoint_frame; +extern can0_VCUSpeedCntrlCommandedTorque_T commanded_torque_frame; +extern can0_VCUSpeedCntrlRPMError_T rpm_error_frame; +extern can0_VCUSpeedCntrlLastRPMError_T last_rpm_error_frame; +extern can0_VCUSpeedCntrlDerivRPMError_T deriv_rpm_error_frame; +extern can0_VCUSpeedCntrlRPMErrorAccumulated_T rpm_error_accumulated_frame; +extern can0_VCUSpeedCntrlLastErrorUpdateTimestamp_T last_error_update_timestamp_frame; + +// PUBLIC STRUCTS +typedef struct { + int32_t kp_times_1000; + int32_t ki_times_1000; + int32_t kd_times_1000; + int32_t i_windup_max; + int32_t i_windup_min; + int32_t min_output_value; + int32_t max_output_value; + int32_t min_input_value; + int32_t max_input_value; + int32_t dt; + uint32_t error_update_timeout; +} SpeedControllerParams; + +typedef struct { + bool enabled; + bool out_of_input_range_throttled; + bool out_of_output_range_throttled; + bool error_update_timed_out; + int32_t rpm_setpoint; + int32_t commanded_torque; + int32_t rpm_error; + int32_t last_rpm_error; + int32_t deriv_rpm_error; + int32_t rpm_error_accumulated; + uint32_t last_error_update_timestamp; +} SpeedControllerInternalVars; + +// PUBLIC FUNCTIONS +void init_speed_controller_defaults(int32_t max_input_speed, + int32_t max_output_torque, uint32_t update_period_millis); +void enable_speed_controller(void); +void disable_speed_controller(void); +void set_speed_controller_setpoint(int32_t rpm); +void update_speed_controller_error(int32_t actual_rpm, + uint32_t actual_rpm_msg_timestamp); +int32_t get_speed_controller_torque_command(void); +bool get_speed_controller_enabled(void); +int32_t get_speed_controller_error(void); +int32_t get_speed_controller_accum(void); +int32_t get_speed_controller_deriv(void); +void set_kp(int32_t new_kp); +void set_ki(int32_t new_ki); + +#endif // ifndef __SPEED_CONTROLLER diff --git a/src/vcu/inc/state_vcu_driving.h b/src/vcu/inc/state_vcu_driving.h index 75af00b..f9e3f9f 100644 --- a/src/vcu/inc/state_vcu_driving.h +++ b/src/vcu/inc/state_vcu_driving.h @@ -6,6 +6,7 @@ #include "state_vcu.h" #include "can_handles.h" #include "controls.h" +#include "speed_controller.h" #define AA_SETUP 10 #define AA_HOLD 10 diff --git a/src/vcu/src/can_handles.c b/src/vcu/src/can_handles.c index 4b054ca..703ec50 100644 --- a/src/vcu/src/can_handles.c +++ b/src/vcu/src/can_handles.c @@ -93,9 +93,22 @@ void handleCAN(CAN_HandleTypeDef *hcan) { case can0_DashRequest: handleDashRequest(&frame); break; - + case can0_DashRequestLC: handleDashRequestLC(&frame); + break; + + case can0_DashSpeedCntrlKiTimes1000: + handleDashSpeedCntrlKiTimes1000(&frame); + break; + + case can0_DashSpeedCntrlKpTimes1000: + handleDashSpeedCntrlKpTimes1000(&frame); + break; + + case can0_DashSpeedCntrlRPMSetpoint: + handle_DashSpeedCntrlRPMSetpoint(&frame); + break; default: break; @@ -115,6 +128,22 @@ void handleBrakeThrottleMsg(Frame *msg) { heartbeats.fcn = HAL_GetTick(); } +void handleDashSpeedCntrlKiTimes1000(Frame *msg) { + can0_DashSpeedCntrlKiTimes1000_T unpacked_msg; + unpack_can0_DashSpeedCntrlKiTimes1000(msg, &unpacked_msg); + set_ki(unpacked_msg.ki_times_1000); +} + +void handleDashSpeedCntrlKpTimes1000(Frame *msg) { + can0_DashSpeedCntrlKpTimes1000_T unpacked_msg; + unpack_can0_DashSpeedCntrlKpTimes1000(msg, &unpacked_msg); + set_kp(unpacked_msg.kp_times_1000); +} + +void handle_DashSpeedCntrlRPMSetpoint(Frame *msg) { + unpack_can0_DashSpeedCntrlRPMSetpoint(msg, &rpm_setpoint); +} + void handleMCVoltageMsg(Frame *msg) { can0_MCVoltage_T unpacked_msg; @@ -449,3 +478,136 @@ void send_mc_fault_clear(void) { can0_MCParameterRequest_Write(&msg); } + +void sendVCUSpeedCntrlKpTimes1000Msg(void) { + LIMIT(can0_VCUSpeedCntrlKpTimes1000) + + can0_VCUSpeedCntrlKpTimes1000_Write(&kp_times_1000_frame); +} + +void sendVCUSpeedCntrlKiTimes1000Msg(void) { + LIMIT(can0_VCUSpeedCntrlKiTimes1000) + + can0_VCUSpeedCntrlKiTimes1000_Write(&ki_times_1000_frame); +} + +void sendVCUSpeedCntrlKdTimes1000Msg(void) { + LIMIT(can0_VCUSpeedCntrlKdTimes1000) + + can0_VCUSpeedCntrlKdTimes1000_Write(&kd_times_1000_frame); +} + +void sendVCUSpeedCntrlIWindupMaxMsg(void) { + LIMIT(can0_VCUSpeedCntrlIWindupMax) + + can0_VCUSpeedCntrlIWindupMax_Write(&i_windup_max_frame); +} + +void sendVCUSpeedCntrlIWindupMinMsg(void) { + LIMIT(can0_VCUSpeedCntrlIWindupMin) + + can0_VCUSpeedCntrlIWindupMin_Write(&i_windup_min_frame); +} + +void sendVCUSpeedCntrlMinOutputValueMsg(void) { + LIMIT(can0_VCUSpeedCntrlMinOutputValue) + + can0_VCUSpeedCntrlMinOutputValue_Write(&min_output_value_frame); +} + +void sendVCUSpeedCntrlMaxOutputValueMsg(void) { + LIMIT(can0_VCUSpeedCntrlMaxOutputValue) + + can0_VCUSpeedCntrlMaxOutputValue_Write(&max_output_value_frame); +} + +void sendVCUSpeedCntrlMinInputValueMsg(void) { + LIMIT(can0_VCUSpeedCntrlMinInputValue) + + can0_VCUSpeedCntrlMinInputValue_Write(&min_input_value_frame); +} + +void sendVCUSpeedCntrlMaxInputValueMsg(void) { + LIMIT(can0_VCUSpeedCntrlMaxInputValue) + + can0_VCUSpeedCntrlMaxInputValue_Write(&max_input_value_frame); +} + +void sendVCUSpeedCntrlErrorUpdateTimeoutMsg(void) { + LIMIT(can0_VCUSpeedCntrlErrorUpdateTimeout) + + can0_VCUSpeedCntrlErrorUpdateTimeout_Write(&dt_frame); +} + +void sendVCUSpeedCntrlDtMsg(void) { + LIMIT(can0_VCUSpeedCntrlDt) + + can0_VCUSpeedCntrlDt_Write(&error_update_timeout_frame); +} + +void sendVCUSpeedCntrlEnabledMsg(void) { + LIMIT(can0_VCUSpeedCntrlEnabled) + + can0_VCUSpeedCntrlEnabled_Write(&enabled_frame); +} + +void sendVCUSpeedCntrlOutOfInputRangeThrottledMsg(void) { + LIMIT(can0_VCUSpeedCntrlOutOfInputRangeThrottled) + + can0_VCUSpeedCntrlOutOfInputRangeThrottled_Write(&out_of_input_range_throttled_frame); +} + +void sendVCUSpeedCntrlOutOfOutputRangeThrottledMsg(void) { + LIMIT(can0_VCUSpeedCntrlOutOfOutputRangeThrottled) + + can0_VCUSpeedCntrlOutOfOutputRangeThrottled_Write(&out_of_output_range_throttled_frame); +} + +void sendVCUSpeedCntrlErrorUpdateTimedOutMsg(void) { + LIMIT(can0_VCUSpeedCntrlErrorUpdateTimedOut) + + can0_VCUSpeedCntrlErrorUpdateTimedOut_Write(&error_update_timed_out_frame); +} + +void sendVCUSpeedCntrlRPMSetpointMsg(void) { + LIMIT(can0_VCUSpeedCntrlRPMSetpoint) + + can0_VCUSpeedCntrlRPMSetpoint_Write(&rpm_setpoint_frame); +} + +void sendVCUSpeedCntrlCommandedTorqueMsg(void) { + LIMIT(can0_VCUSpeedCntrlCommandedTorque) + + can0_VCUSpeedCntrlCommandedTorque_Write(&commanded_torque_frame); +} + +void sendVCUSpeedCntrlRPMErrorMsg(void) { + LIMIT(can0_VCUSpeedCntrlRPMError) + + can0_VCUSpeedCntrlRPMError_Write(&rpm_error_frame); +} + +void sendVCUSpeedCntrlLastRPMErrorMsg(void) { + LIMIT(can0_VCUSpeedCntrlLastRPMError) + + can0_VCUSpeedCntrlLastRPMError_Write(&last_rpm_error_frame); +} + +void sendVCUSpeedCntrlDerivRPMErrorMsg(void) { + LIMIT(can0_VCUSpeedCntrlDerivRPMError) + + can0_VCUSpeedCntrlDerivRPMError_Write(&deriv_rpm_error_frame); +} + +void sendVCUSpeedCntrlRPMErrorAccumulatedMsg(void) { + LIMIT(can0_VCUSpeedCntrlRPMErrorAccumulated) + + can0_VCUSpeedCntrlRPMErrorAccumulated_Write(&rpm_error_accumulated_frame); +} + +void sendVCUSpeedCntrlLastErrorUpdateTimestampMsg(void) { + LIMIT(can0_VCUSpeedCntrlLastErrorUpdateTimestamp) + + can0_VCUSpeedCntrlLastErrorUpdateTimestamp_Write(&last_error_update_timestamp_frame); +} + diff --git a/src/vcu/src/controls.c b/src/vcu/src/controls.c index ab9e67c..9ad36ca 100644 --- a/src/vcu/src/controls.c +++ b/src/vcu/src/controls.c @@ -3,10 +3,17 @@ static bool enabled = false; static int32_t torque_command = 0; static int32_t speed_command = 0; + +static bool lc_state_locked_until_next_rtd = true; + can0_VCUControlsParams_T control_settings = {}; can0_VCUControlsParamsLC_T lc_settings = {}; +can0_DashSpeedCntrlRPMSetpoint_T rpm_setpoint = {}; + static Launch_Control_State_T lc_state = BEFORE; +uint32_t last_time_step = 0; + uint32_t get_front_wheel_speed(void); static bool any_lc_faults(); @@ -34,6 +41,11 @@ void init_controls_defaults(void) { lc_settings.speeding_up_torque = LC_DEFAULT_SPEEDING_UP_TORQUE; lc_settings.speeding_up_speed = LC_DEFAULT_SPEEDING_UP_SPEED; lc_settings.ws_thresh = LC_DEFAULT_WS_THRESH; + + rpm_setpoint.rpm_setpoint = 0; + + init_speed_controller_defaults(MAX_INPUT_SPEED, + MAX_TORQUE, SPEED_CONTROLLER_UPDATE_PERIOD_MS); } void enable_controls(void) { @@ -59,6 +71,19 @@ bool get_controls_enabled(void) { return enabled; } +// locks LC from being re-enabled by outside entities until RTD occurs +void lock_lc_state(void) { + lc_state_locked_until_next_rtd = true; +} + +void lc_rtd_callback(void) { + lc_state_locked_until_next_rtd = false; +} + +bool get_lc_state_locked_until_next_rtd(void) { + return lc_state_locked_until_next_rtd; +} + void execute_controls(void) { if (!enabled) return; bool launch_ctrl_entered = false; @@ -132,19 +157,47 @@ void execute_controls(void) { switch (lc_state) { case BEFORE: sendTorqueCmdMsg(0); + disable_speed_controller(); // Transition if (pedalbox_min(accel) > LC_ACCEL_BEGIN) { lc_state = SPEED_CONTROLLER; - printf("[LAUNCH CONTROL] SPEED CONTROLLER STATE ENTERED\r\n"); + enable_speed_controller(); + printf("[LAUNCH CONTROL] SPEEDING UP STATE ENTERED\r\n"); } break; case SPEED_CONTROLLER: - speed_command = 500; // get_launch_control_speed(front_wheel_speed); - sendSpeedCmdMsg(speed_command, torque_command); + + // speed_command = get_launch_control_speed(front_wheel_speed); + // speed_command = 500; + + + set_speed_controller_setpoint(rpm_setpoint.rpm_setpoint); // RPM + + // Update the internal speed controller with the new speed value + // TODO: replace HAL_GetTick with the timestamp of the message + update_speed_controller_error(mc_readings.speed, HAL_GetTick()); + + int32_t speedControlTorqueOutput = get_speed_controller_torque_command(); + if (speedControlTorqueOutput > torque_command) { + speedControlTorqueOutput = torque_command; + } + + if (HAL_GetTick() - last_time_step > 50) { + //printf("[SC] SETPOINT: %d, ERR: %d, TORQUE: %d, ACCUM: %d, DERIV: %d\r\n", + // speed_command, get_speed_controller_error(), speedControlTorqueOutput, + // get_speed_controller_accum(), get_speed_controller_deriv()); + + last_time_step = HAL_GetTick(); + } + + sendTorqueCmdMsg(speedControlTorqueOutput); + break; case ZERO_TORQUE: sendTorqueCmdMsg(0); + lock_lc_state(); + disable_speed_controller(); if (pedalbox_max(accel) < LC_ACCEL_RELEASE) { lc_state = DONE; @@ -153,6 +206,8 @@ void execute_controls(void) { break; default: sendTorqueCmdMsg(0); + lock_lc_state(); + disable_speed_controller(); printf("ERROR: NO STATE!\r\n"); break; @@ -242,10 +297,12 @@ static bool any_lc_faults(void) { // that is indicative of driver braking intent. The other brake is for regen. printf("[LAUNCH CONTROL ERROR] Brake (%d) too high\r\n", pedalbox.brake_2); return true; - } else /*else if (mc_readings.speed > LC_BACKWARDS_CUTOFF) {m - printf("[LAUNCH CONTROL ERROR] MC reading (%d) is great than cutoff (%d)\r\n", mc_readings.speed, LC_BACKWARDS_CUTOFF); - return true; - }*/ + } + + // else if (mc_readings.speed > LC_BACKWARDS_CUTOFF) { + // printf("[LAUNCH CONTROL ERROR] MC reading (%d) is great than cutoff (%d)\r\n", mc_readings.speed, LC_BACKWARDS_CUTOFF); + // return true; + // } return false; } diff --git a/src/vcu/src/data_processing.py b/src/vcu/src/data_processing.py new file mode 100644 index 0000000..d8c0b11 --- /dev/null +++ b/src/vcu/src/data_processing.py @@ -0,0 +1,90 @@ +import numpy as np +import matplotlib.pyplot as plt +import os + +#DAY = "20180916" +DAY = "20180909" +DATA_DIR = "/home/dani/Documents/racecar/testing_data/" + DAY + "/numpy/" +#DATA_FILE = "183444.npz" +#DATA_FILE = "184836.npz" +#data = np.load(DATA_DIR + DATA_FILE) +# DATA_DIR_INTERP = "/home/dani/Documents/racecar/testing_data/20180909/numpy_interp/" +# data_interp = np.load(DATA_DIR_INTERP + DATA_FILE) + + +def get_column(arr, index): + return list(i[index] for i in arr) + + +def plot(data_time_columns): + plt.plot(data_time_columns[0],data_time_columns[1],label=data_time_columns[2]) + + +# front_wheel_speeds = get_column(data_interp['FrontCanNodeLeftWheelSpeed'],1) +# resolver_speeds = get_column(data_interp['MCMotorPositionInfo'],2) + +# plt.plot(resolver_speeds/front_wheel_speeds, label="Slip ratio") + +#print(data['MCTorqueTimerInfo']) +#print(get_column(data['MCTorqueTimerInfo'],3)[0]) + +# 9/9 +#DATA_FILES = ["192451.npz"]# #["184836.npz","185112.npz","185654.npz","192046.npz","192323.npz","192451.npz","192553.npz","192738.npz","192844.npz","193041.npz","193221.npz", ] +DATA_FILES = ["193344.npz"] # toruqe +# #,"193505.npz",] + +# 9/16 +# DATA_FILES = ["123451.npz"] +# DATA_FILES = ["123019.npz"] +#DATA_FILES = ["172219.npz"] # dip +# DATA_FILES = ["112533.npz"] # backwards + +# DATA_FILES = os.listdir(DATA_DIR) +for DATA_FILE in DATA_FILES:#192451.npz + print(DATA_FILE) + #print(get_column(data['DashRequestLC'],3)) + # print() + + try: + data = np.load(DATA_DIR + DATA_FILE) + + messages = [] + messages.append([get_column(data['MCTorqueTimerInfo'],0), [i * -1 for i in get_column(data['MCTorqueTimerInfo'],3)], "Feedback torque", ""]) + messages.append([get_column(data['MCCommand'],0), get_column(data['MCCommand'],1), "Commanded speed", " "]) + messages.append([get_column(data['MCCommand'],0), get_column(data['MCCommand'],6), "Commanded torque", " "]) + messages.append([get_column(data['FrontCanNodeLeftWheelSpeed'],0), [i/1000 for i in get_column(data['FrontCanNodeLeftWheelSpeed'],2)], "Front wheel speed", ""]) + messages.append([get_column(data['MCFluxInfo'],0), get_column(data['MCFluxInfo'],2), "Iq feedback", ""]) + messages.append([get_column(data['MCFluxInfo'],0), get_column(data['MCFluxInfo'],3), "Flux command", ""]) + messages.append([get_column(data['MCFluxInfo'],0), get_column(data['MCFluxInfo'],4), "Flux feedback", ""]) + except: + continue + # Find min time + min_time = -1 + for message in messages: + if min_time == -1: + min_time = min(message[0]) + else: + min_time = min(min_time, min(message[0])) + + # Subtract out min time + for message in messages: + for i, time in enumerate(message[0]): + message[0][i] = time - min_time + + #print(messages) + + for message in messages: + plt.plot(message[0], message[1], label=message[2], marker=message[3]) + + +# plt.plot(, , label="Feedback torque") +# plt.plot(get_column(data['MCTorqueTimerInfo'],0), [100 for i in get_column(data['MCTorqueTimerInfo'],3)], label="Feedback torque") +# plt.plot(get_column(data['MCCommand'],0), get_column(data['MCCommand'],1), label="Commanded speed") +# plt.plot(get_column(data['MCTorqueTimerInfo'],0), get_column(data['MCTorqueTimerInfo'],1), label="Commanded torque") +# plt.plot(get_column(data['FrontCanNodeLeftWheelSpeed'],0), [i/1000 for i in get_column(data['FrontCanNodeLeftWheelSpeed'],2)], label="Front wheel speed")#, marker='o') +# plt.plot(get_column(data['MCTorqueTimerInfo'],0), [2400 for i in get_column(data['MCTorqueTimerInfo'],3)], label="Feedback toqrue") + #plt.plot(get_column(data['DashRequestLC'],0),get_column(data['DashRequestLC'],3), label="Speeding up torque") + + plt.legend() + plt.ticklabel_format(scientific=False) + plt.show() diff --git a/src/vcu/src/main.c b/src/vcu/src/main.c index 73149cb..e05b0a6 100755 --- a/src/vcu/src/main.c +++ b/src/vcu/src/main.c @@ -88,6 +88,7 @@ int main(void) { lastt = HAL_GetTick(); } if (HAL_GetTick() - lastt2 > 100) { + //printf("Rpm setpoint: %d\r\n", //rpm_setpoint.rpm_setpoint); // printf("CONTROLS PARAMS:\r\n speeding_up_torque: %d\r\n speeding_up_speed: %d\r\n ws_thresh: %d\r\n launch_ctrl_slip_ratio: %d\r\n using_launch_ctrl: %d\r\n accel: %d\r\n wheel speed: %d\r\n", // lc_settings.speeding_up_torque, lc_settings.speeding_up_speed, lc_settings.ws_thresh, lc_settings.launch_ctrl_slip_ratio, lc_settings.using_launch_ctrl, pedalbox_min(accel), get_front_wheel_speed()); // switch (get_lc_state()) { diff --git a/src/vcu/src/speed_controller.c b/src/vcu/src/speed_controller.c new file mode 100644 index 0000000..b6cf68e --- /dev/null +++ b/src/vcu/src/speed_controller.c @@ -0,0 +1,292 @@ +#include "speed_controller.h" + +static SpeedControllerParams speed_controller_params; +static SpeedControllerInternalVars speed_controller_vars; + +can0_VCUSpeedCntrlKpTimes1000_T kp_times_1000_frame = {}; +can0_VCUSpeedCntrlKiTimes1000_T ki_times_1000_frame = {}; +can0_VCUSpeedCntrlKdTimes1000_T kd_times_1000_frame = {}; +can0_VCUSpeedCntrlIWindupMax_T i_windup_max_frame = {}; +can0_VCUSpeedCntrlIWindupMin_T i_windup_min_frame = {}; +can0_VCUSpeedCntrlMinOutputValue_T min_output_value_frame = {}; +can0_VCUSpeedCntrlMaxOutputValue_T max_output_value_frame = {}; +can0_VCUSpeedCntrlMinInputValue_T min_input_value_frame = {}; +can0_VCUSpeedCntrlMaxInputValue_T max_input_value_frame = {}; +can0_VCUSpeedCntrlErrorUpdateTimeout_T error_update_timeout_frame = {}; +can0_VCUSpeedCntrlDt_T dt_frame = {}; +can0_VCUSpeedCntrlEnabled_T enabled_frame = {}; +can0_VCUSpeedCntrlOutOfInputRangeThrottled_T out_of_input_range_throttled_frame = {}; +can0_VCUSpeedCntrlOutOfOutputRangeThrottled_T out_of_output_range_throttled_frame = {}; +can0_VCUSpeedCntrlErrorUpdateTimedOut_T error_update_timed_out_frame = {}; +can0_VCUSpeedCntrlRPMSetpoint_T rpm_setpoint_frame = {}; +can0_VCUSpeedCntrlCommandedTorque_T commanded_torque_frame = {}; +can0_VCUSpeedCntrlRPMError_T rpm_error_frame = {}; +can0_VCUSpeedCntrlLastRPMError_T last_rpm_error_frame = {}; +can0_VCUSpeedCntrlDerivRPMError_T deriv_rpm_error_frame = {}; +can0_VCUSpeedCntrlRPMErrorAccumulated_T rpm_error_accumulated_frame = {}; +can0_VCUSpeedCntrlLastErrorUpdateTimestamp_T last_error_update_timestamp_frame = {}; + +// PRIVATE FUNCTIONS +static void reset_internal_vars(void); +static void update_error_internal(int32_t actual); +static void update_can_frames(void); +static void send_speed_controller_can_msgs(void); + +static void reset_internal_vars(void) { + speed_controller_vars.out_of_input_range_throttled = false; + speed_controller_vars.out_of_output_range_throttled = false; + speed_controller_vars.error_update_timed_out = false; + speed_controller_vars.rpm_setpoint = 0; + speed_controller_vars.commanded_torque = 0; + speed_controller_vars.rpm_error = 0; + speed_controller_vars.deriv_rpm_error = 0; + speed_controller_vars.last_error_update_timestamp = HAL_GetTick(); + speed_controller_vars.last_rpm_error = 0; + speed_controller_vars.rpm_error_accumulated = 0; +} + +void init_speed_controller_defaults(int32_t max_input_speed, + int32_t max_output_torque, uint32_t update_period_millis) { + + // Internal controller vars + speed_controller_vars.enabled = false; + reset_internal_vars(); + + // Parameters + speed_controller_params.kp_times_1000 = 0; + speed_controller_params.ki_times_1000 = 0; //10; + speed_controller_params.kd_times_1000 = 0; //1000; + speed_controller_params.i_windup_max = 100000; + speed_controller_params.i_windup_min = -100000; + speed_controller_params.min_output_value = 0; + speed_controller_params.max_output_value = max_output_torque; + speed_controller_params.min_input_value = 0; + speed_controller_params.max_input_value = max_input_speed; + // PID loop runs on a fixed update interval to keep i and d updates + // consistent. Otherwise, integral and derivative term can become unstable. + speed_controller_params.dt = update_period_millis; + + // TODO: The above constraint must be enforced (enforce dt on the caller probs, may not need to tho) + + // millis of time before controller decides it has not been updated + // recently enough (done both at a controller internal timestamp + // and at a actual rpm can msg timestamp level) + speed_controller_params.error_update_timeout = 50; +} + +void enable_speed_controller(void) { + reset_internal_vars(); + speed_controller_vars.enabled = true; +} + +void disable_speed_controller(void) { + printf("DISABLE SPEED CONTROL!!!!!!!!!!!\r\n"); + speed_controller_vars.enabled = false; + reset_internal_vars(); +} + +void set_speed_controller_setpoint(int32_t rpm) { + // Input is too low + if (rpm < speed_controller_params.min_input_value) { + speed_controller_vars.out_of_input_range_throttled = true; + speed_controller_vars.rpm_setpoint = speed_controller_params.min_input_value; + return; + } + + // Input is too high + if (rpm > speed_controller_params.max_input_value) { + speed_controller_vars.out_of_input_range_throttled = true; + speed_controller_vars.rpm_setpoint = speed_controller_params.max_input_value; + return; + } + + speed_controller_vars.rpm_setpoint = rpm; +} + +int32_t get_speed_controller_error(void) { + return speed_controller_vars.rpm_error; +} + +int32_t get_speed_controller_accum(void) { + return speed_controller_vars.rpm_error_accumulated; +} + +int32_t get_speed_controller_deriv(void) { + return speed_controller_vars.deriv_rpm_error; +} + +void update_speed_controller_error(int32_t actual_rpm, + uint32_t actual_rpm_msg_timestamp) { + + // Check for function call time-outs + // uint32_t func_call_dt = HAL_GetTick() - speed_controller_vars.last_error_update_timestamp; + + // if (func_call_dt > speed_controller_params.error_update_timeout) { + // disable_speed_controller(); + // speed_controller_vars.error_update_timed_out = true; + // return; + // } + + // Check for CAN based time-outs + // uint32_t can_msg_dt = HAL_GetTick() - actual_rpm_msg_timestamp; + + // if (can_msg_dt > speed_controller_params.error_update_timeout) { + // disable_speed_controller(); + // speed_controller_vars.error_update_timed_out = true; + // return; + // } + + // TODO: Overhead/underhead counter output so we know if we are not consistent w/ dt + // Check to see if an update can be performed + if (HAL_GetTick() - speed_controller_vars.last_error_update_timestamp + >= speed_controller_params.dt) { + + update_error_internal(actual_rpm); + + speed_controller_vars.last_error_update_timestamp = HAL_GetTick(); + } + + update_can_frames(); + send_speed_controller_can_msgs(); +} + +static void update_error_internal(int32_t actual) { + // update from the last step + speed_controller_vars.last_rpm_error = speed_controller_vars.rpm_error; + + // take error diff + speed_controller_vars.rpm_error = speed_controller_vars.rpm_setpoint - (-1.0 * actual); // motor speed is negative + + // Multiply by 100 for precision, example: 1234 rpm * 1000 / 12ms = 102833, 1 rpm * 1000 / 12ms = 83 + speed_controller_vars.deriv_rpm_error = + (speed_controller_vars.rpm_error + - speed_controller_vars.last_rpm_error) / speed_controller_params.dt; + + int32_t accum = speed_controller_vars.rpm_error_accumulated + + speed_controller_vars.rpm_error * speed_controller_params.dt; + + // TODO: Double-check dimensions for these + // account for integral wind-up + if (accum > speed_controller_params.i_windup_max) { + accum = speed_controller_params.i_windup_max; + } else if (accum < speed_controller_params.i_windup_min) { + accum = speed_controller_params.i_windup_min; + } + + speed_controller_vars.rpm_error_accumulated = accum; +} + +int32_t get_speed_controller_torque_command(void) { + if (!speed_controller_vars.enabled) { + return 0; + } + + // CAREFUL! ADDING MORE PRECISION HERE WILL OVERFLOW THE INT32 FOR THE VALUES THE CONTROLLER IS EXPECTING + // RUN THIS ON SOME PAPER WITH HIGH AND LOW VALUES + int32_t torque = (speed_controller_params.kp_times_1000 * speed_controller_vars.rpm_error / 1000) + + (speed_controller_params.ki_times_1000 * speed_controller_vars.rpm_error_accumulated / 10000) // divide by one more here for keeping the rate proper + + (speed_controller_params.kd_times_1000 * speed_controller_vars.deriv_rpm_error / 1000); + + if (torque < speed_controller_params.min_output_value) { + speed_controller_vars.out_of_output_range_throttled = true; + return speed_controller_params.min_output_value; + } + + if (torque > speed_controller_params.max_output_value) { + speed_controller_vars.out_of_output_range_throttled = true; + return speed_controller_params.max_output_value; + } + + return torque; +} + +static void update_can_frames(void) { + kp_times_1000_frame.kp_times_1000 = speed_controller_params.kp_times_1000; + + ki_times_1000_frame.ki_times_1000 = speed_controller_params.ki_times_1000; + + kd_times_1000_frame.kd_times_1000 = speed_controller_params.kd_times_1000; + + i_windup_max_frame.i_windup_max = speed_controller_params.i_windup_max; + + i_windup_min_frame.i_windup_min = speed_controller_params.i_windup_min; + + min_output_value_frame.min_output_value = speed_controller_params.min_output_value; + + max_output_value_frame.max_output_value = speed_controller_params.max_output_value; + + min_input_value_frame.min_input_value = speed_controller_params.min_input_value; + + max_input_value_frame.max_input_value = speed_controller_params.max_input_value; + + dt_frame.dt = speed_controller_params.dt; + + error_update_timeout_frame.error_update_timeout = speed_controller_params.error_update_timeout; + + + + enabled_frame.enabled = speed_controller_vars.enabled; + + out_of_input_range_throttled_frame.out_of_input_range_throttled = speed_controller_vars.out_of_input_range_throttled; + + out_of_output_range_throttled_frame.out_of_output_range_throttled = speed_controller_vars.out_of_output_range_throttled; + + error_update_timed_out_frame.error_update_timed_out = speed_controller_vars.error_update_timed_out; + + rpm_setpoint_frame.rpm_setpoint = speed_controller_vars.rpm_setpoint; + + commanded_torque_frame.commanded_torque = speed_controller_vars.commanded_torque; + + rpm_error_frame.rpm_error = speed_controller_vars.rpm_error; + + last_rpm_error_frame.last_rpm_error = speed_controller_vars.last_rpm_error; + + deriv_rpm_error_frame.deriv_rpm_error = speed_controller_vars.deriv_rpm_error; + + rpm_error_accumulated_frame.rpm_error_accumulated = speed_controller_vars.rpm_error_accumulated; + + last_error_update_timestamp_frame.last_error_update_timestamp = speed_controller_vars.last_error_update_timestamp; +} + +void send_speed_controller_can_msgs(void) { + sendVCUSpeedCntrlKpTimes1000Msg(); + sendVCUSpeedCntrlKiTimes1000Msg(); + sendVCUSpeedCntrlKdTimes1000Msg(); + sendVCUSpeedCntrlIWindupMaxMsg(); + sendVCUSpeedCntrlIWindupMinMsg(); + sendVCUSpeedCntrlMinOutputValueMsg(); + sendVCUSpeedCntrlMaxOutputValueMsg(); + sendVCUSpeedCntrlMinInputValueMsg(); + sendVCUSpeedCntrlMaxInputValueMsg(); + sendVCUSpeedCntrlErrorUpdateTimeoutMsg(); + sendVCUSpeedCntrlDtMsg(); + sendVCUSpeedCntrlEnabledMsg(); + sendVCUSpeedCntrlOutOfInputRangeThrottledMsg(); + sendVCUSpeedCntrlOutOfOutputRangeThrottledMsg(); + sendVCUSpeedCntrlErrorUpdateTimedOutMsg(); + sendVCUSpeedCntrlRPMSetpointMsg(); + sendVCUSpeedCntrlCommandedTorqueMsg(); + sendVCUSpeedCntrlRPMErrorMsg(); + sendVCUSpeedCntrlLastRPMErrorMsg(); + sendVCUSpeedCntrlDerivRPMErrorMsg(); + sendVCUSpeedCntrlRPMErrorAccumulatedMsg(); + sendVCUSpeedCntrlLastErrorUpdateTimestampMsg(); +} + +bool get_speed_controller_enabled(void) { + return speed_controller_vars.enabled; +} + +void set_kp(int32_t new_kp) { + //printf("New kp: %d\r\n", new_kp); + if (!speed_controller_vars.enabled) { + speed_controller_params.kp_times_1000 = new_kp; + } +} + +void set_ki(int32_t new_ki) { + //printf("New ki: %d\r\n", new_ki); + if (!speed_controller_vars.enabled) { + speed_controller_params.ki_times_1000 = new_ki; + } +} diff --git a/src/vcu/src/state_vcu_driving.c b/src/vcu/src/state_vcu_driving.c index 3eecb2c..63493f2 100644 --- a/src/vcu/src/state_vcu_driving.c +++ b/src/vcu/src/state_vcu_driving.c @@ -35,7 +35,7 @@ void update_vcu_state_driving(void) { } // Calculate and send motor controller commands. - if (!get_controls_enabled()) { + if (!get_controls_enabled() && !get_lc_state_locked_until_next_rtd()) { enable_controls(); } execute_controls(); diff --git a/src/vcu/src/state_vcu_rtd.c b/src/vcu/src/state_vcu_rtd.c index d842a62..d5ca1e4 100644 --- a/src/vcu/src/state_vcu_rtd.c +++ b/src/vcu/src/state_vcu_rtd.c @@ -1,4 +1,5 @@ #include "state_vcu_rtd.h" +#include "controls.h" const Time_T RTD_HOLD = 1000; @@ -32,6 +33,7 @@ void update_vcu_state_rtd(void) { if (rtd_started) { if (HAL_GetTick() - rtd_last > RTD_HOLD) { if (brk_pressed) { + lc_rtd_callback(); // callback for controls to get ready after any rtd set_vcu_state(VCU_STATE_DRIVING); return; }