First trajectory

ControlledMotor.cpp

@@ -1,7 +1,8 @@
 #include "ControlledMotor.h"
 
 ControlledMotor::ControlledMotor() {
-  this->isControlledMotor = true;
+  // this->isControlledMotor = true;
+  this->type = Type::ControlledMotor;
 }
 
 ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) {

Motor.cpp

@@ -1,7 +1,11 @@
 #include "Motor.h"
+#include <time.h>
+
+#include <Arduino.h>
 
 Motor::Motor() {
-  this->isMotor = true;
+  //   this->isMotor = true;
+  type = Type::Motor;
 }
 
 // Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) {
@@ -25,9 +29,25 @@ Motor::Motor() {
 // }
 
 float Motor::GetSpeed() {
-	return this->currentSpeed;
+  return this->currentSpeed;
 }
 
 void Motor::SetSpeed(float speed) {
-	this->currentSpeed = speed;
+  this->currentSpeed = speed;
+}
+
+bool Motor::Drive(float distance) {
+  if (!this->driving) {
+    this->startTime = time(NULL);
+    this->targetDistance = abs(distance);
+    this->driving = true;
+  }
+  double duration = difftime(time(NULL), this->startTime);
+  if (duration >= this->targetDistance) {
+    this->driving = false;
+    return true;
+  }
+
+  SetSpeed(distance < 0 ? -1 : 1);
+  return false;
 }

Motor.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <time.h>
 #include "Thing.h"
 
 class Motor : public Thing {
@@ -14,6 +15,11 @@ class Motor : public Thing {
   virtual void SetSpeed(float speed);
   float GetSpeed();
 
+  bool Drive(float distance);
+
  protected:
   float currentSpeed = 0;
+  bool driving = false;
+  float targetDistance = 0;
+  time_t startTime = 0;
 };

Propulsion.cpp

@@ -19,7 +19,8 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
   this->motorCount = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
     Thing* thing = things[thingIx].thing;
-    if (thing->isMotor)
+    // if (thing->isMotor)
+    if (thing->type == Thing::Type::Motor)
       motorCount++;
   }
   this->placement = new Placement[motorCount];
@@ -27,7 +28,8 @@ 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;
-    if (thing->isMotor)
+    // if (thing->isMotor)
+    if (thing->type == Thing::Type::Motor)
       this->placement[motorIx++] = things[thingIx];
   }
 }
