From d2b240a514b34e433f2f5a5fefcb5430761b0790 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Fri, 24 Nov 2023 17:32:21 +0100
Subject: [PATCH] Real trajectory

---
 Roboid.cpp | 57 ++++++++++++++++++++++++++++++++++++++----------------
 Roboid.h   | 24 ++++++++++++++++++-----
 2 files changed, 59 insertions(+), 22 deletions(-)

diff --git a/Roboid.cpp b/Roboid.cpp
index a3bd8b2..00223c2 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -13,7 +13,34 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
   propulsion.AddMotors(configuration, thingCount);
 }
 
-bool Roboid::Drive(float forwardDistance) {
+// bool Roboid::Drive(float forwardDistance) {
+//   bool finished = true;
+//   for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++)
+//   {
+//     bool motorFinished = false;
+//     Thing* thing = propulsion.placement[motorIx].thing;
+
+//     switch (thing->type) {
+//       case Thing::Type::ControlledMotor: {
+//         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;
+//   }
+//   return finished;
+// }
+
+bool Roboid::Drive(Waypoint* waypoint) {
   bool finished = true;
   for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) {
     bool motorFinished = false;
@@ -22,44 +49,40 @@ bool Roboid::Drive(float forwardDistance) {
     switch (thing->type) {
       case Thing::Type::ControlledMotor: {
         ControlledMotor* controlledMotor = (ControlledMotor*)thing;
-        motorFinished = controlledMotor->Drive(forwardDistance);
+        motorFinished = controlledMotor->Drive(waypoint->point.z);
         break;
       }
       case Thing::Type::Motor: {
         Motor* motor = (Motor*)thing;
-        motorFinished = motor->Drive(forwardDistance);
+        motorFinished = motor->Drive(waypoint->point.z);
         break;
       }
       default:
         break;
     }
-    if (motorFinished == false) {
+    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;
+void Roboid::FollowTrajectory(Trajectory* trajectory) {
+  this->trajectory = trajectory;
   this->waypointIx = 0;
 }
 
 void Roboid::Update() {
-  if (this->waypoints == nullptr)
+  if (this->trajectory == nullptr)
     return;
 
-  // Serial.printf("Driving waypoints %d %f\n", this->waypointIx,
-  //               waypoints[waypointIx]);
-  if (Drive(this->waypoints[this->waypointIx])) {
+  Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx];
+  // Serial.printf("Driving waypoints %d:  %f %f\n", this->waypointIx,
+  //               waypoint->point.z, waypoint->rotation);
+  if (Drive(waypoint)) {
     this->waypointIx++;
-    if (this->waypointIx == this->waypointCount) {
-      this->waypoints = nullptr;
+    if (this->waypointIx == this->trajectory->waypointCount) {
+      this->trajectory = nullptr;
       this->waypointIx = 0;
-      this->waypointCount = 0;
       propulsion.SetTwistSpeed(0, 0);
     }
   }
diff --git a/Roboid.h b/Roboid.h
index 17af5f3..c8f9c37 100644
--- a/Roboid.h
+++ b/Roboid.h
@@ -5,13 +5,27 @@
 #include "Placement.h"
 #include "Propulsion.h"
 
-class WayPoint {
+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:
-  float* waypoints;
+  Trajectory(){};
+  Trajectory(Waypoint* waypoints, unsigned int waypointCount) {
+    this->waypoints = waypoints;
+    this->waypointCount = waypointCount;
+  }
+
+  Waypoint* waypoints;
+  unsigned int waypointCount;
 };
 
 class Acceleration {
@@ -33,11 +47,11 @@ class Roboid {
 
   void Update();
 
-  bool Drive(float forwardDistance);
+  // bool Drive(float forwardDistance);
+  bool Drive(Waypoint* waypoint);
   void FollowTrajectory(Trajectory* Trajectory);
 
  public:
-  float* waypoints = nullptr;
+  Trajectory* trajectory;
   unsigned int waypointIx = 0;
-  unsigned int waypointCount = 0;
 };
\ No newline at end of file