MCU
Loading...
Searching...
No Matches
launch_controller_integration_testing.h
Go to the documentation of this file.
1// TODO put the launch controller testing in here with the tc mux
2
3#include "TorqueControllerMux.h"
4#include "TorqueControllers.h"
5#include "fake_controller_type.h"
6
7constexpr DrivetrainDynamicReport_s create_drive_report(float speed_fl, float speed_fr, float speed_rl, float speed_rr)
8{
9 DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = {
10 .measuredInverterFLPackVoltage = 550,
11 .measuredSpeeds = {
12 speed_fl,
13 speed_fr,
14 speed_rl,
15 speed_rr},
16 .measuredTorques = {0.0, 0.0, 0.0, 0.0},
17 .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0},
18 .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0}};
19 return simulated_slow_drivetrain_dynamics;
20}
21
22constexpr PedalsSystemData_s create_pedals_data(float accel_percent, float brake_percent)
23{
24 PedalsSystemData_s pedals = {
25 .accelImplausible = false,
26 .brakeImplausible = false,
27 .brakePressed = false,
28 .brakeAndAccelPressedImplausibility = false,
29 .implausibilityExceededMaxDuration = false,
30 .accelPercent = accel_percent,
31 .brakePercent = brake_percent};
32 return pedals;
33}
34
35constexpr float slow_speed = (TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM;
36constexpr auto simulated_slow_drivetrain_dynamics = create_drive_report(slow_speed, slow_speed, slow_speed, slow_speed);
37constexpr auto simulated_barely_launch_drivetrain_dynamics = create_drive_report(0, 2785.86, 0, 0);
38constexpr auto simulated_no_launch_drivetrain_dynamics = create_drive_report(0, 2786.86, 0, 0);
39constexpr float fast_speed = (TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM;
40constexpr auto simulated_fast_drivetrain_dynamics = create_drive_report(fast_speed, fast_speed, fast_speed, fast_speed);
41constexpr auto simulated_full_accel_press = create_pedals_data(1.0, 0);
42constexpr auto simulated_no_accel_press = create_pedals_data(0, 0);
43constexpr auto simulated_accel_and_brake_press = create_pedals_data(1.0, 0.3);
44
45TEST(LaunchIntergationTesting, test_simple_launch_controller)
46{
47
48 SysClock clock = SysClock();
49 SysTick_s cur_tick;
50 cur_tick = clock.tick(0);
51 SharedCarState_s still_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_no_accel_press, {}, {}, {});
52 SharedCarState_s pedal_pressed_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
53 SharedCarState_s no_launch_allowed_state(cur_tick, {}, simulated_no_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
54 SharedCarState_s barely_launch_state(cur_tick, {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
55 SharedCarState_s one_sec_passed_in_launch_state(clock.tick(1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
56 SharedCarState_s one_sec_passed_in_launch_state_w_error(clock.tick(1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_accel_and_brake_press, {}, {}, {});
57
58 // mode 0
59 TorqueControllerSimple tc_simple(1.0f, 1.0f);
60 // mode 1
61 TorqueControllerLoadCellVectoring tc_vec;
62 // mode 2
63 DummyQueue_s q;
64 CASESystem<DummyQueue_s> case_sys(&q, 100, 70, 550, {});
65 TorqueControllerCASEWrapper<DummyQueue_s> case_wrapper(&case_sys);
66
67 // mode 3
68 TorqueControllerSimpleLaunch simple_launch;
69 // mode 4
70 TorqueControllerSlipLaunch slip_launch;
71
72 TorqueControllerMux<5>
73 torque_controller_mux({static_cast<Controller *>(&tc_simple),
74 static_cast<Controller *>(&tc_vec),
75 static_cast<Controller *>(&case_wrapper),
76 static_cast<Controller *>(&simple_launch),
77 static_cast<Controller *>(&slip_launch)},
78 {false, false, true, false, false});
79
80 auto res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
81 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
82 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
83 // tick again to calculate state switch
84
85 // tick again to calculate state switch
86 ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY);
87
88 // press accelerator but with one wheel right at max speed threshold
89 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, no_launch_allowed_state);
90 ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_NOT_READY);
91
92 // // go back to launch ready state
93
94 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
95 ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY);
96
97 // press accel now with one wheelspeed barely under threshold
98
99 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, barely_launch_state);
100 ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCHING);
101
102 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, one_sec_passed_in_launch_state);
103 ASSERT_NEAR((float)((int)((res.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f, 0.02);
104
105 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, one_sec_passed_in_launch_state_w_error);
106 ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_NOT_READY);
107}
108
109TEST(LaunchIntergationTesting, test_slip_launch_controller)
110{
111 SysClock clock = SysClock();
112 SysTick_s cur_tick;
113 cur_tick = clock.tick(0);
114
115 VectornavData_s vn_data{};
116 // mode 0
117 TorqueControllerSimple tc_simple(1.0f, 1.0f);
118 // mode 1
119 TorqueControllerLoadCellVectoring tc_vec;
120 // mode 2
121 DummyQueue_s q;
122 CASESystem<DummyQueue_s> case_sys(&q, 100, 70, 550, {});
123 TorqueControllerCASEWrapper<DummyQueue_s> case_wrapper(&case_sys);
124
125 // mode 3
126 TorqueControllerSimpleLaunch simple_launch;
127 // mode 4
128 TorqueControllerSlipLaunch slip_launch;
129
130 TorqueControllerMux<5>
131 torque_controller_mux({static_cast<Controller *>(&tc_simple),
132 static_cast<Controller *>(&tc_vec),
133 static_cast<Controller *>(&case_wrapper),
134 static_cast<Controller *>(&simple_launch),
135 static_cast<Controller *>(&slip_launch)},
136 {false, false, true, false, false});
137
138 SharedCarState_s still_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_no_accel_press, {}, {}, {});
139 SharedCarState_s pedal_pressed_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
140 SharedCarState_s no_launch_allowed_state(cur_tick, {}, simulated_no_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
141 SharedCarState_s barely_launch_state(cur_tick, {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
142
143 auto res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
144 // getting to slip launch (mode 4)
145 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state);
146 ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY);
147
148 SharedCarState_s one_hundredth_sec_passed_in_launch_state(clock.tick(10000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
149 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, one_hundredth_sec_passed_in_launch_state);
150
151 SharedCarState_s two_hundredth_sec_passed_in_launch_state(clock.tick(20000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
152 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, two_hundredth_sec_passed_in_launch_state);
153
154 ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING);
155 ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET);
156
157 SharedCarState_s more_accel_time_since_launch(clock.tick( BaseLaunchControllerParams::const_accel_time * 1000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}, {}, {});
158 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, more_accel_time_since_launch);
159
160 ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING);
161 ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET);
162
163 // // if velocity is less than the default speed, it should still go at launch speed
164 vn_data.velocity_x = 0; // m/s
165 SharedCarState_s small_vn_vel(clock.tick(( BaseLaunchControllerParams::const_accel_time * 1000) + 1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, vn_data, {}, {});
166 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, small_vn_vel);
167 printf("lower vx_body: %.2f\n", (float)res.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND);
168 ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET); // should still be at initial launch target
169
170 ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING);
171 // // this is approx. the speed the car would be going at 1500 rpm
172 // // we should expect the next calculation to be approx. DEFAULT_SLIP_RATIO % higher than this
173 vn_data.velocity_x = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND; // m/s
174
175 SharedCarState_s big_vn_vel(clock.tick(( BaseLaunchControllerParams::const_accel_time * 1000) + 2000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, vn_data, {}, {});
176 res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, big_vn_vel);
177
178 ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING);
179 printf("launch vx_body: %.2f\n", (float)res.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND);
180 ASSERT_EQ((int16_t)res.speeds_rpm[0] > (BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND), true);
181
182 // TODO this is actually failing, idk what is going on here @walker
183 // ASSERT_NEAR((float)((int)((res.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f, 0.02);
184}
RPM_TO_METERS_PER_SECOND
constexpr const float RPM_TO_METERS_PER_SECOND
Definition: PhysicalParameters.h:13
METERS_PER_SECOND_TO_RPM
constexpr const float METERS_PER_SECOND_TO_RPM
Definition: PhysicalParameters.h:15
BaseLaunchController::get_launch_state
LaunchStates_e get_launch_state()
Definition: BaseLaunchController.h:70
CASESystem
this class with both take in sensor inputs as well as handle calculations for various derived states ...
Definition: CASESystem.h:97
Controller
Base class for all controllers, which define drivetrain command containing different variations of
Definition: BaseController.h:18
TorqueControllerCASEWrapper
makes CASE system a part of Controller hierarchy for use in TC Mux
Definition: CASEControllerWrapper.h:10
TorqueControllerMux::getDrivetrainCommand
DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, TorqueLimit_e controller_command_torque_limit, const SharedCarState_s &input_state)
function that evaluates the mux, controllers and gets the active command
fake_controller_type.h
TEST
TEST(LaunchIntergationTesting, test_simple_launch_controller)
Definition: launch_controller_integration_testing.h:45
create_drive_report
constexpr DrivetrainDynamicReport_s create_drive_report(float speed_fl, float speed_fr, float speed_rl, float speed_rr)
Definition: launch_controller_integration_testing.h:7
simulated_fast_drivetrain_dynamics
constexpr auto simulated_fast_drivetrain_dynamics
Definition: launch_controller_integration_testing.h:40
slow_speed
constexpr float slow_speed
Definition: launch_controller_integration_testing.h:35
simulated_slow_drivetrain_dynamics
constexpr auto simulated_slow_drivetrain_dynamics
Definition: launch_controller_integration_testing.h:36
simulated_barely_launch_drivetrain_dynamics
constexpr auto simulated_barely_launch_drivetrain_dynamics
Definition: launch_controller_integration_testing.h:37
fast_speed
constexpr float fast_speed
Definition: launch_controller_integration_testing.h:39
simulated_full_accel_press
constexpr auto simulated_full_accel_press
Definition: launch_controller_integration_testing.h:41
simulated_no_accel_press
constexpr auto simulated_no_accel_press
Definition: launch_controller_integration_testing.h:42
simulated_accel_and_brake_press
constexpr auto simulated_accel_and_brake_press
Definition: launch_controller_integration_testing.h:43
simulated_no_launch_drivetrain_dynamics
constexpr auto simulated_no_launch_drivetrain_dynamics
Definition: launch_controller_integration_testing.h:38
create_pedals_data
constexpr PedalsSystemData_s create_pedals_data(float accel_percent, float brake_percent)
Definition: launch_controller_integration_testing.h:22
TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE
constexpr const float MAX_SPEED_FOR_MODE_CHANGE
Definition: TorqueControllerMux.h:54
tc_simple
TorqueControllerSimple tc_simple(1.0f, 1.0f)
torque_controller_mux
TCMuxType 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 * >(&db_controller)}, {false, false, true, false, true})
tc_vec
TorqueControllerLoadCellVectoring tc_vec
Definition: main.cpp:308
simple_launch
TorqueControllerSimpleLaunch simple_launch
Definition: main.cpp:313
DrivetrainDynamicReport_s
Definition: SharedDataTypes.h:50
DummyQueue_s
Definition: fake_controller_type.h:5
PedalsSystemData_s
Definition: SharedDataTypes.h:37
PedalsSystemData_s::accelImplausible
bool accelImplausible
Definition: SharedDataTypes.h:38
SharedCarState_s
car state struct that contains state of everything about the car including
Definition: SharedDataTypes.h:149
VectornavData_s
Definition: SharedDataTypes.h:77
Generated by doxygen 1.9.5