impl point tracking

switched longitudinal error equation to not care about lateral offsets

lib/Chassis/src/chassis.h

@@ -37,6 +37,7 @@ public:
     static void Timer4OverflowISRHandler(void);
 
 public:
+    Romi32U4ButtonA buttonA;
     Romi32U4ButtonB buttonB;
     Romi32U4ButtonC buttonC;
 

lib/Chassis/src/utils.cpp

@@ -2,7 +2,7 @@
 
 void TeleplotPrint(const char* var, float value)
 {
-    Serial.print('>');
+    //Serial.print('>');
     Serial.print(var);
     Serial.print(':');
     Serial.print(value);

src/robot-nav.cpp

@@ -18,8 +18,15 @@ void Robot::UpdatePose(const Pose& delta)
     currPose.theta += delta.theta;
 
 #ifdef __NAV_DEBUG__
-    TeleplotPrint("x", currPose.x);
+    Serial.print("pos");
-    TeleplotPrint("y", currPose.y);
+    Serial.print(':');
+    Serial.print(currPose.x);
+    Serial.print(':');
+    Serial.print(currPose.y);
+    Serial.print('|xy\n');
+
+    //TeleplotPrint("x", currPose.x);
+    //TeleplotPrint("y", currPose.y);
     TeleplotPrint("theta", currPose.theta * 180.0 / PI);
 #endif
 }
@@ -75,17 +82,22 @@ void Robot::DriveToPoint(void)
         if (SquareDistance() < TARGET_SQ_DIST) {
             target_heading = destPose.theta;
         } else {
-            target_heading = atan2(destPose.x - currPose.x, destPose.y - currPose.y);
+            target_heading = atan2(destPose.y - currPose.y, destPose.x - currPose.x);
         }
 
         float heading_err = RadError(currPose.theta, target_heading);
-        float forward_err = cos(heading_err) * sqrt(SquareDistance());
+        float forward_err = cos(currPose.theta) * (destPose.x - currPose.x)
+            + sin(currPose.theta) * (destPose.y - currPose.y);
 
         float turn_effort = heading_err * LATERAL_kP;
         float forward_effort = forward_err * LONGITUDINAL_kP;
 
 #ifdef __NAV_DEBUG__
         // Print useful stuff here.
+
+        TeleplotPrint("heading_err", heading_err * RAD_TO_DEG);
+        TeleplotPrint("linear_err", forward_err);
+        TeleplotPrint("linear_dist", SquareDistance());
 #endif
 
         chassis.SetMotorEfforts(forward_effort - turn_effort, forward_effort + turn_effort);

src/robot.cpp

@@ -1,4 +1,5 @@
 #include "robot.h"
+#include "utils.h"
 
 void Robot::InitializeRobot(void)
 {
@@ -76,6 +77,10 @@ void Robot::RobotLoop(void)
         UpdatePose(delta);
         //chassis.SetMotorEfforts(220,-220);
 
+        if (chassis.buttonA.isPressed()) {
+            Pose dest(0.0, 10.0, 0.0);
+            SetDestination(dest);
+        }
         if (chassis.buttonB.isPressed()) {
             EnterCalibTurn();
         }

src/robot.h

@@ -6,7 +6,7 @@ const float TARGET_SQ_DIST = pow(3.0, 2); // distance from setpoint that is ok (
 const float TARGET_RAD_ERR = 0.25; // angle from setpoint that is ok (rad)
 
 const float LONGITUDINAL_kP = 3.0;
-const float LATERAL_kP = 3.0 * DEG_TO_RAD;
+const float LATERAL_kP = 3.0 * RAD_TO_DEG;
 
 class Robot
 {