From aba495d3f1e7c73ec9c4f3fc3e8a601c43e890cf Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 4 Feb 2026 13:38:01 -0500
Subject: [PATCH] transition from Twist to Pose

---
 lib/Chassis/src/chassis.cpp | 28 +++++++++++++++-------------
 lib/Chassis/src/chassis.h   |  4 ++--
 src/robot-nav.cpp           | 10 ++++++----
 src/robot.cpp               |  6 +++---
 src/robot.h                 |  2 +-
 5 files changed, 27 insertions(+), 23 deletions(-)

diff --git a/lib/Chassis/src/chassis.cpp b/lib/Chassis/src/chassis.cpp
index d9cc499..762fb42 100644
--- a/lib/Chassis/src/chassis.cpp
+++ b/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 rightDelta = (float) rightMotor.speed * (float) CONTROL_LOOP_PERIOD_MS / 1000.0;
+    float leftDelta = (float) leftMotor.speed 
+        * (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 
-        / (3.14 * ROBOT_RADIUS);
+        / (2.0 * ROBOT_RADIUS);
 
-    delta.u = distance;
-    delta.omega = deltaHeading;
+    delta.x = distance;
+    delta.theta = deltaHeading;
 
 
     /**
@@ -163,8 +165,8 @@ Twist Chassis::CalcOdomFromWheelMotion(void)
      */
 
 #ifdef __NAV_DEBUG__
-    TeleplotPrint("u", delta.u);
-    TeleplotPrint("omega", delta.omega);
+    TeleplotPrint("u", delta.x);
+    TeleplotPrint("omega", delta.theta);
 #endif
 
     return delta;
diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h
index 0a10287..51bb0e0 100644
--- a/lib/Chassis/src/chassis.h
+++ b/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);
diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp
index b30d40a..878b78b 100644
--- a/src/robot-nav.cpp
+++ b/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.y += twist.u * sin(transitHeading);
+    currPose.x += delta.x * cos(transitHeading);
+    currPose.y += delta.x * sin(transitHeading);
+
+    currPose.theta += delta.theta;
 
 #ifdef __NAV_DEBUG__
     TeleplotPrint("x", currPose.x);
diff --git a/src/robot.cpp b/src/robot.cpp
index 4a39fc8..cf064a8 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -27,11 +27,11 @@ void Robot::RobotLoop(void)
      /**
      * Run the chassis loop, which handles low-level control.
      */
-    Twist velocity;
-    if(chassis.ChassisLoop(velocity))
+    Pose delta;
+    if(chassis.ChassisLoop(delta))
     {
         // We do FK regardless of state
-        UpdatePose(velocity);
+        UpdatePose(delta);
         chassis.SetMotorEfforts(220,-220);
         
         /**
diff --git a/src/robot.h b/src/robot.h
index 40081bd..494f51b 100644
--- a/src/robot.h
+++ b/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);