Testing new angle inversion

alexDickhans · Aug 23, 2024 · a09d98a · a09d98a
1 parent 8368e38
commit a09d98a
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git 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
@@ -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,
             })
         }
     }