@@ -45,7 +47,8 @@ Motor* Propulsion::GetMotor(unsigned int motorId) {
     return nullptr;
 
   Thing* thing = this->placement[motorId].thing;
-  if (thing->isMotor)
+  // if (thing->isMotor)
+  if (thing->type == Thing::Type::Motor)
     return (Motor*)thing;
 
   return nullptr;

Roboid.cpp

@@ -1,5 +1,5 @@
 #include "Roboid.h"
-
+#include <Arduino.h>
 Roboid::Roboid() {
   this->configuration = nullptr;
   this->thingCount = 0;
@@ -16,16 +16,51 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
 bool Roboid::Drive(float forwardDistance) {
   bool finished = true;
   for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) {
-    Motor* motor = (Motor*)propulsion.placement[motorIx].thing;
+    bool motorFinished = false;
-    if (motor == nullptr)
+    Thing* thing = propulsion.placement[motorIx].thing;
-      continue;
-    if (motor->isControlledMotor == false)
-      continue;
 
-    ControlledMotor* controlledMotor = (ControlledMotor*)motor;
+    switch (thing->type) {
-    bool motorFinished = controlledMotor->Drive(forwardDistance);
+      case Thing::Type::ControlledMotor: {
-    if (motorFinished == false)
+        ControlledMotor* controlledMotor = (ControlledMotor*)thing;
+        motorFinished = controlledMotor->Drive(forwardDistance);
+        break;
+      }
+      case Thing::Type::Motor: {
+        Motor* motor = (Motor*)thing;
+        motorFinished = motor->Drive(forwardDistance);
+        break;
+      }
+      default:
+        break;
+    }
+    if (motorFinished == false) {
       finished = false;
+    } else
+      Serial.printf("Motor finished\n");
   }
   return finished;
 }
+
+float waypointArray[] = {1, -1};
+void Roboid::FollowTrajectory(Trajectory* Trajectory) {
+  this->waypoints = waypointArray;
+  this->waypointCount = 2;
+  this->waypointIx = 0;
+}
+
+void Roboid::Update() {
+  if (this->waypoints == nullptr)
+    return;
+
+  // Serial.printf("Driving waypoints %d %f\n", this->waypointIx,
+  //               waypoints[waypointIx]);
+  if (Drive(this->waypoints[this->waypointIx])) {
+    this->waypointIx++;
+    if (this->waypointIx == this->waypointCount) {
+      this->waypoints = nullptr;
+      this->waypointIx = 0;
+      this->waypointCount = 0;
+      propulsion.SetTwistSpeed(0, 0);
+    }
+  }
+}

Roboid.h

@@ -5,6 +5,15 @@
 #include "Placement.h"
 #include "Propulsion.h"
 
+class WayPoint {
+ public:
+  float distance = 0;
+};
+class Trajectory {
+ public:
+  float* waypoints;
+};
+
 class Acceleration {
  public:
   float GetMagnitude() { return 0; };
@@ -22,5 +31,13 @@ class Roboid {
   Placement* configuration;
   unsigned int thingCount;
 
+  void Update();
+
   bool Drive(float forwardDistance);
+  void FollowTrajectory(Trajectory* Trajectory);
+
+ public:
+  float* waypoints = nullptr;
+  unsigned int waypointIx = 0;
+  unsigned int waypointCount = 0;
 };

Sensing.cpp

@@ -29,7 +29,8 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
   sensorCount = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
     Thing* thing = things[thingIx].thing;
-    if (thing->isSensor)
+    // if (thing->isSensor)
+    if (thing->type == Thing::Type::Sensor)
       sensorCount++;
   }
 
@@ -38,7 +39,8 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
   unsigned int sensorIx = 0;
   for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
     Thing* thing = things[thingIx].thing;
-    if (thing->isSensor)
+    // if (thing->isSensor)
+    if (thing->type == Thing::Type::Sensor)
       sensorPlacements[sensorIx++] = things[thingIx];
   }
 }
@@ -52,7 +54,8 @@ Sensor* Sensing::GetSensor(unsigned int sensorId) {
     return nullptr;
 
   Thing* thing = this->sensorPlacements[sensorId].thing;
-  if (thing->isSensor)
+  // if (thing->isSensor)
+  if (thing->type == Thing::Type::Sensor)
     return (Sensor*)thing;
 
   return nullptr;
@@ -167,7 +170,9 @@ bool Sensing::SwitchOn(float fromAngle, float toAngle) {
 }
 
 unsigned int Sensing::ToDepthMapIndex(float angle) {
-  unsigned int depthMapIx = (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) * (float)resolution);
+  unsigned int depthMapIx =
+      (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) *
+                     (float)resolution);
   return depthMapIx;
 }
 

Sensor.cpp

@@ -1,5 +1,6 @@
 #include "Sensor.h"
 
 Sensor::Sensor() {
-  this->isSensor = true;
+  // this->isSensor = true;
+  type = Type::Sensor;
 }

Thing.h

@@ -3,8 +3,14 @@
 /// @brief A thing is a functional component on a robot
 class Thing {
  public:
-  Thing() { isSensor = false, isMotor = false; }
+  Thing() {
-  bool isSensor;
+    type = Type::Undetermined;
-  bool isMotor;
+    // isSensor = false, isMotor = false;
-  bool isControlledMotor;
+  }
+
+  enum class Type { Undetermined, Sensor, Motor, ControlledMotor };
+  Type type = Type::Undetermined;
+  // bool isSensor;
+  // bool isMotor;
+  // bool isControlledMotor;
 };