New trapezoid profile implementation

.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/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;