Make DiffDrive subclass of propulsion

DifferentialDrive.cpp

@@ -0,0 +1,57 @@
+#include "DifferentialDrive.h"
+
+#include "FloatSingle.h"
+
+DifferentialDrive::DifferentialDrive(){};
+
+DifferentialDrive::DifferentialDrive(Placement leftMotorPlacement,
+                                     Placement rightMotorPlacement) {
+  this->motorCount = 2;
+  this->placement = new Placement[2];
+  this->placement[0] = leftMotorPlacement;
+  this->placement[1] = rightMotorPlacement;
+  Motor* leftMotor = (Motor*)leftMotorPlacement.thing;
+  leftMotor->direction = Motor::Direction::CounterClockwise;
+  Motor* rightMotor = (Motor*)rightMotorPlacement.thing;
+  rightMotor->direction = Motor::Direction::Clockwise;
+}
+
+void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
+  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
+    Thing* thing = placement[motorIx].thing;
+    if (thing->type == Thing::UncontrolledMotorType) {
+      Motor* motor = (Motor*)thing;
+      if (motor == nullptr)
+        continue;
+
+      float xPosition = placement[motorIx].position.x;
+      if (xPosition < 0)
+        motor->SetSpeed(leftSpeed);
+      else if (xPosition > 0)
+        motor->SetSpeed(rightSpeed);
+
+    } else if (thing->type == Thing::ControlledMotorType) {
+      ControlledMotor* motor = (ControlledMotor*)placement[motorIx].thing;
+      if (motor == nullptr)
+        continue;
+
+      float xPosition = placement[motorIx].position.x;
+      if (xPosition < 0)
+        motor->SetTargetSpeed(leftSpeed);
+      else if (xPosition > 0)
+        motor->SetTargetSpeed(rightSpeed);
+    }
+  };
+}
+
+void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
+  float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
+  float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
+  SetTargetSpeeds(leftSpeed, rightSpeed);
+}
+
+void DifferentialDrive::SetTwistSpeed(float forward, float yaw, float pitch) {
+  float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
+  float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
+  SetTargetSpeeds(leftSpeed, rightSpeed);
+}

DifferentialDrive.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "Propulsion.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+class DifferentialDrive : public Propulsion {
+ public:
+  DifferentialDrive();
+  DifferentialDrive(Placement leftMotorPlacement,
+                    Placement rightMotorPlacement);
+  void SetTargetSpeeds(float leftSpeed, float rightSpeed);
+
+  virtual void SetTwistSpeed(float forward, float yaw) override;
+  virtual void SetTwistSpeed(float forward, float yaw, float pitch) override;
+};
+
+}  // namespace RoboidControl
+}  // namespace Passer
+using namespace Passer::RoboidControl;

Perception.cpp

@@ -4,6 +4,11 @@
 
 Perception::Perception() {}
 
