Cleanup based on ControlledMotor branch

DifferentialDrive.cpp

@@ -29,17 +29,6 @@ void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
         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);
     }
   };
 }
@@ -49,10 +38,3 @@ void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
   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);
-// }

Perception.cpp

@@ -1,5 +1,6 @@
 #include "Perception.h"
 #include "Angle.h"
+#include "DistanceSensor.h"
 #include "Switch.h"
 
 Perception::Perception() {}
@@ -203,18 +204,14 @@ void Perception::SetDepthMap(float angle, float distance) {
   depthMap[depthMapIx] = distance;
 }
 
-DistanceSensor* Perception::GetSensor(float angle) {
+Sensor* Perception::GetSensor(float angle) {
   angle = Angle::Normalize(angle);
 
   for (unsigned int ix = 0; ix < this->sensorCount; ix++) {
     Placement placement = this->sensorPlacements[ix];
     if (abs(placement.horizontalDirection - angle) < 0.01F)
-      return (DistanceSensor*)placement.thing;
+      return (Sensor*)placement.thing;
   }
 
-  DistanceSensor* distanceSensor = new DistanceSensor();
+  return nullptr;
-  Placement* newPlacement = new Placement(distanceSensor, angle);
-  AddSensors(newPlacement, 1);
-
-  return distanceSensor;
 }

Perception.h

@@ -14,7 +14,7 @@ class Perception {
   /// @brief Template to make it possible to leave out ths sensorCount
   /// @tparam sensorCount
   /// @param sensors An array of sensor placements
-  template <size_t sensorCount>
+  template <unsigned int sensorCount>
   inline Perception(Placement (&sensors)[sensorCount]) {
     Perception(sensors, sensorCount);
   }
@@ -73,7 +73,7 @@ class Perception {
   unsigned int ToDepthMapIndex(float angle);
   void SetDepthMap(float angle, float distance);
 
-  DistanceSensor* GetSensor(float angle);
+  Sensor* GetSensor(float angle);
 
  protected:
   // SensorPlacement* sensors = nullptr;

Placement.h

@@ -1,8 +1,5 @@
 #pragma once
 
-#include "ControlledMotor.h"
-#include "DistanceSensor.h"
-#include "Motor.h"
 #include "Thing.h"
 #include "Vector2.h"
 #include "Vector3.h"

Propulsion.cpp

@@ -1,5 +1,4 @@
 #include "Propulsion.h"
-#include "ControlledMotor.h"
 
 #include "FloatSingle.h"
 
@@ -16,8 +15,6 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
     Thing* thing = things[thingIx].thing;
     if (thing->IsMotor())
       motorCount++;
-    if (thing->type == Thing::ControlledMotorType)
-      hasOdometer = true;
   }
   this->placement = new Placement[motorCount];
 
@@ -56,13 +53,6 @@ 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;
-    if (thing->type == Thing::ControlledMotorType) {
-      ControlledMotor* motor = (ControlledMotor*)thing;
-      motor->Update(currentTimeMs);
-    }
-  }
   this->lastUpdateTime = currentTimeMs;
 }
 
@@ -78,57 +68,3 @@ void Propulsion::SetTwistSpeed(Vector3 linear,
                                float yaw,
                                float pitch,
                                float roll) {}
-
-/// @brief Odometer returns the total distance traveled since start
-/// @return The total distance
-/// This returns the average distance of all wheels. The odometer cannot be
-/// reset. When using a non-directional encoder, the distance is always
-/// increasing. When using a directional encoder, the distance may go down when
-/// the robot is driving backward.
-/// When no wheel encoder is present, this function always returns zero.
-float Propulsion::GetOdometer() {
-  float odometer = 0;
-  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Thing* thing = placement[motorIx].thing;
-    if ((thing->type & Thing::ControlledMotorType) != 0) {
-      ControlledMotor* motor = (ControlledMotor*)thing;
-      odometer += motor->encoder->GetDistance() / this->motorCount;
-    }
-  }
-  return odometer;
-}
-
-bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) {
-  if (!this->driving) {
-    this->startTime = time(NULL);
-    this->targetDistance = point.magnitude();
-    if (hasOdometer)
-      this->startOdometer = GetOdometer();
-    this->driving = true;
-  }
-  if (hasOdometer) {
-    float distance = GetOdometer() - this->startOdometer;
-    if (distance >= this->targetDistance) {
-      this->driving = false;
-      point = Vector3::zero;
-      rotation = 0;
-    }
-  } else {
-    double duration = difftime(time(NULL), this->startTime);
-    if (duration >= this->targetDistance) {
-      this->driving = false;
-      point = Vector3::zero;
-      rotation = 0;
-    }
-  }
-
-  if (rotation > 0)
-    rotation = 1;
-  if (rotation < 0)
-    rotation = -1;
-  SetTwistSpeed(point.normalized() * this->maxSpeed, rotation);
-
-  Update(currentTimeMs);
-
-  return (!this->driving);
-}

