src/robot-nav.cpp

@@ -42,37 +42,20 @@ void Robot::SetDestination(const Pose& dest)
     robotState = ROBOT_DRIVE_TO_POINT;
 }
 
-float Robot::SquareDistance(void) {
-    return pow((destPose.x - currPose.x),2) + pow((destPose.y-currPose.y),2);
-}
-
 bool Robot::CheckReachedDestination(void)
 {
+    bool retVal = false;
     /**
      * TODO: Add code to check if you've reached destination here.
      */
 
-    float rad_error = abs(destPose.theta - currPose.theta);
-
-    return SquareDistance() < TARGET_SQ_DIST && rad_error < TARGET_RAD_ERR;
+    return retVal;
 }
 
 void Robot::DriveToPoint(void)
 {
     if(robotState == ROBOT_DRIVE_TO_POINT)
     {
-        float target_heading;
-
-        if (SquareDistance() < TARGET_SQ_DIST) {
-            target_heading = destPose.theta;
-        } else {
-            target_heading = atan2(destPose.x - currPose.x, destPose.y - currPose.y);
-        }
-
-        float heading_err = destPose.theta - target_heading;
-        float forward_effort = cos(heading_err);
-
-
         /**
          * TODO: Add your IK algorithm here. 
          */

src/robot.h

@@ -2,9 +2,6 @@
 
 #include "chassis.h"
 
-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)
-
 class Robot
 {
 protected:
@@ -52,7 +49,4 @@ protected:
     void CalibDrive(void);
     bool CheckReachedDestination(void);
     void HandleDestination(void);
-
-    // Square distance between current and target pose
-    float SquareDistance(void);
 };