diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp
index b9663d0..bb8e158 100644
--- a/src/robot-nav.cpp
+++ b/src/robot-nav.cpp
@@ -62,10 +62,6 @@ float Robot::RadError(float current, float destination) {
 
 bool Robot::CheckReachedDestination(void)
 {
-    /**
-     * TODO: Add code to check if you've reached destination here.
-     */
-
     return SquareDistance() < TARGET_SQ_DIST 
         && RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
 }
@@ -83,20 +79,16 @@ void Robot::DriveToPoint(void)
         }
 
         float heading_err = RadError(currPose.theta, target_heading);
-        float forward_effort = cos(heading_err) * sqrt(SquareDistance());
+        float forward_err = cos(heading_err) * sqrt(SquareDistance());
 
-
-        /**
-         * TODO: Add your IK algorithm here. 
-         */
+        float turn_effort = heading_err * LATERAL_kP;
+        float forward_effort = forward_err * LONGITUDINAL_kP;
 
 #ifdef __NAV_DEBUG__
         // Print useful stuff here.
 #endif
 
-        /**
-         * TODO: Call chassis.SetMotorEfforts() to command the motion, based on your calculations above.
-         */
+        chassis.SetMotorEfforts(forward_effort - turn_effort, forward_effort + turn_effort);
     }
 }
 
@@ -105,4 +97,5 @@ void Robot::HandleDestination(void)
     /**
      * TODO: Stop and change state. Turn off LED.
      */
+    EnterIdleState(); // TODO: sequence
 }
diff --git a/src/robot.h b/src/robot.h
index 8ddd65a..4f8d975 100644
--- a/src/robot.h
+++ b/src/robot.h
@@ -5,6 +5,9 @@
 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 LATERAL_kP = 3.0 * DEG_TO_RAD;
+
 class Robot
 {
 protected: