path sequencing

src/robot-nav.cpp

@@ -3,6 +3,7 @@
  */
 
 #include "robot.h"
+#include "utils.h"
 
 void Robot::UpdatePose(const Pose& delta)
 {
@@ -23,7 +24,7 @@ void Robot::UpdatePose(const Pose& delta)
     Serial.print(currPose.x);
     Serial.print(':');
     Serial.print(currPose.y);
-    Serial.print('|xy\n');
+    Serial.print("|xy\n");
 
     //TeleplotPrint("x", currPose.x);
     //TeleplotPrint("y", currPose.y);
@@ -85,6 +86,17 @@ float boost(float input, float boost) {
     return input;
 }
 
+Pose PATH[2] = {
+    Pose(43.0,0,0),
+    Pose(0,0,0),
+};
+
+void Robot::DrivePath(void) {
+    currPose = Pose(); // clear pose
+    index = 0;
+    SetDestination(PATH[index]);
+}
+
 void Robot::DriveToPoint(void)
 {
     if(robotState == ROBOT_DRIVE_TO_POINT)
@@ -112,16 +124,23 @@ void Robot::DriveToPoint(void)
         TeleplotPrint("linear_dist", SquareDistance());
 #endif
 
-        //chassis.SetMotorEfforts(20, 20);
         float boost_f = 15.0;
         chassis.SetMotorEfforts(boost(forward_effort - turn_effort,boost_f), boost(forward_effort + turn_effort, boost_f));
     }
 }
 
+#define len(x) sizeof(x) / sizeof(x[0])
 void Robot::HandleDestination(void)
 {
     /**
      * TODO: Stop and change state. Turn off LED.
      */
-    EnterIdleState(); // TODO: sequence
+    index += 1;
+    if (index == len(PATH)) { 
+        EnterIdleState();
+    }
+
+    TeleplotPrint("path_index", index);
+
+    SetDestination(PATH[index]);
 }

src/robot.cpp

@@ -79,6 +79,7 @@ void Robot::RobotLoop(void)
 
         if (chassis.buttonA.isPressed()) {
             Pose dest(0.0, 10.0, 0.0);
+            index = 0;
             SetDestination(dest);
         }
         if (chassis.buttonB.isPressed()) {

src/robot.h

@@ -36,6 +36,8 @@ protected:
     Pose currPose;
     Pose destPose;
     
+    int index = 0;
+    
 public:
     Robot(void) {keyString.reserve(10);}
     void InitializeRobot(void);
@@ -51,6 +53,7 @@ protected:
     void UpdatePose(const Pose& u);
     void SetDestination(const Pose& destination);
     void DriveToPoint(void);
+    void DrivePath(void);
     void CalibTurn(void);
     void CalibDrive(void);
     bool CheckReachedDestination(void);