Apply suggestion from review

leka · Feb 1, 2024 · c1cc2f0 · c1cc2f0
1 parent da5d3fd
commit c1cc2f0
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 18 deletions.
diff --git a/spikes/lk_auto_charge/SealStrategy.cpp b/spikes/lk_auto_charge/SealStrategy.cpp
@@ -53,26 +53,10 @@ void SealStrategy::run()
 
 	if (abs_float(angles.roll) > kRollTolerance) {
 		auto speed_offset = convertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll);
-		if (is_right_tilted) {
-			if (should_move_forward) {
-				spinRight(kMinPwmOutput, kMinPwmOutput + speed_offset);
-			} else {
-				spinLeft(kMinPwmOutput, kMinPwmOutput + speed_offset);
-			}
-		} else {
-			if (should_move_forward) {
-				spinLeft(kMinPwmOutput + speed_offset, kMinPwmOutput);
-			} else {
-				spinRight(kMinPwmOutput + speed_offset, kMinPwmOutput);
-			}
-		}
+		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);
-		if (should_move_forward) {
-			moveForward(speed);
-		} else {
-			moveBackward(speed);
-		}
+		moveToFixPitch(should_move_forward, speed);
 	} else {
 		stopMotors();
 	}
@@ -110,3 +94,29 @@ void SealStrategy::moveBackward(float speed)
 	_motor_left.spin(Rotation::clockwise, speed);
 	_motor_right.spin(Rotation::counterClockwise, speed);
 }
+
+void SealStrategy::spinToFixRoll(bool is_right_tilted, bool should_move_forward, float speed_offset)
+{
+	if (is_right_tilted) {
+		if (should_move_forward) {
+			spinRight(kMinPwmOutput, kMinPwmOutput + speed_offset);
+		} else {
+			spinLeft(kMinPwmOutput, kMinPwmOutput + speed_offset);
+		}
+	} else {
+		if (should_move_forward) {
+			spinLeft(kMinPwmOutput + speed_offset, kMinPwmOutput);
+		} else {
+			spinRight(kMinPwmOutput + speed_offset, kMinPwmOutput);
+		}
+	}
+}
+
+void SealStrategy::moveToFixPitch(bool should_move_forward, float speed)
+{
+	if (should_move_forward) {
+		moveForward(speed);
+	} else {
+		moveBackward(speed);
+	}
+}
diff --git a/spikes/lk_auto_charge/SealStrategy.h b/spikes/lk_auto_charge/SealStrategy.h
@@ -42,6 +42,9 @@ class SealStrategy
 	void moveForward(float speed);
 	void moveBackward(float speed);
 
+	void spinToFixRoll(bool is_right_tilted, bool should_move_forward, float speed_offset);
+	void moveToFixPitch(bool should_move_forward, float speed);
+
 	interface::EventLoop &_event_loop;
 	interface::Timeout &_timeout;