lib/Chassis/src/chassis.h

@@ -37,7 +37,6 @@ 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,7 +3,6 @@
  */
 
 #include "robot.h"
-#include "utils.h"
 
 void Robot::UpdatePose(const Pose& delta)
 {
@@ -19,15 +18,8 @@ void Robot::UpdatePose(const Pose& delta)
     currPose.theta += delta.theta;
 
 #ifdef __NAV_DEBUG__
-    Serial.print("pos");
-    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("x", currPose.x);
+    TeleplotPrint("y", currPose.y);
     TeleplotPrint("theta", currPose.theta * 180.0 / PI);
 #endif
 }
@@ -46,21 +38,7 @@ void Robot::SetDestination(const Pose& dest)
     Serial.print(dest.y);
     Serial.print('\n');
 
-    // 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;
+    destPose = dest;
     robotState = ROBOT_DRIVE_TO_POINT;
 }
 
@@ -88,37 +66,6 @@ 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)
@@ -128,49 +75,27 @@ void Robot::DriveToPoint(void)
         if (SquareDistance() < TARGET_SQ_DIST) {
             target_heading = destPose.theta;
         } else {
-            target_heading = atan2(destPose.y - currPose.y, destPose.x - currPose.x);
+            target_heading = atan2(destPose.x - currPose.x, destPose.y - currPose.y);
         }
 
         float heading_err = RadError(currPose.theta, target_heading);
-        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 forward_err = cos(heading_err) * sqrt(SquareDistance());
 
         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
 
-        float boost_f = 15.0;
-        chassis.SetMotorEfforts(boost(forward_effort - turn_effort,boost_f), boost(forward_effort + turn_effort, boost_f));
+        chassis.SetMotorEfforts(forward_effort - turn_effort, forward_effort + turn_effort);
     }
 }
 
-#define len(x) sizeof(x) / sizeof(x[0])
 void Robot::HandleDestination(void)
 {
     /**
      * TODO: Stop and change state. Turn off LED.
      */
-    index += 1;
-    if (index == len(PATH)) { 
-        EnterIdleState();
-        return;
-    }
-
-    TeleplotPrint("path_index", index);
-
-    SetDestination(PATH[index]);
+    EnterIdleState(); // TODO: sequence
 }

src/robot.cpp

@@ -1,5 +1,4 @@
 #include "robot.h"
-#include "utils.h"
 
 void Robot::InitializeRobot(void)
 {
@@ -77,12 +76,6 @@ 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,10 +5,8 @@
 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 = 4.0;
-const float LATERAL_kP = 1.7 * RAD_TO_DEG;
-
-const Pose offsetPose = Pose(100.0, 100.0, 0.0); // hack to avoid negatives
+const float LONGITUDINAL_kP = 3.0;
+const float LATERAL_kP = 3.0 * DEG_TO_RAD;
 
 class Robot
 {
@@ -38,8 +36,6 @@ protected:
     Pose currPose;
     Pose destPose;
     
-    int index = 0;
-    
 public:
     Robot(void) {keyString.reserve(10);}
     void InitializeRobot(void);
@@ -55,7 +51,6 @@ 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);