second order ish IK

lib/Chassis/src/chassis.cpp

@@ -139,7 +139,22 @@ void Chassis::SetMotorEfforts(int16_t left, int16_t right)
 
 Twist Chassis::CalcOdomFromWheelMotion(void)
 {
-    Twist velocity;
+    Twist delta;
+
+    float leftDelta = (float) leftMotor.speed * (float) CONTROL_LOOP_PERIOD_MS / 1000.0;
+    float rightDelta = (float) rightMotor.speed * (float) CONTROL_LOOP_PERIOD_MS / 1000.0;
+
+    float distance = (leftDelta + rightDelta) / 2.0;
+    distance /= LEFT_TICKS_PER_CM;
+
+    float deltaHeading = (rightDelta - leftDelta) 
+        / LEFT_TICKS_PER_CM 
+        / (3.14 * ROBOT_RADIUS);
+
+    delta.u = distance;
+    delta.omega = deltaHeading;
+
+
     /**
      * TODO: Calculate velocities from wheel motion, which are held in leftMotor.spped and rightMotor.speed.
      * Note that you might want to calculate the deltas instead of speeds (to save some floating point maths). 
@@ -148,9 +163,9 @@ Twist Chassis::CalcOdomFromWheelMotion(void)
      */
 
 #ifdef __NAV_DEBUG__
-    TeleplotPrint("u", velocity.u);
+    TeleplotPrint("u", delta.u);
-    TeleplotPrint("omega", velocity.omega);
+    TeleplotPrint("omega", delta.omega);
 #endif
 
-    return velocity;
+    return delta;
 }

src/robot-nav.cpp

@@ -10,12 +10,16 @@ void Robot::UpdatePose(const Twist& twist)
      * TODO: Add your FK algorithm to update currPose here.
      */
 
+    float transitHeading = currPose.theta + (twist.omega * 0.5);
+
+    currPose.x += twist.u * cos(transitHeading);
+    currPose.y += twist.u * sin(transitHeading);
+
 #ifdef __NAV_DEBUG__
     TeleplotPrint("x", currPose.x);
     TeleplotPrint("y", currPose.y);
     TeleplotPrint("theta", currPose.theta);
 #endif
-
 }
 
 /**
@@ -69,4 +73,4 @@ void Robot::HandleDestination(void)
     /**
      * TODO: Stop and change state. Turn off LED.
      */
-}
+}