Feature/magnetometer class

.gitignore

@@ -5,4 +5,5 @@
 .vscode/settings.json
 .vscode/ipch
 env.h
-/.history
+/.history
+.DS_Store

include/robot/control.h

@@ -20,8 +20,8 @@ struct ControlSetting
 static ControlSetting leftMotorControl;
 static ControlSetting rightMotorControl;
 
-static MotionProfile profileA = {MAX_VELOCITY_TPS, MAX_ACCELERATION_TPSPS, 0, 0, 0, 0, 75.0}; // maxVelocity, maxAcceleration, currentPosition, currentVelocity, targetPosition, targetVelocity
-static MotionProfile profileB = {MAX_VELOCITY_TPS, MAX_ACCELERATION_TPSPS, 0, 0, 0, 0, 75.0}; // maxVelocity, maxAcceleration, currentPosition, currentVelocity, targetPosition, targetVelocity
+static MotionProfile profileA = {THEORETICAL_MAX_VELOCITY_TPS, THEORETICAL_MAX_ACCELERATION_TPSPS, 0, 0, 0, 0, 75.0}; // maxVelocity, maxAcceleration, currentPosition, currentVelocity, targetPosition, targetVelocity
+static MotionProfile profileB = {THEORETICAL_MAX_VELOCITY_TPS, THEORETICAL_MAX_ACCELERATION_TPSPS, 0, 0, 0, 0, 75.0}; // maxVelocity, maxAcceleration, currentPosition, currentVelocity, targetPosition, targetVelocity
 
 void setLeftMotorControl(ControlSetting control);
 void setRightMotorControl(ControlSetting control);

include/robot/magnet.h

@@ -0,0 +1,35 @@
+#ifndef MAGNET_H
+#define MAGNET_H
+
+#include "Arduino.h"
+#include "DFRobot_BMM350.h"
+
+struct MagnetReading {
+    float x;
+    float y;
+    float z;
+};
+
+class Magnet {
+    public:
+        Magnet();
+        void set_hard_iron_offset(float x, float y, float z);
+        void set_soft_iron_matrix(float matrix[3][3]);
+        struct MagnetReading read_calibrated_data();
+        float getCompassDegree(struct MagnetReading mag);
+        float readDegreesRaw();
+        float readDegrees();
+        bool isDataReady() { return bmm350.getDataReadyState(); }
+    private:
+        float hard_iron_offset[3] = { -23.71, -5.45, -8.27 };
+        float soft_iron_matrix[3][3] = {
+            { 1.017, -0.024, 0.023 },
+            { -0.024, 0.994, 0.002 },
+            { 0.023, 0.002, .991 }
+        };
+        DFRobot_BMM350_I2C bmm350;
+
+        float previousReading = 0.0;
+};
+
+#endif // MAGNET_H

include/robot/profiledPIDController.h

@@ -0,0 +1,40 @@
+#ifndef PROFILED_PID_CONTROLLER_H
+#define PROFILED_PID_CONTROLLER_H
+
+#include "robot/pidController.h"
+#include "robot/trapezoidalProfileNew.h"
+
+class ProfiledPIDController {
+public:
+    ProfiledPIDController(double kp, double ki, double kd,
+                          double minOutput, double maxOutput,
+                          const TrapezoidProfile::Constraints& constraints)
+        : pid(kp, ki, kd, minOutput, maxOutput), profile(constraints), lastTime(0.0) {}
+
+    // Call this every control loop
+    double Compute(double goalPosition, double actualPosition, double actualVelocity, double dt) {
+        TrapezoidProfile::State current(actualPosition, actualVelocity);
+        TrapezoidProfile::State goal(goalPosition, 0.0); // Assume goal velocity is zero
+
+        // Generate profile for current time
+        TrapezoidProfile::State profiledSetpoint = profile.calculate(lastTime, current, goal);
+
+        // PID tracks profiled position
+        double output = pid.Compute(profiledSetpoint.position, actualPosition, dt);
+
+        lastTime += dt;
+        return output;
+    }
+
+    void Reset() {
+        pid.Reset();
+        lastTime = 0.0;
+    }
+
+private:
+    PIDController pid;
+    TrapezoidProfile profile;
+    double lastTime;
+};
+
+#endif // PROFILED_PID_CONTROLLER_H

include/robot/trapezoidalProfileNew.h

@@ -0,0 +1,63 @@
+#ifndef TRAPEZOIDAL_PROFILE_NEW_H
+#define TRAPEZOIDAL_PROFILE_NEW_H
+
+#include <stdexcept>
+#include <cmath>
+
+class TrapezoidProfile
+{
+public:
+    struct Constraints
+    {
+        double maxVelocity;
+        double maxAcceleration;
+
+        Constraints(double maxVelocity, double maxAcceleration)
+        {
+            if (maxVelocity < 0.0 || maxAcceleration < 0.0)
+            {
+                throw std::runtime_error("Constraints must be non-negative");
+            }
+            this->maxVelocity = maxVelocity;
+            this->maxAcceleration = maxAcceleration;
+            // Remove MathSharedStore.reportUsage for now (Java-specific)
+        }
+    };
+
+    struct State
+    {
+        double position = 0.0;
+        double velocity = 0.0;
+
+        State() = default;
+        State(double position, double velocity)
+            : position(position), velocity(velocity) {}
+
+        bool operator==(const State& rhs) const
+        {
+            return position == rhs.position && velocity == rhs.velocity;
+        }
+    };
+
+    TrapezoidProfile(const Constraints& constraints)
+        : m_constraints(constraints) {}
+
+    State calculate(double t, const State& current, const State& goal);
+
+    // bool isFinished(double t) const { return t >= totalTime(); }
+
+private:
+    static bool shouldFlipAcceleration(const State& initial, const State& goal)
+    {
+        return initial.position > goal.position;
+    }
+
+    State direct(const State& in, int m_direction) const
+    {
+        return State(in.position * m_direction, in.velocity * m_direction);
+    }
+
+    Constraints m_constraints;
+};
+
+#endif

include/utils/config.h

@@ -33,8 +33,11 @@ extern gpio_num_t ONBOARD_LED_PIN;
 extern int TICKS_PER_ROTATION;
 extern float TRACK_WIDTH_INCHES;
 extern float WHEEL_DIAMETER_INCHES;
-extern float MAX_VELOCITY_TPS;
-extern float MAX_ACCELERATION_TPSPS;
+extern float THEORETICAL_MAX_VELOCITY_TPS;
+extern float VELOCITY_LIMIT_TPS;
+extern float THEORETICAL_MAX_ACCELERATION_TPSPS;
+extern float ACCELERATION_LIMIT_TPSPS;
+extern float MIN_MOTOR_POWER;
 extern float TILES_TO_TICKS;
 
 extern float PID_POSITION_TOLERANCE;

include/utils/logging.h

@@ -12,6 +12,7 @@ void serialLog(std::string value, int serialLoggingLevel);
 void serialLogln(const char *message, int serialLoggingLevel);
 void serialLogln(int value, int serialLoggingLevel);
 void serialLogln(double value, int serialLoggingLevel);
+void serialLogln(float value, int serialLoggingLevel);
 void serialLogln(std::string value, int serialLoggingLevel);
 
 void serialLogError(char message[], int error);

lib/DFRobot_BMM350/LICENSE

@@ -0,0 +1,7 @@
+Copyright 2010 DFRobot Co.Ltd
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.