Diff.Drive Ant

Arduino/Things/DRV8833.cpp

@@ -1,13 +1,119 @@
 #include "DRV8833.h"
 
+#include <Arduino.h>
+
 namespace RoboidControl {
 
-DRV8833::DRV8833(unsigned char pinAIn1,
+DRV8833Motor::DRV8833Motor(Participant* participant,
+                           unsigned char pinIn1,
+                           unsigned char pinIn2,
+                           RotationDirection direction)
+    : Thing(participant) {
+  this->pinIn1 = pinIn1;
+  this->pinIn2 = pinIn2;
+
+#if (ESP32)
+  in1Ch = nextAvailablePwmChannel++;
+  ledcSetup(in1Ch, 500, 8);
+  ledcAttachPin(pinIn1, in1Ch);
+  in2Ch = nextAvailablePwmChannel++;
+  ledcSetup(in2Ch, 500, 8);
+  ledcAttachPin(pinIn2, in2Ch);
+#else
+  pinMode(pinIn1, OUTPUT);  // configure the in1 pin to output mode
+  pinMode(pinIn2, OUTPUT);  // configure the in1 pin to output mode
+#endif
+
+  this->direction = direction;
+}
+
+void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
+  this->maxRpm = rpm;
+}
+
+void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
+  Thing::SetAngularVelocity(velocity);
+  // ignoring rotation axis for now.
+  // Spherical angularVelocity = this->GetAngularVelocity();
+  float angularSpeed = velocity.distance;  // in degrees/sec
+
+  float rpm = angularSpeed / 360 * 60;
+  float motorSpeed = rpm / this->maxRpm;
+
+  uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
+  // if (direction == RotationDirection::CounterClockwise)
+  //   speed = -speed;
+  // Determine the rotation direction
+  if (velocity.direction.horizontal.InDegrees() < 0)
+    motorSpeed = -motorSpeed;
+
+  std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm " << rpm
+            << ", motor signal = " << (int)motorSignal << "\n";
+
+#if (ESP32)
+  if (motorSpeed == 0) {  // stop
+    ledcWrite(in1Ch, 0);
+    ledcWrite(in2Ch, 0);
+
+  } else if (motorSpeed > 0) {  // forward
+#if FAST_DECAY
+    ledcWrite(in1Ch, motorSignal);
+    ledcWrite(in2Ch, 0);
+#else
+    ledcWrite(in1Ch, 255);
+    ledcWrite(in2Ch, 255 - motorSignal);
+#endif
+  } else {  // (motorSpeed < 0) reverse
+#if FAST_DECAY
+    ledcWrite(in1Ch, 0);
+    ledcWrite(in2Ch, motorSignal);
+#else
+    ledcWrite(in1Ch, 255 - motorSignal);
+    ledcWrite(in2Ch, 255);
+#endif
+  }
+#else  // not ESP32
+  if (motorSpeed == 0) {  // stop
+    analogWrite(pinIn1, 0);
+    analogWrite(pinIn2, 0);
+
+  } else if (motorSpeed > 0) {  // forward
+#if FAST_DECAY
+    analogWrite(pinIn1, motorSignal);
+    analogWrite(pinIn2, 0);
+#else
+    analogWrite(pinIn1, 255);
+    analogWrite(pinIn2, 255 - motorSignal);
+#endif
+  } else {  // (motorSpeed < 0) reverse
+#if FAST_DECAY
+    analogWrite(pinIn1, 0);
+    analogWrite(pinIn2, motorSignal);
+#else
+    analogWrite(pinIn1, 255 - motorSignal);
+    analogWrite(pinIn2, 255);
+#endif
+  }
+#endif
+}
+
+DRV8833::DRV8833(Participant* participant,
+                 unsigned char pinAIn1,
                  unsigned char pinAIn2,
                  unsigned char pinBIn1,
                  unsigned char pinBIn2,
                  unsigned char pinStandby,
-                 RotationDirection directionA,
-                 RotationDirection directionB) {}
+                 DRV8833Motor::RotationDirection directionA,
+                 DRV8833Motor::RotationDirection directionB)
+    : Thing(participant) {
+  this->pinStandby = pinStandby;
+  if (pinStandby != 255)
+    pinMode(pinStandby, OUTPUT);
+
+  this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, directionA);
+  this->motorA->name = "Motor A";
+  this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, directionB);
+  this->motorB->name = "Motor B";
+}
 
 }  // namespace RoboidControl

Arduino/Things/DRV8833.h

@@ -1,14 +1,37 @@
 #pragma once
 
 #include "Thing.h"
