diff --git a/libs/BehaviorKit/include/behaviors/AutochargeSeal.h b/libs/BehaviorKit/include/behaviors/AutochargeSeal.h index e74740637c..aa07b99a87 100644 --- a/libs/BehaviorKit/include/behaviors/AutochargeSeal.h +++ b/libs/BehaviorKit/include/behaviors/AutochargeSeal.h @@ -22,6 +22,7 @@ class AutochargeSeal : public interface::Behavior auto id() -> BehaviorID final; void run() final; + void loop(); void stop() final; private: diff --git a/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp b/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp index a3a398f39a..91f658074a 100644 --- a/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp +++ b/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp @@ -27,26 +27,31 @@ void AutochargeSeal::run() must_stop = true; // TODO: Update for UT do { - auto angles = _imukit.getEulerAngles(); - auto is_right_tilted = angles.roll > 0; - auto should_move_forward = angles.pitch > 0; - - auto abs_float = [](float value) { return value > 0 ? value : -value; }; - - if (abs_float(angles.roll) > kRollTolerance) { - auto speed_offset = convertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll); - spinToFixRoll(is_right_tilted, should_move_forward, speed_offset); - } else if (abs_float(angles.pitch) > kPitchTolerance) { - auto speed = convertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch); - moveToFixPitch(should_move_forward, speed); - } else { - stopMotors(); - } - - rtos::ThisThread::sleep_for(10ms); + loop(); } while (!must_stop); } +void AutochargeSeal::loop() +{ + auto angles = _imukit.getEulerAngles(); + auto is_right_tilted = angles.roll > 0; + auto should_move_forward = angles.pitch > 0; + + auto abs_float = [](float value) { return value > 0 ? value : -value; }; + + if (abs_float(angles.roll) > kRollTolerance) { + auto speed_offset = convertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll); + spinToFixRoll(is_right_tilted, should_move_forward, speed_offset); + } else if (abs_float(angles.pitch) > kPitchTolerance) { + auto speed = convertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch); + moveToFixPitch(should_move_forward, speed); + } else { + stopMotors(); + } + + rtos::ThisThread::sleep_for(10ms); +} + void AutochargeSeal::stop() { must_stop = true; diff --git a/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp b/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp index d8393dd2a8..7191057550 100644 --- a/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp +++ b/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp @@ -62,7 +62,7 @@ TEST_F(AutochargeSealTest, id) ASSERT_EQ(autocharge_seal.id(), behavior_id::autocharge_seal); } -TEST_F(AutochargeSealTest, runNothingToFix) +TEST_F(AutochargeSealTest, loopNothingToFix) { auto angles = EulerAngles {0, 0, 0}; @@ -70,67 +70,67 @@ TEST_F(AutochargeSealTest, runNothingToFix) EXPECT_CALL(mock_motor_left, stop); EXPECT_CALL(mock_motor_right, stop); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixPositivePitch) +TEST_F(AutochargeSealTest, loopFixPositivePitch) { auto angles = EulerAngles {10, 0, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsMoveForward(); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixNegativePitch) +TEST_F(AutochargeSealTest, loopFixNegativePitch) { auto angles = EulerAngles {-10, 0, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsMoveBackward(); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixPositiveRollPositivePitch) +TEST_F(AutochargeSealTest, loopFixPositiveRollPositivePitch) { auto angles = EulerAngles {10, 10, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsSpinRight(); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixPositiveRollNegativePitch) +TEST_F(AutochargeSealTest, loopFixPositiveRollNegativePitch) { auto angles = EulerAngles {-10, 10, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsSpinLeft(); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixNegativeRollPositivePitch) +TEST_F(AutochargeSealTest, loopFixNegativeRollPositivePitch) { auto angles = EulerAngles {10, -10, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsSpinLeft(); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixNegativeRollNegativePitch) +TEST_F(AutochargeSealTest, loopFixNegativeRollNegativePitch) { auto angles = EulerAngles {-10, -10, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsSpinRight(); - autocharge_seal.run(); + autocharge_seal.loop(); } TEST_F(AutochargeSealTest, stop)