From 29b5d0fbad2b4dd3a8759729e64add1c8320d5e3 Mon Sep 17 00:00:00 2001
From: Yann Locatelli <yann.locatelli2@gmail.com>
Date: Thu, 14 Dec 2023 17:54:32 +0100
Subject: [PATCH] Apply suggestion from review

---
 spikes/lk_auto_charge/SealStrategy.cpp | 46 ++++++++++++++++----------
 spikes/lk_auto_charge/SealStrategy.h   |  3 ++
 2 files changed, 31 insertions(+), 18 deletions(-)

diff --git a/spikes/lk_auto_charge/SealStrategy.cpp b/spikes/lk_auto_charge/SealStrategy.cpp
index c766371440..908add48a3 100644
--- 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
index 469facdaaf..72b16d07b9 100644
--- 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;