From fec0bbf709b2f35621e89a8b9ed453afd0381b2c Mon Sep 17 00:00:00 2001
From: usedhondacivic <micacer48@gmail.com>
Date: Mon, 25 May 2026 10:57:58 -0700
Subject: [PATCH] Fixes for motor rotation direction

---
 mote-firmware/src/tasks.rs            | 20 ++++++++++----------
 mote-firmware/src/tasks/drive_base.rs |  7 ++++---
 mote-firmware/src/tasks/imu/lib.rs    |  2 +-
 3 files changed, 15 insertions(+), 14 deletions(-)

diff --git a/mote-firmware/src/tasks.rs b/mote-firmware/src/tasks.rs
index 6444ad6..e14067d 100644
--- a/mote-firmware/src/tasks.rs
+++ b/mote-firmware/src/tasks.rs
@@ -30,23 +30,23 @@ assign_resources! {
         usb: USB
     },
     left_encoder: LeftEncoderResources{
-        phase_a: PIN_6,
-        phase_b: PIN_7,
-    },
-    right_encoder: RightEncoderResources{
         phase_a: PIN_8,
         phase_b: PIN_9,
     },
+    right_encoder: RightEncoderResources{
+        phase_a: PIN_6,
+        phase_b: PIN_7,
+    },
     encoder_driver: EncoderDriverResources{
         pio: PIO1,
     },
     drv8833_resources: DRV8833Resources{
-        left_pwm: PWM_SLICE5,
-        left_a: PIN_10,
-        left_b: PIN_11,
-        right_pwm: PWM_SLICE6,
-        right_a: PIN_12,
-        right_b: PIN_13,
+        left_pwm: PWM_SLICE6,
+        left_a: PIN_12,
+        left_b: PIN_13,
+        right_pwm: PWM_SLICE5,
+        right_a: PIN_10,
+        right_b: PIN_11,
         sleep: PIN_18,
     },
     imu: ImuResources{
diff --git a/mote-firmware/src/tasks/drive_base.rs b/mote-firmware/src/tasks/drive_base.rs
index 10b2452..d0b5aaf 100644
--- a/mote-firmware/src/tasks/drive_base.rs
+++ b/mote-firmware/src/tasks/drive_base.rs
@@ -102,9 +102,10 @@ impl<'d, T: SetDutyCycle, P: Instance, const SM: usize> Motor<'d, T, P, SM> {
         }
 
         // Update the joint state
-        self.joint_state.postition_rad = encoder_pulses_to_rad(self.encoder_value);
-        self.joint_state.velocity_rad_per_s = measurement / dt;
-        self.joint_state.effort_percent = deadband_adjusted_output;
+        // Negated to convert rotation direction to right hand coordinate system
+        self.joint_state.postition_rad = -(encoder_pulses_to_rad(self.encoder_value));
+        self.joint_state.velocity_rad_per_s = -(measurement / dt);
+        self.joint_state.effort_percent = -deadband_adjusted_output;
     }
 }
 
diff --git a/mote-firmware/src/tasks/imu/lib.rs b/mote-firmware/src/tasks/imu/lib.rs
index 0a20039..0ad330a 100644
--- a/mote-firmware/src/tasks/imu/lib.rs
+++ b/mote-firmware/src/tasks/imu/lib.rs
@@ -35,7 +35,7 @@ const CHIP_ID: u8 = 0x69; // FOR LSM6DS33
 const CHIP_ID_TRC: u8 = 0x6A; // FOR LSM6DS3-TRC, DIFFERENT FROM LSM6DS33 WHOAMI
 
 // Earth gravity constant for acceleration conversion
-const EARTH_GRAVITY: f32 = -9.80665;
+const EARTH_GRAVITY: f32 = 9.80665;
 
 /// 6-DoF IMU accelerometer + gyro
 pub struct Lsm6ds3TRC<I2C> {