+Perception::Perception(Placement* sensors, unsigned int sensorCount) {
+  this->sensorCount = sensorCount;
+  this->sensorPlacements = sensors;
+}
+
 void Perception::AddSensors(Placement* things, unsigned int thingCount) {
   sensorCount = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
@@ -32,7 +37,7 @@ Sensor* Perception::GetSensor(unsigned int sensorId) {
     return nullptr;
 
   Thing* thing = this->sensorPlacements[sensorId].thing;
-  if (thing->type & Thing::SensorType != 0)
+  if ((thing->type & Thing::SensorType) != 0)
     return (Sensor*)thing;
 
   return nullptr;
@@ -43,7 +48,7 @@ float Perception::DistanceForward(float angle) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
     Sensor* sensor = (Sensor*)placement.thing;
-    if (sensor->type & Thing::SensorType != 0)
+    if ((sensor->type & Thing::SensorType) != 0)
       continue;
 
     DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@@ -60,7 +65,7 @@ float Perception::DistanceLeft(float angle) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
     Sensor* sensor = (Sensor*)placement.thing;
-    if (sensor->type & Thing::SensorType != 0)
+    if ((sensor->type & Thing::SensorType) != 0)
       continue;
 
     DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@@ -95,7 +100,7 @@ float Perception::DistanceUp(float angle) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
     Sensor* sensor = (Sensor*)placement.thing;
-    if (sensor->type & Thing::SensorType != 0)
+    if ((sensor->type & Thing::SensorType) != 0)
       continue;
 
     DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@@ -112,7 +117,7 @@ float Perception::DistanceDown(float angle) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
     Sensor* sensor = (Sensor*)placement.thing;
-    if (sensor->type & Thing::SensorType != 0)
+    if ((sensor->type & Thing::SensorType) != 0)
       continue;
 
     DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;

Perception.h

@@ -10,6 +10,7 @@ class Perception {
  public:
   /// @brief Setup perception
   Perception();
+  Perception(Placement* sensors, unsigned int sensorCount);
 
   // void AddSensors(SensorPlacement* sensors, unsigned int sensorCount);
   void AddSensors(Placement* sensors, unsigned int sensorCount);

Propulsion.cpp

@@ -10,10 +10,10 @@ Propulsion::Propulsion() {
   this->motorCount = 0;
 }
 
-void Propulsion::AddMotors(Placement *things, unsigned int thingCount) {
+void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
   this->motorCount = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
-    Thing *thing = things[thingIx].thing;
+    Thing* thing = things[thingIx].thing;
     if ((thing->type & Thing::MotorType) != 0)
       motorCount++;
     if (thing->type == (int)Thing::Type::ControlledMotor)
@@ -23,34 +23,36 @@ void Propulsion::AddMotors(Placement *things, unsigned int thingCount) {
 
   unsigned int motorIx = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
-    Thing *thing = things[thingIx].thing;
+    Thing* thing = things[thingIx].thing;
     if ((thing->type & Thing::MotorType) != 0)
       this->placement[motorIx++] = things[thingIx];
   }
 }
 
-void Propulsion::AddQuadcopter(Quadcopter *quadcopter) {
+void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
   this->quadcopter = quadcopter;
 }
 
-unsigned int Propulsion::GetMotorCount() { return this->motorCount; }
+unsigned int Propulsion::GetMotorCount() {
+  return this->motorCount;
+}
 
-Motor *Propulsion::GetMotor(unsigned int motorId) {
+Motor* Propulsion::GetMotor(unsigned int motorId) {
   if (motorId >= this->motorCount)
     return nullptr;
 
-  Thing *thing = this->placement[motorId].thing;
+  Thing* thing = this->placement[motorId].thing;
   if ((thing->type & Thing::MotorType) != 0)
-    return (Motor *)thing;
+    return (Motor*)thing;
 
   return nullptr;
 }
 
-Placement *Propulsion::GetMotorPlacement(unsigned int motorId) {
+Placement* Propulsion::GetMotorPlacement(unsigned int motorId) {
   if (motorId >= this->motorCount)
     return nullptr;
 
-  Placement *placement = &this->placement[motorId];
+  Placement* placement = &this->placement[motorId];
   if ((placement->thing->type & Thing::MotorType) != 0)
     return placement;
 
@@ -59,72 +61,74 @@ Placement *Propulsion::GetMotorPlacement(unsigned int motorId) {
 
 void Propulsion::Update(float currentTimeMs) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Thing *thing = placement[motorIx].thing;
+    Thing* thing = placement[motorIx].thing;
     if (thing->type == Thing::ControlledMotorType) {
-      ControlledMotor *motor = (ControlledMotor *)thing;
+      ControlledMotor* motor = (ControlledMotor*)thing;
       motor->Update(currentTimeMs);
     }
   }
   this->lastUpdateTime = currentTimeMs;
 }
 
-void Propulsion::SetMaxSpeed(float maxSpeed) { this->maxSpeed = abs(maxSpeed); }
+void Propulsion::SetMaxSpeed(float maxSpeed) {
-
+  this->maxSpeed = abs(maxSpeed);
-void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
-  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Thing *thing = placement[motorIx].thing;
-    if (thing->type == Thing::UncontrolledMotorType) {
-      Motor *motor = (Motor *)thing;
-      if (motor == nullptr)
-        continue;
-
-      float xPosition = placement[motorIx].position.x;
-      if (xPosition < 0)
-        motor->SetSpeed(leftSpeed);
-      else if (xPosition > 0)
-        motor->SetSpeed(rightSpeed);
-
-    } else if (thing->type == Thing::ControlledMotorType) {
-      ControlledMotor *motor = (ControlledMotor *)placement[motorIx].thing;
-      if (motor == nullptr)
-        continue;
-
-      float xPosition = placement[motorIx].position.x;
-      if (xPosition < 0)
-        motor->SetTargetSpeed(leftSpeed);
-      else if (xPosition > 0)
-        motor->SetTargetSpeed(rightSpeed);
-    }
-  };
 }
 
-void Propulsion::SetDiffDriveVelocities(float leftVelocity,
+// void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
-                                        float rightVelocity) {
+//   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
+//     Thing *thing = placement[motorIx].thing;
-    // Placement placement = placement[motorIx];
+//     if (thing->type == Thing::UncontrolledMotorType) {
-    // if (placement.position.x < 0)
+//       Motor *motor = (Motor *)thing;
-    //   placement.controlledMotor->SetTargetVelocity(leftVelocity);
+//       if (motor == nullptr)
-    // else if (placement.position.x > 0)
+//         continue;
-    //   placement.controlledMotor->SetTargetVelocity(rightVelocity);
+
-  };
+//       float xPosition = placement[motorIx].position.x;
-}
+//       if (xPosition < 0)
+//         motor->SetSpeed(leftSpeed);
+//       else if (xPosition > 0)
+//         motor->SetSpeed(rightSpeed);
+
+//     } else if (thing->type == Thing::ControlledMotorType) {
+//       ControlledMotor *motor = (ControlledMotor *)placement[motorIx].thing;
+//       if (motor == nullptr)
+//         continue;
+
+//       float xPosition = placement[motorIx].position.x;
+//       if (xPosition < 0)
+//         motor->SetTargetSpeed(leftSpeed);
+//       else if (xPosition > 0)
+//         motor->SetTargetSpeed(rightSpeed);
+//     }
+//   };
+// }
+
+// void Propulsion::SetDiffDriveVelocities(float leftVelocity,
+//                                         float rightVelocity) {
+//   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
+//     // Placement placement = placement[motorIx];
+//     // if (placement.position.x < 0)
+//     //   placement.controlledMotor->SetTargetVelocity(leftVelocity);
+//     // else if (placement.position.x > 0)
+//     //   placement.controlledMotor->SetTargetVelocity(rightVelocity);
+//   };
+// }
 
 void Propulsion::SetTwistSpeed(float forward, float yaw) {
   // This is configuration dependent, a drone will do something completely
   // different...
-  float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
+  // float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
-  float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
+  // float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
-  SetDiffDriveSpeed(leftSpeed, rightSpeed);
+  // SetDiffDriveSpeed(leftSpeed, rightSpeed);
 }
 
 void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) {
-  float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
+  // float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
-  float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
+  // float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
-  SetDiffDriveSpeed(leftSpeed, rightSpeed);
+  // SetDiffDriveSpeed(leftSpeed, rightSpeed);
 
-  if (quadcopter != nullptr) {
+  // if (quadcopter != nullptr) {
-    quadcopter->SetTwistSpeed(forward, yaw, pitch);
+  //   quadcopter->SetTwistSpeed(forward, yaw, pitch);
-  }
+  // }
 }
 
 void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
@@ -136,18 +140,21 @@ void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
 
 void Propulsion::SetTwistVelocity(float forwardVelocity,
                                   float turningVelocity) {
-  float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1, 1);
+  // float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1,
-  float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity, -1, 1);
+  // 1); float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity,
-  SetDiffDriveVelocities(leftVelocity, rightVelocity);
+  // -1, 1); SetDiffDriveVelocities(leftVelocity, rightVelocity);
 }
 
