follow sa path

PATH is in lab coordinates, then transformed

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

@@ -3,6 +3,7 @@
  */
 
 #include "robot.h"
+#include "utils.h"
 
 void Robot::UpdatePose(const Pose& delta)
 {
@@ -18,8 +19,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
 }
@@ -38,7 +46,21 @@ void Robot::SetDestination(const Pose& dest)
     Serial.print(dest.y);
     Serial.print('\n');
 
-    destPose = dest;
+    // transform to board space
+
+    Pose transformed;
+
+    //// rotate clockwise
+    transformed.x = dest.y;
+    transformed.y = -dest.x;
+    transformed.theta = dest.theta;
+
+    // add offset
+    transformed.x += offsetPose.x;
+    transformed.y += offsetPose.y;
+    transformed.theta += offsetPose.theta;
+
+    destPose = transformed;
     robotState = ROBOT_DRIVE_TO_POINT;
 }
 
@@ -66,6 +88,37 @@ bool Robot::CheckReachedDestination(void)
         && RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
 }
 
+float boost(float input, float boost) {
+    if (input == 0.0) {
+        return input;
+    }
+    if (input > 0.0) {
+        input += boost;
+    } else {
+        input -= boost;
+    }
+    return input;
+}
+
+// x forward
+// y left
+//
+// goal y forward x right
+const int PATH_LEN=3;
+Pose PATH[PATH_LEN] = {
+    Pose(0.0, 81.0, 0.0),
+    Pose(40.0, 40.0, M_PI/ 2.0),
+    Pose(0.0, 0.0, 0.0),
+
+};
+
+
+void Robot::DrivePath(void) {
+    currPose = offsetPose; // clear pose
+    index = 0;
+    SetDestination(PATH[index]);
+}
+
 void Robot::DriveToPoint(void)
 {
     if(robotState == ROBOT_DRIVE_TO_POINT)
@@ -75,27 +128,49 @@ 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);
+
+
+        if (heading_err > 15.0) {
+            forward_err = 0.0;
+        }
 
         float turn_effort = heading_err * LATERAL_kP;
         float forward_effort = forward_err * LONGITUDINAL_kP;
 
+        turn_effort = max(min(turn_effort, 100.0), -100.0); // constrain turn_effort
+
 #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);
+        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();
+        return;
+    }
+
+    TeleplotPrint("path_index", index);
+
+    SetDestination(PATH[index]);
 }

src/robot.cpp

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

src/robot.h

@@ -5,8 +5,10 @@
 const float TARGET_SQ_DIST = pow(3.0, 2); // distance from setpoint that is ok (cm)
 const float TARGET_RAD_ERR = 0.25; // angle from setpoint that is ok (rad)
 
-const float LONGITUDINAL_kP = 3.0;
+const float LONGITUDINAL_kP = 4.0;
-const float LATERAL_kP = 3.0 * DEG_TO_RAD;
+const float LATERAL_kP = 1.7 * RAD_TO_DEG;
+
+const Pose offsetPose = Pose(100.0, 100.0, 0.0); // hack to avoid negatives
 
 class Robot
 {
@@ -36,6 +38,8 @@ protected:
     Pose currPose;
     Pose destPose;
     
+    int index = 0;
+    
 public:
     Robot(void) {keyString.reserve(10);}
     void InitializeRobot(void);
@@ -51,6 +55,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);