#include "TorqueControllerMux.h"
#include "TorqueControllers.h"
#include "fake_controller_type.h"

Include dependency graph for launch_controller_integration_testing.h:

This graph shows which files directly or indirectly include this file:

Functions
constexpr DrivetrainDynamicReport_s	create_drive_report (float speed_fl, float speed_fr, float speed_rl, float speed_rr)

constexpr PedalsSystemData_s	create_pedals_data (float accel_percent, float brake_percent)

	TEST (LaunchIntergationTesting, test_simple_launch_controller)

	TEST (LaunchIntergationTesting, test_slip_launch_controller)

Variables
constexpr float	slow_speed = (TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM

constexpr auto	simulated_slow_drivetrain_dynamics = create_drive_report(slow_speed, slow_speed, slow_speed, slow_speed)

constexpr auto	simulated_barely_launch_drivetrain_dynamics = create_drive_report(0, 2785.86, 0, 0)

constexpr auto	simulated_no_launch_drivetrain_dynamics = create_drive_report(0, 2786.86, 0, 0)

constexpr float	fast_speed = (TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM

constexpr auto	simulated_fast_drivetrain_dynamics = create_drive_report(fast_speed, fast_speed, fast_speed, fast_speed)

constexpr auto	simulated_full_accel_press = create_pedals_data(1.0, 0)

constexpr auto	simulated_no_accel_press = create_pedals_data(0, 0)

constexpr auto	simulated_accel_and_brake_press = create_pedals_data(1.0, 0.3)

Function Documentation

◆ create_drive_report()

constexpr DrivetrainDynamicReport_s create_drive_report	(	float	speed_fl,
		float	speed_fr,
		float	speed_rl,
		float	speed_rr
	)

constexpr

Definition at line 7 of file launch_controller_integration_testing.h.

{
    DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = {
        .measuredInverterFLPackVoltage = 550,
        .measuredSpeeds = {
            speed_fl,
            speed_fr,
            speed_rl,
            speed_rr},
        .measuredTorques = {0.0, 0.0, 0.0, 0.0},
        .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0},
        .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0}};
    return simulated_slow_drivetrain_dynamics;
}

◆ create_pedals_data()

constexpr PedalsSystemData_s create_pedals_data	(	float	accel_percent,
		float	brake_percent
	)

constexpr

Definition at line 22 of file launch_controller_integration_testing.h.

{
    PedalsSystemData_s pedals = {
        .accelImplausible = false,
        .brakeImplausible = false,
        .brakePressed = false,
        .brakeAndAccelPressedImplausibility = false,
        .implausibilityExceededMaxDuration = false,
        .accelPercent = accel_percent,
        .brakePercent = brake_percent};
    return pedals;
}

◆ TEST() [1/2]

TEST	(	LaunchIntergationTesting	,
		test_simple_launch_controller
	)

Definition at line 45 of file launch_controller_integration_testing.h.

{
 
    SysClock clock = SysClock();
    SysTick_s cur_tick;
    cur_tick = clock.tick(0);
    SharedCarState_s still_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_no_accel_press, {}, {}, {});
    SharedCarState_s pedal_pressed_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
    SharedCarState_s no_launch_allowed_state(cur_tick, {}, simulated_no_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
    SharedCarState_s barely_launch_state(cur_tick, {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
    SharedCarState_s one_sec_passed_in_launch_state(clock.tick(1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
    SharedCarState_s one_sec_passed_in_launch_state_w_error(clock.tick(1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_accel_and_brake_press, {}, {}, {});
 
    // mode 0
    TorqueControllerSimple tc_simple(1.0f, 1.0f);
    // mode 1
    TorqueControllerLoadCellVectoring tc_vec;
    // mode 2
    DummyQueue_s q;
    CASESystem<DummyQueue_s> case_sys(&q, 100, 70, 550, {});
    TorqueControllerCASEWrapper<DummyQueue_s> case_wrapper(&case_sys);
 
    // mode 3
    TorqueControllerSimpleLaunch simple_launch;
    // mode 4
    TorqueControllerSlipLaunch slip_launch;
 
    TorqueControllerMux<5>
        torque_controller_mux({static_cast<Controller *>(&tc_simple),
                               static_cast<Controller *>(&tc_vec),
                               static_cast<Controller *>(&case_wrapper),
                               static_cast<Controller *>(&simple_launch),
                               static_cast<Controller *>(&slip_launch)},
                              {false, false, true, false, false});
 
    auto res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
    // tick again to calculate state switch
 
    // tick again to calculate state switch
    ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY);
 
    // press accelerator but with one wheel right at max speed threshold
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, no_launch_allowed_state);
    ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_NOT_READY);
 
    // // go back to launch ready state
 
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
    ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY);
 
    // press accel now with one wheelspeed barely under threshold
 
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, barely_launch_state);
    ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCHING);
 
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, one_sec_passed_in_launch_state);
    ASSERT_NEAR((float)((int)((res.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f, 0.02);
 
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, one_sec_passed_in_launch_state_w_error);
    ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_NOT_READY);
}