-void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed,
+void Propulsion::SetLinearSpeed(Vector3 velocity,
+                                float yawSpeed,
                                 float rollSpeed) {
   if (quadcopter != nullptr)
     quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
 }
 
-Quadcopter *Propulsion::GetQuadcopter() { return quadcopter; }
+Quadcopter* Propulsion::GetQuadcopter() {
+  return quadcopter;
+}
 
 /// @brief Odometer returns the total distance traveled since start
 /// @return The total distance
@@ -159,9 +166,9 @@ Quadcopter *Propulsion::GetQuadcopter() { return quadcopter; }
 float Propulsion::GetOdometer() {
   float odometer = 0;
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Thing *thing = placement[motorIx].thing;
+    Thing* thing = placement[motorIx].thing;
     if ((thing->type & Thing::ControlledMotorType) != 0) {
-      ControlledMotor *motor = (ControlledMotor *)thing;
+      ControlledMotor* motor = (ControlledMotor*)thing;
       odometer += motor->encoder->GetDistance() / this->motorCount;
     }
   }

Propulsion.h

@@ -1,28 +1,29 @@
 #pragma once
+
 #include "ControlledMotor.h"
 #include "Placement.h"
 #include "Quadcopter.h"
 #include "Vector2.h"
 
-#include <time.h>
+// #include <time.h>
 
 namespace Passer {
 namespace RoboidControl {
 
 class Propulsion {
-public:
+ public:
   /// @brief Setup sensing
   Propulsion();
 
   void Update(float currentTimeMs);
 
-  void AddMotors(Placement *motors, unsigned int motorCount);
+  void AddMotors(Placement* motors, unsigned int motorCount);
-  void AddQuadcopter(Quadcopter *quadcopter);
+  void AddQuadcopter(Quadcopter* quadcopter);
-  Quadcopter *GetQuadcopter();
+  Quadcopter* GetQuadcopter();
 
   unsigned int GetMotorCount();
-  Motor *GetMotor(unsigned int motorIx);
+  Motor* GetMotor(unsigned int motorIx);
-  Placement *GetMotorPlacement(unsigned int motorIx);
+  Placement* GetMotorPlacement(unsigned int motorIx);
 
   /// @brief Set the maximum linear speed.
   /// @param maxSpeed The new maximum linear speed
@@ -33,16 +34,17 @@ public:
   void SetMaxSpeed(float maxSpeed);
 
   // Velocity control
-  void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
+  // void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
-  void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
+  // void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
 
-  void SetTwistSpeed(float forward, float yaw);
+  virtual void SetTwistSpeed(float forward, float yaw);
-  void SetTwistSpeed(float forward, float yaw, float pitch);
+  virtual void SetTwistSpeed(float forward, float yaw, float pitch);
-  void SetTwistSpeed(Vector3 linear, float yaw);
+  virtual void SetTwistSpeed(Vector3 linear, float yaw);
-  void SetTwistVelocity(float forward, float yaw);
+  virtual void SetTwistVelocity(float forward, float yaw);
 
   // Think: drones
-  void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F,
+  void SetLinearSpeed(Vector3 direction,
+                      float yawSpeed = 0.0F,
                       float rollSpeed = 0.0F);
 
   // Position control (or actually, distance control)
@@ -50,11 +52,11 @@ public:
 
   float GetOdometer();
 
-  Placement *placement = nullptr;
+  Placement* placement = nullptr;
   unsigned int motorCount = 0;
 
-protected:
+ protected:
-  Quadcopter *quadcopter = nullptr;
+  Quadcopter* quadcopter = nullptr;
 
   float maxSpeed = 1;
 

Roboid.cpp

@@ -2,21 +2,26 @@
 #include <Arduino.h>
 
 Roboid::Roboid() {
-  this->configuration = nullptr;
+  // this->configuration = nullptr;
-  this->thingCount = 0;
+  // this->thingCount = 0;
 }
 
-Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
+// Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
-  this->configuration = configuration;
+//   // this->configuration = configuration;
-  this->thingCount = thingCount;
+//   // this->thingCount = thingCount;
 
-  perception.AddSensors(configuration, thingCount);
+//   perception.AddSensors(configuration, thingCount);
-  propulsion.AddMotors(configuration, thingCount);
+//   propulsion.AddMotors(configuration, thingCount);
+// }
+
+Roboid::Roboid(Perception* perception, Propulsion* propulsion) {
+  this->perception = perception;
+  this->propulsion = propulsion;
 }
 
 bool Roboid::Drive(Waypoint* waypoint, float currentTimeMs) {
   bool finished =
-      propulsion.Drive(waypoint->point, waypoint->rotation, currentTimeMs);
+      propulsion->Drive(waypoint->point, waypoint->rotation, currentTimeMs);
   return finished;
 }
 

Roboid.h

@@ -2,8 +2,9 @@
 
 #include "Activation.h"
 #include "Perception.h"
-#include "Placement.h"
+// #include "Placement.h"
-#include "Propulsion.h"
+//  #include "Propulsion.h"
+#include "DifferentialDrive.h"
 
 namespace Passer {
 namespace RoboidControl {
@@ -40,13 +41,14 @@ class Roboid {
  public:
   Roboid();
   Roboid(Placement configuration[], unsigned int thingCount);
+  Roboid(Perception* perception, Propulsion* propulsion);
 
-  Perception perception;
+  Perception* perception;
-  Propulsion propulsion;
+  Propulsion* propulsion;
   Acceleration acceleration;
 
-  Placement* configuration;
+  // Placement* configuration;
-  unsigned int thingCount;
+  // unsigned int thingCount;
 
   void Update(float currentTimeMs);
 

VectorAlgebra

@@ -1 +1 @@
-Subproject commit 493a3f748907b4fb7e64177f94b7cb98a951af4c
+Subproject commit 006ea046e4bb57b033823ba5a3fad0dfc7f5dfba