Propulsion.h

@@ -1,12 +1,9 @@
 #pragma once
 
-#include "ControlledMotor.h"
+#include "Motor.h"
 #include "Placement.h"
-// #include "Quadcopter.h"
 #include "Vector2.h"
 
-// #include <time.h>
-
 namespace Passer {
 namespace RoboidControl {
 
@@ -41,11 +38,6 @@ class Propulsion {
                              float pitch = 0.0F,
                              float roll = 0.0F);  // 3D
 
-  // Position control (or actually, distance control)
-  bool Drive(Vector3 point, float rotation, float currentTimeMs);
-
-  float GetOdometer();
-
   Placement* placement = nullptr;
   unsigned int motorCount = 0;
 
@@ -54,8 +46,6 @@ class Propulsion {
 
   bool driving = false;
   float targetDistance = 0;
-  bool hasOdometer = false;
-  float startOdometer;
   time_t startTime;
   float lastUpdateTime;
 };

Roboid.cpp

@@ -1,47 +1,9 @@
 #include "Roboid.h"
 #include <Arduino.h>
 
-Roboid::Roboid() {
+Roboid::Roboid() {}
-  // this->configuration = nullptr;
-  // this->thingCount = 0;
-}
-
-// Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
-//   // this->configuration = configuration;
-//   // this->thingCount = thingCount;
-
-//   perception.AddSensors(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);
-  return finished;
-}
-
-void Roboid::FollowTrajectory(Trajectory* trajectory) {
-  this->trajectory = trajectory;
-  this->waypointIx = 0;
-}
-
-void Roboid::Update(float currentTimeMs) {
-  if (this->trajectory == nullptr)
-    return;
-
-  Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx];
-  if (Drive(waypoint, currentTimeMs)) {
-    this->waypointIx++;
-    if (this->waypointIx >= this->trajectory->waypointCount) {
-      this->trajectory = nullptr;
-      this->waypointIx = 0;
-      // stop
-      // propulsion.SetTwistSpeed(0, 0);
-    }
-  }
-}

Roboid.h

@@ -1,42 +1,11 @@
 #pragma once
 
-#include "Activation.h"
 #include "Perception.h"
-// #include "Placement.h"
+#include "Propulsion.h"
-//  #include "Propulsion.h"
-#include "DifferentialDrive.h"
 
 namespace Passer {
 namespace RoboidControl {
 
-class Waypoint {
- public:
-  Waypoint(float forwardDistance, float rotation) {
-    this->point = Vector3(0, 0, forwardDistance);
-    this->distance = forwardDistance;
-    this->rotation = rotation;
-  }
-  float distance = 0;
-  Vector3 point = Vector3(0, 0, 0);
-  float rotation = 0;
-};
-class Trajectory {
- public:
-  Trajectory(){};
-  Trajectory(Waypoint* waypoints, unsigned int waypointCount) {
-    this->waypoints = waypoints;
-    this->waypointCount = waypointCount;
-  }
-
-  Waypoint* waypoints;
-  unsigned int waypointCount;
-};
-
-class Acceleration {
- public:
-  float GetMagnitude() { return 0; };
-};
-
 class Roboid {
  public:
   Roboid();
@@ -45,19 +14,8 @@ class Roboid {
 
   Perception* perception;
   Propulsion* propulsion;
-  Acceleration acceleration;
-
-  // Placement* configuration;
-  // unsigned int thingCount;
 
   void Update(float currentTimeMs);
-
-  bool Drive(Waypoint* waypoint, float currentTimeMs);
-  void FollowTrajectory(Trajectory* Trajectory);
-
- public:
-  Trajectory* trajectory;
-  unsigned int waypointIx = 0;
 };
 }  // namespace RoboidControl
 }  // namespace Passer