From a09d98a3be2776dfadfdece4235f0d0bc5db4dc7 Mon Sep 17 00:00:00 2001
From: Alex Dickhans <adickhans@gmail.com>
Date: Fri, 23 Aug 2024 16:32:00 -0600
Subject: [PATCH] Testing new angle inversion

---
 Cargo.toml   | 1 -
 src/mp_2d.rs | 6 +++++-
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index b0273e2..43f6e99 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -11,7 +11,6 @@ simba = { version = "0.9.0", default-features = false, features = [
 nalgebra = { version = "0.33.0", default-features = false, features = [
     "macros", "alloc"
 ] }
-#vexide = { git = "https://github.com/vexide/vexide.git" }
 uom = { version = "0.36.0", default-features = false, features = ["f64", "si"] }
 serde_json = { version = "^1.0.125", default-features = false, features = ["alloc"], optional = true }
 serde = { version = "^1.0.208", default-features = false, features = ["alloc", "derive"], optional = true }
diff --git a/src/mp_2d.rs b/src/mp_2d.rs
index dfea60d..beac319 100644
--- a/src/mp_2d.rs
+++ b/src/mp_2d.rs
@@ -175,12 +175,16 @@ impl MotionProfile for MotionProfile2d {
             ));
             let curvature = bezier.get_curvature(t_local);
 
+            let mut pose = bezier.get(t_local);
+
+            pose.z = inverted_multiplier * pose.z;
+
             Some(MotionCommand {
                 desired_velocity: inverted_multiplier * desired_velocity,
                 desired_angular: AngularVelocity::new::<radian_per_second>(
                     desired_velocity.get::<meter_per_second>() * curvature,
                 ),
-                desired_pose: bezier.get(t_local),
+                desired_pose: pose,
             })
         }
     }