diff --git a/.gitignore b/.gitignore index a133ed9..8040b7d 100644 --- a/.gitignore +++ b/.gitignore @@ -8,6 +8,8 @@ test/**/*.png # Annoying ropeproject **/.ropeproject +test/**/*.png + # vim *.swp diff --git a/can_spec_my18.yml b/can_spec_my18.yml index f7e4987..a3d7853 100644 --- a/can_spec_my18.yml +++ b/can_spec_my18.yml @@ -370,16 +370,6 @@ buses: position: 6 length: 1 c_type: bool - lc_state: - position: 7 - length: 3 - c_type: enum - enum: - BEFORE: 0 - SPEEDING_UP: 1 - SPEED_CONTROLLER: 2 - ZERO_TORQUE: 3 - DONE: 4 VCUErrors: can_id: 0x0F4 period: 111ms @@ -686,6 +676,31 @@ buses: position: 60 length: 1 c_type: bool + DashRequestLC: + can_id: 0x0D5 + is_big_endian: true + period: 69ms + segments: + speeding_up_torque: + position: 0 + length: 16 + c_type: uint16_t + speeding_up_speed: + position: 16 + length: 16 + c_type: uint16_t + ws_thresh: + position: 32 + length: 16 + c_type: uint16_t + launch_ctrl_slip_ratio: # Ranges from 105 to 115 + position: 48 + length: 8 + c_type: uint16_t + using_launch_ctrl: + position: 56 + length: 1 + c_type: bool VCUControlsParams: can_id: 0x0D4 is_big_endian: true @@ -735,6 +750,41 @@ buses: position: 60 length: 1 c_type: bool + VCUControlsParamsLC: + can_id: 0x0D6 + is_big_endian: true + period: 200ms + segments: + speeding_up_torque: + position: 0 + length: 16 + c_type: uint16_t + speeding_up_speed: + position: 16 + length: 16 + c_type: uint16_t + ws_thresh: + position: 32 + length: 16 + c_type: uint16_t + launch_ctrl_slip_ratio: # Ranges from 105 to 115 + position: 48 + length: 8 + c_type: uint16_t + using_launch_ctrl: + position: 56 + length: 1 + c_type: bool + lc_state: + position: 57 + length: 3 + c_type: enum + enum: + BEFORE: 0 + SPEEDING_UP: 1 + SPEED_CONTROLLER: 2 + ZERO_TORQUE: 3 + DONE: 4 ButtonRequest: can_id: 0x0D8 period: 73ms diff --git a/src/dashboard/inc/carstats.h b/src/dashboard/inc/carstats.h index 23ee4fb..629e4df 100644 --- a/src/dashboard/inc/carstats.h +++ b/src/dashboard/inc/carstats.h @@ -58,9 +58,12 @@ typedef struct { // Controls values can0_DashRequest_T controls; + can0_DashRequestLC_T lc_controls; can0_VCUControlsParams_T vcu_controls; + can0_VCUControlsParamsLC_T vcu_lc_controls; bool vcu_controls_received; + bool vcu_lc_controls_received; Buttons_T buttons; } carstats_t; diff --git a/src/dashboard/inc/page_manager.h b/src/dashboard/inc/page_manager.h index eb2f1c5..3420519 100644 --- a/src/dashboard/inc/page_manager.h +++ b/src/dashboard/inc/page_manager.h @@ -9,6 +9,7 @@ typedef enum { DASH_PAGE_CRITICAL = 0, DASH_PAGE_TRACTION, DASH_PAGE_REGEN, + DASH_PAGE_LAUNCH_CONTROL, DASH_PAGE_TEMP_LIM, DASH_PAGE_VOLT_LIM, DASH_PAGE_FAULT, diff --git a/src/dashboard/src/carstats.c b/src/dashboard/src/carstats.c index e2fd9f7..3159c5f 100644 --- a/src/dashboard/src/carstats.c +++ b/src/dashboard/src/carstats.c @@ -78,9 +78,9 @@ void can_handle_bms_heartbeat(carstats_t *cs) { cs->soc = msg.soc; } -void can_handle_mc_temperature1(carstats_t *cs) { - can0_MCTemperature1_T msg; - unpack_can0_MCTemperature1(&frame, &msg); +void can_handle_mc_temperature(carstats_t *cs) { + can0_MCTemperature_T msg; + unpack_can0_MCTemperature(&frame, &msg); int16_t max = msg.module_a_temp; if (msg.module_b_temp > max) @@ -117,6 +117,11 @@ void can_handle_vcu_controls(carstats_t *cs) { cs->vcu_controls_received = true; } +void can_handle_vcu_controls_lc(carstats_t *cs) { + unpack_can0_VCUControlsParamsLC(&frame, &cs->vcu_lc_controls); + cs->vcu_lc_controls_received = true; +} + void can_update_carstats(carstats_t *cs) { handle_can_error(Can_RawRead(&frame)); @@ -151,8 +156,8 @@ void can_update_carstats(carstats_t *cs) { case can0_VCUHeartbeat: can_handle_vcu_heartbeat(cs); break; - case can0_MCTemperature1: - can_handle_mc_temperature1(cs); + case can0_MCTemperature: + can_handle_mc_temperature(cs); break; case can0_ButtonRequest: can_handle_button_request(cs); @@ -162,6 +167,10 @@ void can_update_carstats(carstats_t *cs) { break; case can0_VCUControlsParams: can_handle_vcu_controls(cs); + break; + case can0_VCUControlsParamsLC: + can_handle_vcu_controls_lc(cs); + break; default: // do nothing break; diff --git a/src/dashboard/src/dispatch.c b/src/dashboard/src/dispatch.c index 67c1abe..13e5312 100644 --- a/src/dashboard/src/dispatch.c +++ b/src/dashboard/src/dispatch.c @@ -99,7 +99,14 @@ void dispatch_init() { carstats.controls.using_voltage_limiting = false; carstats.controls.active_aero_enabled = false; + carstats.lc_controls.speeding_up_torque = -1; + carstats.lc_controls.speeding_up_speed = -1; + carstats.lc_controls.ws_thresh = -1; + carstats.lc_controls.launch_ctrl_slip_ratio = -1; + carstats.lc_controls.using_launch_ctrl = false; + carstats.vcu_controls_received = false; + carstats.vcu_lc_controls_received = false; init_button_state(&carstats.buttons.left); init_button_state(&carstats.buttons.right); @@ -169,6 +176,7 @@ void update_lights(void) { 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)); } void vcu_controls_update(void) { diff --git a/src/dashboard/src/page_manager.c b/src/dashboard/src/page_manager.c index ace7e81..7e8e18c 100644 --- a/src/dashboard/src/page_manager.c +++ b/src/dashboard/src/page_manager.c @@ -49,6 +49,7 @@ void draw_nav_line(page_manager_t *pm, NHD_US2066_OLED *oled) { void draw_critical_page(page_manager_t *pm, NHD_US2066_OLED *oled); 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); @@ -60,8 +61,11 @@ void page_manager_update(page_manager_t *pm, NHD_US2066_OLED *oled) { draw_critical_page(pm, oled); break; case DASH_PAGE_REGEN: - draw_regen_page(pm, oled); - break; + draw_regen_page(pm, oled); + break; + case DASH_PAGE_LAUNCH_CONTROL: + draw_launch_control_page(pm, oled); + break; case DASH_PAGE_TEMP_LIM: draw_temp_lim_page(pm, oled); break; @@ -430,6 +434,122 @@ void draw_volt_lim_page(page_manager_t *pm, NHD_US2066_OLED *oled) { oled_print(oled, ">"); } +void draw_launch_control_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, 1, 5); + + if (stats->buttons.left.action == BUTTON_ACTION_TAP) { + switch (var_toggled) { + case 1: + stats->lc_controls.using_launch_ctrl ^= 1; + break; + case 2: + stats->lc_controls.launch_ctrl_slip_ratio -= 1; + break; + case 3: + stats->lc_controls.speeding_up_torque -= 100; + break; + case 4: + stats->lc_controls.speeding_up_speed -= 200; + break; + case 5: + stats->lc_controls.ws_thresh -= 100; + break; + } + } + + if (stats->buttons.right.action == BUTTON_ACTION_TAP) { + switch (var_toggled) { + case 1: + stats->lc_controls.using_launch_ctrl ^= 1; + break; + case 2: + stats->lc_controls.launch_ctrl_slip_ratio += 1; + break; + case 3: + stats->lc_controls.speeding_up_torque += 100; + break; + case 4: + stats->lc_controls.speeding_up_speed += 200; + break; + case 5: + stats->lc_controls.ws_thresh += 100; + break; + } + } + + oled_clearline(oled, 0); + oled_set_pos(oled, 0, 0); + switch (stats->vcu_lc_controls.lc_state) { + case 0: + oled_print(oled, "BEFORE"); + break; + case 1: + oled_print(oled, "SPEEDING UP"); + break; + case 2: + oled_print(oled, "SPEED CONTROLLER"); + break; + case 3: + oled_print(oled, "ZERO TORQUE"); + break; + case 4: + oled_print(oled, "DONE"); + break; + } + + oled_clearline(oled, 1); + oled_set_pos(oled, 1, 0); + + stats->lc_controls.launch_ctrl_slip_ratio = LOOPOVER(stats->lc_controls.launch_ctrl_slip_ratio, 101, 130); + stats->lc_controls.speeding_up_torque = LOOPOVER(stats->lc_controls.speeding_up_torque, 0, 2400); + stats->lc_controls.speeding_up_speed = LOOPOVER(stats->lc_controls.speeding_up_speed, 0, 5000); + stats->lc_controls.ws_thresh = LOOPOVER(stats->lc_controls.ws_thresh, 0, 2500); + + oled_print(oled, "LCTRL "); + + oled_print(oled, (stats->lc_controls.using_launch_ctrl) ? "ON" : "OFF"); + + oled_rprint_pad(oled, "SR ", 4); + + if (stats->lc_controls.launch_ctrl_slip_ratio != 255) { + oled_print_num(oled, stats->lc_controls.launch_ctrl_slip_ratio); + } else { + oled_print(oled, DATA_UNKNOWN); + } + + 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); + } else { + oled_print(oled, DATA_UNKNOWN); + } + + oled_rprint_pad(oled, "SPD ", 4); + if (stats->lc_controls.speeding_up_speed != 65535) { + oled_print_num(oled, stats->lc_controls.speeding_up_speed); + } else { + oled_print(oled, DATA_UNKNOWN); + } + + oled_clearline(oled, 3); + oled_set_pos(oled, 3, 0); + + oled_print(oled, "WS THRESH "); + if (stats->lc_controls.ws_thresh != 65535) { + oled_print_num(oled, stats->lc_controls.ws_thresh); + } else { + oled_print(oled, DATA_UNKNOWN); + } +} + void draw_fault_page(page_manager_t *pm, NHD_US2066_OLED *oled) { carstats_t *stats = pm->stats; diff --git a/src/frontCANnode/src/output.c b/src/frontCANnode/src/output.c index d1af2f5..d26aa77 100644 --- a/src/frontCANnode/src/output.c +++ b/src/frontCANnode/src/output.c @@ -50,6 +50,11 @@ void write_can_left_wheel_speed_msg() { msg.left_32b = input.speed->can_node_left_32b_wheel_speed; msg.left_16b = input.speed->can_node_left_16b_wheel_speed; + /*Serial_Print("32l: "); + Serial_PrintNumber(msg.left_32b, 10); + Serial_Print(", 16l: "); + Serial_PrintlnNumber(msg.left_16b, 10);*/ + handle_can_error(can0_FrontCanNodeLeftWheelSpeed_Write(&msg)); } @@ -61,6 +66,11 @@ void write_can_right_wheel_speed_msg() { msg.right_32b = input.speed->can_node_right_32b_wheel_speed; msg.right_16b = input.speed->can_node_right_16b_wheel_speed; + /*Serial_Print("32r: "); + Serial_PrintNumber(msg.right_32b, 10); + Serial_Print(", 16r: "); + Serial_PrintlnNumber(msg.right_16b, 10);*/ + handle_can_error(can0_FrontCanNodeRightWheelSpeed_Write(&msg)); } void handle_can_error(Can_ErrorID_T error) { diff --git a/src/vcu/inc/controls.h b/src/vcu/inc/controls.h index 45260dd..fd3c3dd 100644 --- a/src/vcu/inc/controls.h +++ b/src/vcu/inc/controls.h @@ -14,6 +14,18 @@ #define MAX_ACCEL_VAL 1000 #define MIN_ACCEL_VAL 0 +// Launch control constants +// After wheel speed crosses this threshold, start using slip controller +#define LC_DEFAULT_WS_THRESH 45 // rpm +#define LC_GR 347 // Gear ratio times 100 +#define LC_ACCEL_BEGIN 950 // 95% +#define LC_ACCEL_RELEASE 50 // 5% +#define LC_BRAKE_BEGIN 150 // We want a lower threshold than normal to make it super easy to exit LC +#define LC_BACKWARDS_CUTOFF 10 +#define LC_DEFAULT_SLIP_RATIO 112 +#define LC_DEFAULT_SPEEDING_UP_TORQUE 1000 +#define LC_DEFAULT_SPEEDING_UP_SPEED 500 + // RG = regen #define RG_MOTOR_SPEED_THRESH 250 // RPM #define RG_CAR_SPEED_THRESH 5 // kph @@ -31,6 +43,14 @@ #define FRONT_BRAKE brake_1 #define REAR_BRAKE brake_2 +typedef enum { + BEFORE, + SPEEDING_UP, + SPEED_CONTROLLER, + ZERO_TORQUE, + DONE, +} Launch_Control_State_T; + // TL = temp limiting #define MAX_TEMP 5800 // centiCelsius @@ -40,11 +60,13 @@ #define TEMP_LOG_LENGTH 200 extern can0_VCUControlsParams_T control_settings; +extern can0_VCUControlsParamsLC_T lc_settings; // INTERACTION FUNCTIONS void enable_controls(void); void disable_controls(void); bool get_controls_enabled(void); void execute_controls(void); +void set_lc_zero_torque(void); #endif // ifndef __TORQUE_CALC diff --git a/src/vcu/src/can_handles.c b/src/vcu/src/can_handles.c index 7b5af31..4b054ca 100644 --- a/src/vcu/src/can_handles.c +++ b/src/vcu/src/can_handles.c @@ -93,6 +93,9 @@ void handleCAN(CAN_HandleTypeDef *hcan) { case can0_DashRequest: handleDashRequest(&frame); break; + + case can0_DashRequestLC: + handleDashRequestLC(&frame); default: break; @@ -293,6 +296,37 @@ void handleDashRequest(Frame *msg) { control_settings.active_aero_enabled = unpacked_msg.active_aero_enabled; } +void handleDashRequestLC(Frame *msg) { + can0_DashRequestLC_T unpacked_msg; + + unpack_can0_DashRequestLC(msg, &unpacked_msg); + + // printf("speeding_up_torque: %d\r\n", unpacked_msg.speeding_up_torque); + // printf("speeding_up_speed: %d\r\n", unpacked_msg.speeding_up_speed); + // printf("ws_thresh: %d\r\n", unpacked_msg.ws_thresh); + // printf("launch_ctrl_slip_ratio: %d\r\n", unpacked_msg.launch_ctrl_slip_ratio); + // printf("using_launch_ctrl: %d\r\n", unpacked_msg.using_launch_ctrl); + + lc_settings.using_launch_ctrl = unpacked_msg.using_launch_ctrl; + + + if (unpacked_msg.speeding_up_torque != 65535) { + lc_settings.speeding_up_torque = unpacked_msg.speeding_up_torque; + } + + if (unpacked_msg.speeding_up_speed != 65535) { + lc_settings.speeding_up_speed = unpacked_msg.speeding_up_speed; + } + + if (unpacked_msg.ws_thresh != 65535) { + lc_settings.ws_thresh = unpacked_msg.ws_thresh; + } + + if (unpacked_msg.launch_ctrl_slip_ratio != 255) { + lc_settings.launch_ctrl_slip_ratio = unpacked_msg.launch_ctrl_slip_ratio; + } +} + void send_VCUHeartbeat(void) { LIMIT(can0_VCUHeartbeat); @@ -334,6 +368,13 @@ void send_VCUControlsParams(void) { can0_VCUControlsParams_Write(&control_settings); } +void send_VCUControlsParamsLC(void) { + LIMIT(can0_VCUControlsParamsLC); + lc_settings.lc_state = get_lc_state(); + + can0_VCUControlsParamsLC_Write(&lc_settings); +} + void send_VCUControlsMonitoring(void) { LIMIT(can0_VCUControlsMonitoring); @@ -344,6 +385,7 @@ void send_VCU(void) { send_VCUHeartbeat(); send_VCUErrors(); send_VCUControlsParams(); + send_VCUControlsParamsLC(); send_VCUControlsMonitoring(); } diff --git a/src/vcu/src/controls.c b/src/vcu/src/controls.c index 1fd7557..ab9e67c 100644 --- a/src/vcu/src/controls.c +++ b/src/vcu/src/controls.c @@ -4,12 +4,18 @@ static bool enabled = false; static int32_t torque_command = 0; static int32_t speed_command = 0; can0_VCUControlsParams_T control_settings = {}; +can0_VCUControlsParamsLC_T lc_settings = {}; +static Launch_Control_State_T lc_state = BEFORE; + +uint32_t get_front_wheel_speed(void); +static bool any_lc_faults(); static int32_t hinge_limiter(int32_t x, int32_t m, int32_t e, int32_t c); // PRIVATE FUNCTIONS static int32_t get_torque(void); static int32_t get_regen_torque(void); +static int32_t get_launch_control_speed(uint32_t front_wheel_speed); static int32_t get_temp_limited_torque(int32_t pedal_torque); static int32_t get_voltage_limited_torque(int32_t pedal_torque); @@ -22,12 +28,19 @@ void init_controls_defaults(void) { control_settings.volt_lim_min_gain = 0; control_settings.volt_lim_min_voltage = 300; control_settings.torque_temp_limited = false; + + lc_settings.using_launch_ctrl = false; + lc_settings.launch_ctrl_slip_ratio = LC_DEFAULT_SLIP_RATIO; + 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; } void enable_controls(void) { enabled = true; torque_command = 0; speed_command = 0; + lc_state = BEFORE; unlock_brake_valve(); } @@ -36,6 +49,7 @@ void disable_controls(void) { enabled = false; torque_command = 0; speed_command = 0; + lc_state = DONE; set_brake_valve(false); lock_brake_valve(); @@ -47,6 +61,7 @@ bool get_controls_enabled(void) { void execute_controls(void) { if (!enabled) return; + bool launch_ctrl_entered = false; torque_command = get_torque(); controls_monitoring.raw_torque = torque_command; @@ -69,34 +84,26 @@ void execute_controls(void) { // Extra check to ensure we are only sending regen torque when allowed if (torque_command == 0) { torque_command = regen_torque; - } - else { - // Only use limits when we're not doing regen + } else { // Only use limits and launch control when we're not doing regen + // Even if we are doing launch control, calculate limits for logging also so that we are not doing + // launch control when we're supposed to be limiting + int32_t voltage_limited_torque = get_voltage_limited_torque(torque_command); - // static uint32_t last_vt = 0; - // if (HAL_GetTick() - last_vt > 10) { - // printf("VT: %d\r\n", voltage_limited_torque); - // - // last_vt = HAL_GetTick(); - // } + if (!control_settings.using_voltage_limiting) voltage_limited_torque = torque_command; int32_t temp_limited_torque = get_temp_limited_torque(torque_command); - // static uint32_t last_tt = 0; - // if (HAL_GetTick() - last_tt > 10) { - // printf("TT: %d\r\n", temp_limited_torque); - // - // last_tt = HAL_GetTick(); - // } + if (!control_settings.using_temp_limiting) temp_limited_torque = torque_command; - control_settings.torque_temp_limited = temp_limited_torque < torque_command; + uint32_t torque_temp_limited = temp_limited_torque < torque_command; + // Whether torque is temp limited needs to be sent out over CAN so the dash knows not to allow regen + control_settings.torque_temp_limited = torque_temp_limited; int32_t min_sensor_torque; if (voltage_limited_torque < temp_limited_torque) { min_sensor_torque = voltage_limited_torque; - } - else { + } else { min_sensor_torque = temp_limited_torque; } @@ -105,15 +112,57 @@ void execute_controls(void) { int32_t limited_torque; if (dash_limited_torque < min_sensor_torque) { limited_torque = dash_limited_torque; - } - else { + } else { limited_torque = min_sensor_torque; } torque_command = limited_torque; + bool torque_limited = limited_torque < controls_monitoring.raw_torque; + + if (launch_ctrl_entered = (lc_settings.using_launch_ctrl && lc_state != DONE && !torque_limited)) { + // We shouldn't be braking in launch control, so reset regen valve + set_brake_valve(false); + uint32_t front_wheel_speed = get_front_wheel_speed(); + + // Mini FSM + if (any_lc_faults() && lc_state != DONE && lc_state != BEFORE) { + lc_state = ZERO_TORQUE; + printf("[LAUNCH CONTROL] ZERO TORQUE STATE ENTERED\r\n"); + } + switch (lc_state) { + case BEFORE: + sendTorqueCmdMsg(0); + + // Transition + if (pedalbox_min(accel) > LC_ACCEL_BEGIN) { + lc_state = SPEED_CONTROLLER; + printf("[LAUNCH CONTROL] SPEED CONTROLLER STATE ENTERED\r\n"); + } + break; + case SPEED_CONTROLLER: + speed_command = 500; // get_launch_control_speed(front_wheel_speed); + sendSpeedCmdMsg(speed_command, torque_command); + break; + case ZERO_TORQUE: + sendTorqueCmdMsg(0); + + if (pedalbox_max(accel) < LC_ACCEL_RELEASE) { + lc_state = DONE; + printf("[LAUNCH CONTROL] DONE STATE ENTERED\r\n"); + } + break; + default: + sendTorqueCmdMsg(0); + + printf("ERROR: NO STATE!\r\n"); + break; + } + } + } + if (!launch_ctrl_entered) { + // In LC we already sent our command above + sendTorqueCmdMsg(torque_command); } - - sendTorqueCmdMsg(torque_command); } static int32_t get_torque(void) { @@ -150,10 +199,68 @@ static int32_t get_regen_torque() { } } - // Regen is negative torque, and we've calculated a positive number so far return -1 * regen_torque; } +static int32_t get_launch_control_speed(uint32_t front_wheel_speed) { +uint32_t front_wheel_speedRPM = front_wheel_speed / 1000; + // Divide 100 because slip ratio is times 100, divide by 100 again because + // gear ratio is also multiplied by 100 + int32_t target_speed = front_wheel_speedRPM * lc_settings.launch_ctrl_slip_ratio * LC_GR / (100 * 100); + return target_speed; +} + +uint32_t get_front_wheel_speed() { + uint32_t left_front_speed; + uint32_t right_front_speed; + + // If the 32 bit wheel speed is zero, then it's disconnect (in which case we should + // use the 16 bit one), or the wheels are actually not moving, so the 16 bit one will + // be zero as well + if (wheel_speeds.front_left_32b_wheel_speed == 0) { + left_front_speed = wheel_speeds.front_left_16b_wheel_speed; + } else { + left_front_speed = wheel_speeds.front_left_32b_wheel_speed; + } + + if (wheel_speeds.front_right_32b_wheel_speed == 0) { + right_front_speed = wheel_speeds.front_right_16b_wheel_speed; + } else { + right_front_speed = wheel_speeds.front_right_32b_wheel_speed; + } + + uint32_t avg_wheel_speed = left_front_speed/2 + right_front_speed/2; + return left_front_speed; +} + +static bool any_lc_faults(void) { + if (pedalbox_min(accel) < LC_ACCEL_BEGIN) { + printf("[LAUNCH CONTROL ERROR] Accel min (%d) too low\r\n", pedalbox_min(accel)); + return true; + } else if (pedalbox.brake_2 > LC_BRAKE_BEGIN) { + // In this test, we only need to check brake_2 because it is the only brake line + // 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; + }*/ + return false; +} + +void set_lc_state_before(void) { + lc_state = BEFORE; +} + +void set_lc_zero_torque(void) { + lc_state = ZERO_TORQUE; +} + +Launch_Control_State_T get_lc_state() { + return lc_state; +} + static int32_t get_temp_limited_torque(int32_t pedal_torque) { uint32_t temp_sum = 0; for (uint32_t i = 0; i < TEMP_LOG_LENGTH; i++) { @@ -167,13 +274,6 @@ static int32_t get_temp_limited_torque(int32_t pedal_torque) { // Thresh was in degrees, so multiply it by 100 int32_t thresh_cC = control_settings.temp_lim_thresh_temp * 100; - // static uint32_t lastt = 0; - // if (HAL_GetTick() - lastt > 100) { - // printf("Filtered Temp: %d\tTemp threshold: %d\r\n", temp_cC, thresh_cC); - // - // lastt = HAL_GetTick(); - // } - int32_t gain = hinge_limiter(temp_cC, control_settings.temp_lim_min_gain, thresh_cC, MAX_TEMP); controls_monitoring.tl_gain = gain; @@ -187,13 +287,6 @@ static int32_t get_voltage_limited_torque(int32_t pedal_torque) { int32_t cell_voltage = (cs_readings.V_bus - 72) / 720; controls_monitoring.voltage_used = cell_voltage; - // static uint32_t lastt = 0; - // if (HAL_GetTick() - lastt > 100) { - // printf("Voltage: %d\tVoltage threshold: %d\r\n", cell_voltage, control_settings.volt_lim_min_voltage); - // - // lastt = HAL_GetTick(); - // } - int32_t gain = hinge_limiter(cell_voltage, control_settings.volt_lim_min_gain, control_settings.volt_lim_min_voltage, MIN_VOLTAGE); controls_monitoring.vl_gain = gain; diff --git a/src/vcu/src/main.c b/src/vcu/src/main.c index ca714dd..73149cb 100755 --- a/src/vcu/src/main.c +++ b/src/vcu/src/main.c @@ -75,6 +75,7 @@ int main(void) { HAL_GPIO_WritePin(GPIO(ACTIVE_AERO), control_settings.active_aero_enabled); static uint32_t lastt = 0; + static uint32_t lastt2 = 0; print_gate_faults(false); if (HAL_GetTick() - lastt > 100) { @@ -84,12 +85,31 @@ int main(void) { send_mc_fault_clear(); } - // printf("CONTROLS PARAMS:\r\n using_regen: %d\r\n using_voltage_limiting: %d\r\n using_temp_limiting: %d\r\n regen_bias: %d\r\n limp_factor: %d\r\n temp_lim_min_gain: %d\r\n temp_lim_thresh_temp: %d\r\n temp_abs_thesh: %d\r\n volt_lim_min_gain: %d\r\n volt_lim_min_voltage: %d\r\n volt_abs_thresh: %d\r\n\r\n", - // control_settings.using_regen, control_settings.using_voltage_limiting, control_settings.using_temp_limiting, control_settings.regen_bias, control_settings.limp_factor, control_settings.temp_lim_min_gain, control_settings.temp_lim_thresh_temp, MAX_TEMP, control_settings.volt_lim_min_gain, control_settings.volt_lim_min_voltage, MIN_VOLTAGE); - - lastt = HAL_GetTick(); } + if (HAL_GetTick() - lastt2 > 100) { + // 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()) { + // case BEFORE: + // printf("BEFORE\r\n"); + // break; + // case SPEEDING_UP: + // printf("SPEEDING_UP\r\n"); + // break; + // case SPEED_CONTROLLER: + // printf("SPEED_CONTROLLER\r\n"); + // break; + // case ZERO_TORQUE: + // printf("SPEEDING_UP\r\n"); + // break; + // case DONE: + // printf("DONE\r\n"); + // break; + // } + // printf("\r\n"); + lastt2 = HAL_GetTick(); + } } } diff --git a/src/vcu/src/state_error.c b/src/vcu/src/state_error.c index 302d1ac..9840026 100644 --- a/src/vcu/src/state_error.c +++ b/src/vcu/src/state_error.c @@ -59,6 +59,7 @@ void update_no_error_state(void) { } void enter_recoverable_error_state(void) { + set_lc_zero_torque(); printf("[ERROR FSM : RECOVERABLE_ERROR_STATE] ENTERED!\r\n"); } @@ -89,6 +90,7 @@ void update_recoverable_error_state(void) { void enter_fatal_error_state(void) { printf("[ERROR FSM : FATAL_ERROR_STATE] ENTERED!\r\n"); + set_lc_zero_torque(); } void update_fatal_error_state(void) {