◆ TEST() [2/2]

TEST	(	LaunchIntergationTesting	,
		test_slip_launch_controller
	)

Definition at line 109 of file launch_controller_integration_testing.h.

{
    SysClock clock = SysClock();
    SysTick_s cur_tick;
    cur_tick = clock.tick(0);
 
    VectornavData_s vn_data{};
    // mode 0
    TorqueControllerSimple tc_simple(1.0f, 1.0f);
    // mode 1
    TorqueControllerLoadCellVectoring tc_vec;
    // mode 2
    DummyQueue_s q;
    CASESystem<DummyQueue_s> case_sys(&q, 100, 70, 550, {});
    TorqueControllerCASEWrapper<DummyQueue_s> case_wrapper(&case_sys);
 
    // mode 3
    TorqueControllerSimpleLaunch simple_launch;
    // mode 4
    TorqueControllerSlipLaunch slip_launch;
 
    TorqueControllerMux<5>
        torque_controller_mux({static_cast<Controller *>(&tc_simple),
                               static_cast<Controller *>(&tc_vec),
                               static_cast<Controller *>(&case_wrapper),
                               static_cast<Controller *>(&simple_launch),
                               static_cast<Controller *>(&slip_launch)},
                              {false, false, true, false, false});
 
    SharedCarState_s still_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_no_accel_press, {}, {}, {});
    SharedCarState_s pedal_pressed_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
    SharedCarState_s no_launch_allowed_state(cur_tick, {}, simulated_no_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
    SharedCarState_s barely_launch_state(cur_tick, {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
 
    auto res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
    // getting to slip launch (mode 4)
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
    ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY);
 
    SharedCarState_s one_hundredth_sec_passed_in_launch_state(clock.tick(10000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, one_hundredth_sec_passed_in_launch_state);
 
    SharedCarState_s two_hundredth_sec_passed_in_launch_state(clock.tick(20000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, two_hundredth_sec_passed_in_launch_state);
 
    ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING);
    ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET);
 
    SharedCarState_s more_accel_time_since_launch(clock.tick( BaseLaunchControllerParams::const_accel_time * 1000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, more_accel_time_since_launch);
 
    ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING);
    ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET);
 
    //     // if velocity is less than the default speed, it should still go at launch speed
    vn_data.velocity_x = 0; // m/s
    SharedCarState_s small_vn_vel(clock.tick(( BaseLaunchControllerParams::const_accel_time * 1000) + 1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, vn_data, {}, {});
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, small_vn_vel);
    printf("lower vx_body: %.2f\n", (float)res.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND);
    ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET); // should still be at initial launch target
 
    ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING);
    //     // this is approx. the speed the car would be going at 1500 rpm
    //     // we should expect the next calculation to be approx. DEFAULT_SLIP_RATIO % higher than this
    vn_data.velocity_x = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND; // m/s
 
    SharedCarState_s big_vn_vel(clock.tick(( BaseLaunchControllerParams::const_accel_time * 1000) + 2000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, vn_data, {}, {});
    res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, big_vn_vel);
 
    ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING);
    printf("launch vx_body: %.2f\n", (float)res.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND);
    ASSERT_EQ((int16_t)res.speeds_rpm[0] > (BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND), true);
 
    // TODO this is actually failing, idk what is going on here @walker
    // ASSERT_NEAR((float)((int)((res.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f, 0.02);
}