+#include "Things/DifferentialDrive.h"
 
 /// @brief Support for a DRV8833 motor controller
 namespace RoboidControl {
-class DRV8833 {
-public:
+
+class DRV8833Motor : public Thing {
+ public:
   /// @brief Motor turning direction
   enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
 
+  /// @brief Setup the DC motor
+  /// @param pinIn1 the pin number for the in1 signal
+  /// @param pinIn2 the pin number for the in2 signal
+  /// @param direction the forward turning direction of the motor
+  DRV8833Motor(Participant* participant,
+               unsigned char pinIn1,
+               unsigned char pinIn2,
+               RotationDirection direction = RotationDirection::Clockwise);
+  void SetMaxRPM(unsigned int rpm);
+
+  virtual void SetAngularVelocity(Spherical velocity) override;
+
+ protected:
+  unsigned char pinIn1 = 255;
+  unsigned char pinIn2 = 255;
+  RotationDirection direction = RotationDirection::Clockwise;
+  unsigned int maxRpm = 200;
+};
+
+class DRV8833 : public Thing {
+ public:
   /// @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
@@ -18,16 +41,20 @@ public:
   /// 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,
+  DRV8833(Participant* participant,
+          unsigned char pinAIn1,
           unsigned char pinAIn2,
           unsigned char pinBIn1,
           unsigned char pinBIn2,
           unsigned char pinStandby = 255,
-          RotationDirection directionA = RotationDirection::Clockwise,
-          RotationDirection directionB = RotationDirection::Clockwise);
+          DRV8833Motor::RotationDirection directionA = DRV8833Motor::RotationDirection::Clockwise,
+          DRV8833Motor::RotationDirection directionB = DRV8833Motor::RotationDirection::Clockwise);
 
-    Thing* motorA = nullptr;
-    Thing* motorB = nullptr;
+  DRV8833Motor* motorA = nullptr;
+  DRV8833Motor* motorB = nullptr;
+
+ protected:
+  unsigned char pinStandby = 255;
 };
 
 }  // namespace RoboidControl

Thing.h

@@ -126,13 +126,13 @@ class Thing {
 
   /// @brief Set the linear velocity of the thing
   /// @param linearVelocity The new linear velocity in local space, in meters per second
-  void SetLinearVelocity(Spherical16 linearVelocity);
+  virtual void SetLinearVelocity(Spherical16 linearVelocity);
   /// @brief Get the linear velocity of the thing
   /// @return The linear velocity in local space, in meters per second
   virtual Spherical16 GetLinearVelocity();
   /// @brief Set the angular velocity of the thing
   /// @param angularVelocity the new angular velocity in local space
-  void SetAngularVelocity(Spherical16 angularVelocity);
+  virtual void SetAngularVelocity(Spherical16 angularVelocity);
   /// @brief Get the angular velocity of the thing
   /// @return The angular velocity in local space
   virtual Spherical16 GetAngularVelocity();

Things/DifferentialDrive.cpp

@@ -2,11 +2,14 @@
 
 namespace RoboidControl {
 
-RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) : Thing(participant) {}
+RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) : Thing(participant) {
+  // this->leftWheel = new Thing(participant);
+  // this->rightWheel = new Thing(participant);
+}
 
 void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation) {
-  this->wheelDiameter = abs(wheelDiameter);
-  this->wheelSeparation = abs(wheelSeparation);
+  this->wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
+  this->wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
   this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
 
   float distance = this->wheelSeparation / 2;
@@ -27,6 +30,27 @@ void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
     this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }
 
-void DifferentialDrive::Update(unsigned long currentMs) {}
+void DifferentialDrive::Update(unsigned long currentMs) {
+  // this assumes forward velocity only....
+  float linearVelocity = this->GetLinearVelocity().distance;
+
+  Spherical angularVelocity = this->GetAngularVelocity();
+  float angularSpeed = angularVelocity.distance * Deg2Rad;  // in degrees/sec
+  // Determine the rotation direction
+  if (angularVelocity.direction.horizontal.InDegrees() < 0)
+    angularSpeed = -angularSpeed;
+
+  // wheel separation can be replaced by this->leftwheel->position->distance
+  float speedLeft = (linearVelocity + angularSpeed * this->wheelSeparation / 2) / this->wheelRadius * Rad2Deg;
+  if (this->leftWheel != nullptr)
+    this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
+
+  float speedRight = (linearVelocity - angularSpeed * this->wheelSeparation / 2) / this->wheelRadius * Rad2Deg;
+  if (this->rightWheel != nullptr)
+    this->rightWheel->SetAngularVelocity(Spherical(speedRight, Direction::right));
+
+    std::cout << "lin. speed " << linearVelocity << "   ang. speed " << angularVelocity.distance << "   left wheel  "
+            << speedLeft << "   right wheel " << speedRight << "\n";
+}
 
 }  // namespace RoboidControl

Things/DifferentialDrive.h

@@ -15,8 +15,8 @@ class DifferentialDrive : public Thing {
   virtual void Update(unsigned long currentMs) override;
 
  protected:
-  /// @brief The diameter of a wheel in meters
-  float wheelDiameter = 1.0f;
+  /// @brief The radius of a wheel in meters
+  float wheelRadius = 1.0f;
   /// @brief The distance between the wheels in meters
   float wheelSeparation = 1.0f;