transition from Twist to Pose

lib/Chassis/src/chassis.cpp

@@ -95,7 +95,7 @@ void Chassis::InititalizeChassis(void)
 /**
  * The main Chassis loop.
  */
-bool Chassis::ChassisLoop(Twist& velocity)
+bool Chassis::ChassisLoop(Pose& delta)
 {
     bool retVal = false;
 
@@ -109,7 +109,7 @@ bool Chassis::ChassisLoop(Twist& velocity)
 #endif
 
         /* Update the wheel velocity so it gets back to Robot. */
-        velocity = CalcOdomFromWheelMotion();
+        delta = CalcOdomFromWheelMotion();
 
         loopFlag = 0;
 
@@ -137,22 +137,24 @@ void Chassis::SetMotorEfforts(int16_t left, int16_t right)
 }
 
 
-Twist Chassis::CalcOdomFromWheelMotion(void)
+Pose Chassis::CalcOdomFromWheelMotion(void)
 {
-    Twist delta;
+    Pose delta;
 
-    float leftDelta = (float) leftMotor.speed * (float) CONTROL_LOOP_PERIOD_MS / 1000.0;
+    float leftDelta = (float) leftMotor.speed 
-    float rightDelta = (float) rightMotor.speed * (float) CONTROL_LOOP_PERIOD_MS / 1000.0;
+        * (float) CONTROL_LOOP_PERIOD_MS / 1000.0 
+        / LEFT_TICKS_PER_CM;
+    float rightDelta = (float) rightMotor.speed 
+        * (float) CONTROL_LOOP_PERIOD_MS / 1000.0
+        / RIGHT_TICKS_PER_CM;
 
     float distance = (leftDelta + rightDelta) / 2.0;
-    distance /= LEFT_TICKS_PER_CM;
 
     float deltaHeading = (rightDelta - leftDelta) 
-        / LEFT_TICKS_PER_CM 
+        / (2.0 * ROBOT_RADIUS);
-        / (3.14 * ROBOT_RADIUS);
 
-    delta.u = distance;
+    delta.x = distance;
-    delta.omega = deltaHeading;
+    delta.theta = deltaHeading;
 
 
     /**
@@ -163,8 +165,8 @@ Twist Chassis::CalcOdomFromWheelMotion(void)
      */
 
 #ifdef __NAV_DEBUG__
-    TeleplotPrint("u", delta.u);
+    TeleplotPrint("u", delta.x);
-    TeleplotPrint("omega", delta.omega);
+    TeleplotPrint("omega", delta.theta);
 #endif
 
     return delta;

lib/Chassis/src/chassis.h

@@ -30,13 +30,13 @@ public:
     void InititalizeChassis(void);
 
     /* Where the bulk of the work for the motors gets done. */
-    bool ChassisLoop(Twist&);
+    bool ChassisLoop(Pose&);
 
     /* Needed for managing motors. */
     static void Timer4OverflowISRHandler(void);
 
 public:
-    Twist CalcOdomFromWheelMotion(void);
+    Pose CalcOdomFromWheelMotion(void);
 
 
     void SetMotorEfforts(int16_t, int16_t);

src/robot-nav.cpp

@@ -4,16 +4,18 @@
 
 #include "robot.h"
 
-void Robot::UpdatePose(const Twist& twist)
+void Robot::UpdatePose(const Pose& delta)
 {
     /**
      * TODO: Add your FK algorithm to update currPose here.
      */
 
-    float transitHeading = currPose.theta + (twist.omega * 0.5);
+    float transitHeading = currPose.theta + (delta.theta * 0.5);
 
-    currPose.x += twist.u * cos(transitHeading);
+    currPose.x += delta.x * cos(transitHeading);
-    currPose.y += twist.u * sin(transitHeading);
+    currPose.y += delta.x * sin(transitHeading);
+
+    currPose.theta += delta.theta;
 
 #ifdef __NAV_DEBUG__
     TeleplotPrint("x", currPose.x);

src/robot.cpp

@@ -27,11 +27,11 @@ void Robot::RobotLoop(void)
      /**
      * Run the chassis loop, which handles low-level control.
      */
-    Twist velocity;
+    Pose delta;
-    if(chassis.ChassisLoop(velocity))
+    if(chassis.ChassisLoop(delta))
     {
         // We do FK regardless of state
-        UpdatePose(velocity);
+        UpdatePose(delta);
         chassis.SetMotorEfforts(220,-220);
         
         /**

src/robot.h

@@ -38,7 +38,7 @@ protected:
     void EnterIdleState(void);
 
     // /* Navigation methods.*/
-    void UpdatePose(const Twist& u);
+    void UpdatePose(const Pose& u);
     void SetDestination(const Pose& destination);
     void DriveToPoint(void);
     bool CheckReachedDestination(void);