From c50e890708d68eb29ce3062c7254e9b415f56266 Mon Sep 17 00:00:00 2001
From: Chi Huu Huynh <73843190+Chi-EEE@users.noreply.github.com>
Date: Tue, 9 Jan 2024 15:07:09 +0000
Subject: [PATCH] Add code for setting wheel variable

---
 app/raspberry_pi/src/car/display/CarConsole.cpp          | 2 ++
 .../src/car/system/movement/wheels/FrontWheel.cxx        | 9 ++++++---
 .../src/car/system/movement/wheels/RearWheel.cxx         | 7 ++++++-
 3 files changed, 14 insertions(+), 4 deletions(-)

diff --git a/app/raspberry_pi/src/car/display/CarConsole.cpp b/app/raspberry_pi/src/car/display/CarConsole.cpp
index 29a41212..2364279a 100644
--- a/app/raspberry_pi/src/car/display/CarConsole.cpp
+++ b/app/raspberry_pi/src/car/display/CarConsole.cpp
@@ -327,6 +327,8 @@ namespace car::display {
 					// main thread). Using `screen.Post(task)` is threadsafe.
 					screen.Post([&]
 						{
+							this->car_system->setFrontWheelsAngle({ front_wheel_angle * 1.0f });
+							this->car_system->setRearWheelsSpeed({ rear_wheel_speed });
 							this->car_system->update();
 						}
 					);
diff --git a/app/raspberry_pi/src/car/system/movement/wheels/FrontWheel.cxx b/app/raspberry_pi/src/car/system/movement/wheels/FrontWheel.cxx
index 8e9615e5..d7d7d144 100644
--- a/app/raspberry_pi/src/car/system/movement/wheels/FrontWheel.cxx
+++ b/app/raspberry_pi/src/car/system/movement/wheels/FrontWheel.cxx
@@ -42,13 +42,16 @@ namespace car::system::movement::wheels
 		}
 
 		// Some of the code was from: https://github.com/chaoticmachinery/pca9685
-		int setAngle(const int& angle)
+		void setAngle(const int& angle)
 		{
-			this->angle = std::clamp(angle, 0, 180);
+			const int new_angle = std::clamp(angle, 0, 180);
+			if (new_angle == this->angle) {
+				return;
+			}
+			this->angle = new_angle;
 			int analog_angle = getAnalogAngle();
 			this->pwm->setPWM(channel, 0, analog_angle);
 			spdlog::info("Channel: {} \tAngle: {} \tAnalog Angle: {}", this->channel, angle, analog_angle);
-			return (0);
 		}
 
 	private:
diff --git a/app/raspberry_pi/src/car/system/movement/wheels/RearWheel.cxx b/app/raspberry_pi/src/car/system/movement/wheels/RearWheel.cxx
index 21abc1b7..b78998d8 100644
--- a/app/raspberry_pi/src/car/system/movement/wheels/RearWheel.cxx
+++ b/app/raspberry_pi/src/car/system/movement/wheels/RearWheel.cxx
@@ -52,7 +52,12 @@ namespace car::system::movement::wheels
 
 		void setSpeed(const int &speed)
 		{
-			this->speed = std::clamp(speed, 0, 100);
+			const int new_speed = std::clamp(speed, 0, 100);
+			if (new_speed == this->speed)
+			{
+				return;
+			}
+			this->speed = new_speed;
 			const int pulse_wide = (this->speed / 100.0f) * 4095;
 			this->pwm->setPWM(PWM_A, 0, pulse_wide);
 		}