From 7dd894602a44c9ca56d7454b4e7edeea9517c2d1 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Mon, 3 Mar 2025 17:12:43 +0100
Subject: [PATCH] Add differential drive

---
 Arduino/Things/DRV8833.cpp   | 13 +++++++++++++
 Arduino/Things/DRV8833.h     | 33 +++++++++++++++++++++++++++++++++
 Things/DifferentialDrive.cpp | 32 ++++++++++++++++++++++++++++++++
 Things/DifferentialDrive.h   | 30 ++++++++++++++++++++++++++++++
 4 files changed, 108 insertions(+)
 create mode 100644 Arduino/Things/DRV8833.cpp
 create mode 100644 Arduino/Things/DRV8833.h
 create mode 100644 Things/DifferentialDrive.cpp
 create mode 100644 Things/DifferentialDrive.h

diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp
new file mode 100644
index 0000000..3b3b0a1
--- /dev/null
+++ b/Arduino/Things/DRV8833.cpp
@@ -0,0 +1,13 @@
+#include "DRV8833.h"
+
+namespace RoboidControl {
+
+DRV8833::DRV8833(unsigned char pinAIn1,
+                 unsigned char pinAIn2,
+                 unsigned char pinBIn1,
+                 unsigned char pinBIn2,
+                 unsigned char pinStandby,
+                 RotationDirection directionA,
+                 RotationDirection directionB) {}
+
+}  // namespace RoboidControl
\ No newline at end of file
diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h
new file mode 100644
index 0000000..afea9e3
--- /dev/null
+++ b/Arduino/Things/DRV8833.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include "Thing.h"
+
+/// @brief Support for a DRV8833 motor controller
+namespace RoboidControl {
+class DRV8833 {
+public:
+  /// @brief Motor turning direction
+  enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
+
+  /// @brief Setup a DRV8833 motor controller
+  /// @param pinAIn1 The pin number connected to the AIn1 port
+  /// @param pinAIn2 The pin number connected to the AIn2 port
+  /// @param pinBIn1 The pin number connected to the BIn1 port
+  /// @param pinBIn2 The pin number connceted to the BIn2 port
+  /// @param pinStandby The pin number connected to the standby port, 255
+  /// indicated that the port is not connected
+  /// @param directionA The forward turning direction of motor A
+  /// @param directionB The forward turning direction of motor B
+  DRV8833(unsigned char pinAIn1,
+          unsigned char pinAIn2,
+          unsigned char pinBIn1,
+          unsigned char pinBIn2,
+          unsigned char pinStandby = 255,
+          RotationDirection directionA = RotationDirection::Clockwise,
+          RotationDirection directionB = RotationDirection::Clockwise);
+
+    Thing* motorA = nullptr;
+    Thing* motorB = nullptr;
+};
+
+}  // namespace RoboidControl
\ No newline at end of file
diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp
new file mode 100644
index 0000000..531822b
--- /dev/null
+++ b/Things/DifferentialDrive.cpp
@@ -0,0 +1,32 @@
+#include "DifferentialDrive.h"
+
+namespace RoboidControl {
+
+RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) : Thing(participant) {}
+
+void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation) {
+  this->wheelDiameter = abs(wheelDiameter);
+  this->wheelSeparation = abs(wheelSeparation);
+  this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
+
+  float distance = this->wheelSeparation / 2;
+  if (leftWheel != nullptr)
+    this->leftWheel->SetPosition(Spherical(distance, Direction::left));
+  if (rightWheel != nullptr)
+    this->rightWheel->SetPosition(Spherical(distance, Direction::right));
+}
+
+void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
+  float distance = this->wheelSeparation / 2;
+  this->leftWheel = leftWheel;
+  if (leftWheel != nullptr)
+    this->leftWheel->SetPosition(Spherical(distance, Direction::left));
+
+  this->rightWheel = rightWheel;
+  if (rightWheel != nullptr)
+    this->rightWheel->SetPosition(Spherical(distance, Direction::right));
+}
+
+void DifferentialDrive::Update(unsigned long currentMs) {}
+
+}  // namespace RoboidControl
\ No newline at end of file
diff --git a/Things/DifferentialDrive.h b/Things/DifferentialDrive.h
new file mode 100644
index 0000000..fb7bda2
--- /dev/null
+++ b/Things/DifferentialDrive.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include "Thing.h"
+
+namespace RoboidControl {
+
+/// @brief A thing which can move itself using a differential drive system
+class DifferentialDrive : public Thing {
+ public:
+  DifferentialDrive(Participant* participant);
+
+  void SetDimensions(float wheelDiameter, float wheelSeparation);
+  void SetMotors(Thing* leftWheel, Thing* rightWheel);
+
+  virtual void Update(unsigned long currentMs) override;
+
+ protected:
+  /// @brief The diameter of a wheel in meters
+  float wheelDiameter = 1.0f;
+  /// @brief The distance between the wheels in meters
+  float wheelSeparation = 1.0f;
+
+  /// @brief Convert revolutions per second to meters per second
+  float rpsToMs = 1.0f;
+
+  Thing* leftWheel = nullptr;
+  Thing* rightWheel = nullptr;
+};
+
+}  // namespace RoboidControl
\ No newline at end of file