Both wheels can now move instead of the right one only

Chi-EEE · Jan 10, 2024 · 5215c30 · 5215c30
1 parent 37c6a73
commit 5215c30
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 51 deletions.
diff --git a/app/raspberry_pi/repository/packages/t/tb6612/tb6612/include/TB6612.h b/app/raspberry_pi/repository/packages/t/tb6612/tb6612/include/TB6612.h
@@ -23,9 +23,13 @@ class TB6612
 
     void setOffset(bool offset);
 
+    const int& getMotorPin() const;
+
+    const int& getPWMPin() const;
+
 private:
-    int motor_pin;
-    int pwm_pin;
+    const int motor_pin;
+    const int pwm_pin;
     bool offset = true;
 };
 

diff --git a/app/raspberry_pi/repository/packages/t/tb6612/tb6612/src/TB6612.cpp b/app/raspberry_pi/repository/packages/t/tb6612/tb6612/src/TB6612.cpp
@@ -33,3 +33,13 @@ void TB6612::setOffset(bool offset)
 {
     this->offset = offset;
 }
+
+const int &TB6612::getMotorPin() const
+{
+    return this->motor_pin;
+}
+
+const int &TB6612::getPWMPin() const
+{
+    return this->pwm_pin;
+}
diff --git a/app/raspberry_pi/src/car/system/movement/controller/CarWheelController.cxx b/app/raspberry_pi/src/car/system/movement/controller/CarWheelController.cxx
@@ -33,58 +33,69 @@ namespace car::system::movement::controller
 	class CarWheelController : public AbstractWheelController
 	{
 	public:
-		CarWheelController() {
+		CarWheelController()
+		{
 			this->pwm = std::make_shared<PCA9685>();
 			this->rear_left_wheel = std::make_unique<RearWheel>(
 				this->pwm,
-				std::make_unique<TB6612>(Motor_A, PWM_A)
-			);
+				std::make_unique<TB6612>(Motor_A, PWM_A));
 			this->rear_right_wheel = std::make_unique<RearWheel>(
 				this->pwm,
-				std::make_unique<TB6612>(Motor_B, PWM_B)
-			);
+				std::make_unique<TB6612>(Motor_B, PWM_B));
 			this->front_left_wheel = std::make_unique<FrontWheel>(
 				this->pwm,
-				0
-			);
+				0);
 			this->front_right_wheel = std::make_unique<FrontWheel>(
 				this->pwm,
-				1
-			);
+				1);
 		};
 
-		~CarWheelController() {};
+		~CarWheelController()
+		{
+			this->front_left_wheel->reset();
+			this->front_right_wheel->reset();
+			this->rear_left_wheel->stop();
+			this->rear_right_wheel->stop();
+			this->pwm->reset();
+		};
 
-		void initialize() override {
+		void initialize() override
+		{
 			this->pwm->init(BUS_NUMBER, 0x40);
 			std::this_thread::sleep_for(std::chrono::milliseconds(100));
 			this->pwm->setPWMFreq(FREQUENCY);
 			std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 		}
 
-		void setRearWheelsSpeed(const SpeedCommand& speed_command) override {
+		void setRearWheelsSpeed(const SpeedCommand &speed_command) override
+		{
 			this->setRearLeftWheelSpeed(speed_command);
 			this->setRearRightWheelSpeed(speed_command);
 		}
 
-		void setFrontWheelsAngle(const AngleCommand& angle_command) override {
+		void setFrontWheelsAngle(const AngleCommand &angle_command) override
+		{
 			this->setFrontLeftWheelAngle(angle_command);
 			this->setFrontRightWheelAngle(angle_command);
 		}
 
-		void setRearLeftWheelSpeed(const SpeedCommand& speed_command) override {
+		void setRearLeftWheelSpeed(const SpeedCommand &speed_command) override
+		{
 			this->rear_left_wheel->setSpeed(speed_command.speed);
 		}
 
-		void setRearRightWheelSpeed(const SpeedCommand& speed_command) override {
+		void setRearRightWheelSpeed(const SpeedCommand &speed_command) override
+		{
 			this->rear_right_wheel->setSpeed(speed_command.speed);
 		}
 
-		void setFrontLeftWheelAngle(const AngleCommand& angle_command) override {
+		void setFrontLeftWheelAngle(const AngleCommand &angle_command) override
+		{
 			this->front_left_wheel->setAngle(angle_command.angle);
 		}
 
-		void setFrontRightWheelAngle(const AngleCommand& angle_command) override {
+		void setFrontRightWheelAngle(const AngleCommand &angle_command) override
+		{
 			this->front_right_wheel->setAngle(angle_command.angle);
 		}
 

diff --git a/app/raspberry_pi/src/car/system/movement/wheels/FrontWheel.cxx b/app/raspberry_pi/src/car/system/movement/wheels/FrontWheel.cxx
@@ -54,6 +54,10 @@ namespace car::system::movement::wheels
 			spdlog::info("Channel: {} \tAngle: {} \tAnalog Angle: {}", this->channel, angle, analog_angle);
 		}
 
+		void reset() {
+			this->setAngle(90);
+		}
+
 	private:
 		const std::shared_ptr<PCA9685> pwm;
 		const int channel;

diff --git a/app/raspberry_pi/src/car/system/movement/wheels/RearWheel.cxx b/app/raspberry_pi/src/car/system/movement/wheels/RearWheel.cxx
@@ -15,18 +15,10 @@ namespace car::system::movement::wheels
 {
 	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<PCA9685> pwm, std::unique_ptr<TB6612> motor) : pwm(pwm),
 																				 motor(std::move(motor))
 		{
-			this->forward_A = true;
-
 			this->speed = 0;
 		}
 
@@ -59,41 +51,18 @@ namespace car::system::movement::wheels
 			}
 			this->speed = new_speed;
 			const int pulse_wide = (this->speed / 100.0f) * 4095;
-			this->pwm->setPWM(PWM_A, 0, pulse_wide);
+			this->pwm->setPWM(this->motor->getPWMPin(), 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