Fix IMU getRotationDelta, fix Pose ctor

include/lemlib/chassis/chassis.hpp

@@ -86,8 +86,8 @@ class Chassis {
 
         float prevDist = 0; // the previous distance travelled by the movement
 
-        std::unique_ptr<Odom> odom;
-        std::unique_ptr<Movement> movement;
-        std::unique_ptr<pros::Task> task;
+        std::unique_ptr<Odom> odom = nullptr;
+        std::unique_ptr<Movement> movement = nullptr;
+        std::unique_ptr<pros::Task> task = nullptr;
 };
 } // namespace lemlib

include/lemlib/devices/gyro/gyro.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cstdint>
+#include <math.h>
 
 namespace lemlib {
 class Gyro {
@@ -74,6 +75,6 @@ class Gyro {
          */
         virtual std::uint8_t getPort() = 0;
     protected:
-        float lastAngle = 0;
+        float lastAngle = M_PI_2;
 };
 } // namespace lemlib

include/lemlib/odom/odom.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <math.h>
 #include "lemlib/pose.hpp"
 
 namespace lemlib {
@@ -30,6 +31,7 @@ class Odom {
          */
         void setPose(Pose pose);
     protected:
-        Pose pose = Pose(0, 0, 0);
+        // heading set to M_PI_2 because odom should default to IMU 0 angle
+        Pose pose = Pose(0, 0, M_PI_2);
 };
 } // namespace lemlib

src/lemlib/devices/gyro/imu.cpp

@@ -48,7 +48,7 @@ bool Imu::isCalibrating() const { return imu.is_calibrating(); }
  *
  * This function checks if the Imu is connected, is not calibrating,
  */
-bool Imu::isCalibrated() { return (isConnected() && !imu.is_calibrating() && !std::isinf(imu.get_heading())); }
+bool Imu::isCalibrated() { return isConnected() && !imu.is_calibrating() && !std::isinf(imu.get_heading()); }
 
 /**
  * return whether the IMU is installed
@@ -70,9 +70,9 @@ float Imu::getHeading() {
  * Get the rotation of the imu in radians and in standard position
  */
 float Imu::getRotation() {
-    const float rotation = imu.get_rotation();
+    const float rotation = M_PI_2 - degToRad(imu.get_rotation());
     lastAngle = rotation;
-    return (M_PI - degToRad(imu.get_rotation()));
+    return rotation;
 }
 
 /**

src/lemlib/pose.cpp

@@ -24,11 +24,11 @@ namespace lemlib {
  * @param y component
  * @param theta heading. Defaults to 0
  */
-Pose::Pose(float x, float y, float theta) {
-    x = x;
-    y = y;
-    theta = theta;
-}
+Pose::Pose(float x, float y, float theta)
+    :x(x),
+    y(y),
+    theta(theta)
+    {}
 
 /**
  * @brief Add a pose to this pose