Apply suggestion from oral review

Move algorithm into another function
leka · Feb 1, 2024 · 2146a83 · 2146a83
1 parent 558f6da
commit 2146a83
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 31 deletions.
diff --git 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
@@ -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
@@ -62,75 +62,75 @@ 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};
 
 	EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles));
 	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)