Create code for Wheels

app/raspberry_pi/src/car/system/lidar/LidarDummy.hpp

@@ -5,6 +5,8 @@
 
 #include <fstream>
 
+#include <spdlog/spdlog.h>
+
 #include "LidarDevice.hpp"
 
 namespace car::system::lidar {
@@ -13,6 +15,7 @@ namespace car::system::lidar {
 	public:
 		LidarDummy()
 		{
+			spdlog::warn("Currently using the LidarDummy");
 		};
 
 		void initialize() const override {};

app/raspberry_pi/src/car/system/lidar/LidarScanner.hpp

@@ -19,27 +19,19 @@ namespace car::system::lidar
 	public:
 		LidarScanner(const std::string &lidar_port, const bool &enabled) : lidar(RPLidar::create(lidar_port).value()), enabled(enabled)
 		{
-			if (!this->enabled)
-			{
-				spdlog::warn("The Lidar Scanner is disabled");
-			};
 		};
 
 		~LidarScanner(){};
 
 		void initialize() const override
 		{
-			if (!this->enabled)
-				return;
 			this->lidar->reset();
 			this->lidar->stop();
 			this->lidar->stop_motor();
 		};
 
 		void start() const override
 		{
-			if (!this->enabled)
-				return;
 			this->lidar->start_motor();
 
 			// auto info_result = lidar->get_info();
@@ -60,27 +52,19 @@ namespace car::system::lidar
 		};
 		std::vector<Measure> scan() const override
 		{
-			if (!this->enabled)
-			{
-				std::vector<Measure> measures;
-				return measures;
-			}
 			std::function<std::vector<Measure>()> scanGenerator = this->lidar->iter_scans();
 			return scanGenerator();
 		};
 
 		void terminate() const override
 		{
-			if (!this->enabled)
-				return;
 			this->lidar->stop();
 			this->lidar->stop_motor();
 			this->lidar->disconnect();
 		}
 
 	private:
 		std::unique_ptr<RPLidar> lidar;
-		const bool enabled;
 	};
 }
 

app/raspberry_pi/src/car/system/movement/wheels/FrontWheel.hpp

@@ -1,6 +1,56 @@
+#include <algorithm>
+
+#include <PCA9685.h>
+#include <spdlog/spdlog.h>
+
 namespace car::system::movement::wheels
 {
-    class FrontWheel {
+	class FrontWheel {
+	private:
+		/**
+		 * Following method clamps the x to in_min and in_max.
+		 * Afterwards, it puts the result of that into the range of out_min and out_max
+		*/
+		static int map(int x, int in_min, int in_max, int out_min, int out_max)
+		{
+			return ((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min);
+		}
+
+		static constexpr int MIN_PULSE_WIDTH = 900;
+		static constexpr int MAX_PULSE_WIDTH = 2100;
+		static constexpr int FREQUENCY = 50;
+
+	public:
+		FrontWheel(std::shared_ptr<PCA9685> pwm, int channel) : pwm(pwm), channel(channel) {
+		}
+
+		// Some of the code was from: https://github.com/chaoticmachinery/pca9685
+		int getAnalogAngle() const
+		{
+			float pulse_wide = map(this->angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
+			int analog_angle = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
+			return analog_angle;
+		}
+
+		int getAngle() const
+		{
+			return this->angle;
+		}
+
+		// Some of the code was from: https://github.com/chaoticmachinery/pca9685
+		int setAngle(const int& angle)
+		{
+			this->angle = std::clamp(angle, 0, 180);
+			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:
+		const std::shared_ptr<PCA9685> pwm;
+		const int channel;
 
-    };
+		int angle;
+	};
 } // namespace car::system::movement::wheels

app/raspberry_pi/src/car/system/movement/wheels/RearWheel.hpp

@@ -1,6 +1,89 @@
+#include <memory>
+
+#include <spdlog/spdlog.h>
+
+#include <PCA9865.h>
+#include <TB6612.hpp>
+
+// Made with the help of ChatGPT
+
 namespace car::system::movement::wheels
 {
-    class RearWheel {
+	class RearWheel {
+	private:
+		static constexpr int Motor_A = 17;
+		static constexpr int Motor_B = 27;
+		static constexpr int PWM_A = 4;
+		static constexpr int PWM_B = 5;
+	public:
+		RearWheel(std::shared_ptr<PCA9865> pwm, std::unique_ptr<TB6612> motor) :
+			pwm(pwm),
+			motor(motor)
+		{
+			this->forward_A = true;
+
+			this->speed = 0;
+		}
+
+			void forward()
+		{
+			this->motor->forward();
+		}
+
+		void backward()
+		{
+			this->motor->backward();
+		}
+
+		void stop()
+		{
+			this->motor->stop();
+		}
+
+		int getSpeed() const
+		{
+			return this->speed;
+		}
+
+		void setSpeed(const int& speed)
+		{
+			this->speed = std::clamp(speed, 0, 100);
+			const int pulse_wide = (this->speed / 100.0f) * 4095;
+			this->pwm->setPWM(PWM_A, 0, pulse_wide);
+		}
+
+		void ready()
+		{
+			this->motor->setOffset(this->forward_A);
+			this->stop();
+		}
+
+		void calibration()
+		{
+			this->setSpeed(50);
+			this->forward();
+			this->cali_forward = this->forward_A;
+		}
+
+		void caliLeft()
+		{
+			this->cali_forward = (1 + this->cali_forward) & 1;
+			this->motor->setOffset(this->cali_forward);
+			this->forward();
+		}
+
+		void caliOK()
+		{
+			this->forward_A = this->cali_forward;
+			this->stop();
+		}
+
+	private:
+		std::shared_ptr<PCA9685> pwm;
+		std::unique_ptr<TB6612> motor;
 
-    };
+		int forward_A;
+		int cali_forward;
+		int speed;
+	};
 } // namespace car::system::movement::wheels

app/raspberry_pi/src/main.cpp

@@ -36,7 +36,6 @@ int main()
 	using namespace rplidar;
 
 	// spdlog::set_level(spdlog::level::off);
-	//test();
 
 	std::string websocket_url = getWebSocketUrl();
 	spdlog::info("Got websocket url: {}", websocket_url);