Full refactor

.gitignore

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

env.h.schema

@@ -4,7 +4,7 @@
 #define WIFI_SSID "RoboNet-Legacy"
 #define WIFI_PASSWORD ""
 
-#define RUN_OFFLINE false
+#define ONLINE false
 
 #define STARTUP_DELAY 0
 
@@ -16,7 +16,7 @@
 #define PING_TIMEOUT 1000
 #define PING_MAX_MISSES 2
 
-// The amount of serialLogging to do
+// The amount of debugging to do
 // 0: None
 // 1: Error Logging (Logs only when an error occurs)
 // 2: General Logging (Logs when important things happen)

include/robot/lights.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <Arduino.h>
+
+class Light {
+    public:
+        Light(gpio_num_t pin);
+
+        void tick();
+
+        short raw_value();
+        bool value();
+        bool held_value();
+        void reset();
+
+        bool changed_this_tick();
+        unsigned long last_changed_time();
+
+    private:
+        gpio_num_t pin;
+
+        short _raw_value;
+
+        bool _value;
+        bool _held_value;
+
+        bool _changed_this_tick;
+        unsigned long _last_changed_time;
+};
+
+void setupIR();
+void activateIR();
+void deactivateIR();

include/robot/motion-controller.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include <optional>
+#include <string>
+
+#include "robot/pid.h"
+#include "utils/geometry.h"
+
+enum CenteringStatus {
+    NOT_CENTERING,
+
+    // The robot has started centering
+    STARTED,
+
+    // Has hit the first edge, and is now aligning to it
+    ALIGNING_EDGE_1,
+
+    ALIGNED_EDGE_1,
+
+    CENTERED_Y_AXIS,
+
+    ALIGNED_EDGE_2,
+};
+
+class MotionController {
+    public:
+        enum MotionPhase {
+            // Traveling to the destination
+            TRAVELLING,
+
+            // Aligning to the correct rotation
+            ALIGNING,
+            ARRIVED
+        };
+
+        MotionController();
+
+        MotionPhase phase();
+        void set_goal(Coordinate2D goal_destination, double goal_angle, std::optional<std::string> id);
+        void tick(uint32_t delta);
+
+        void print_status();
+        void reset();
+    private:
+        std::optional<std::string> actionID;
+
+        PIDController DistVelocityController; 
+        PIDController AVelocityController;
+
+        MotionPhase _phase;
+        MotionPhase _prev_phase;
+
+        double goal_angle;
+        Coordinate2D goal_position;
+};

include/robot/motor.h

@@ -1,8 +1,55 @@
-#ifndef CHESSBOT_MOTOR_H
-#define CHESSBOT_MOTOR_H
+#pragma once
 
-void setupMotors();
-void setLeftPower(float power);
-void setRightPower(float power);
+#include <Encoder.h>
+#include <stdint.h>
 
-#endif
+#include "robot/pid.h"
+#include "utils/functions.h"
+#include "utils/config.h"
+
+
+const double TIRE_RADIUS = 5.9;
+const double TIRE_CIRCUMFERENCE = M_PI * 2 * TIRE_RADIUS;
+
+class Motor {
+    public:
+        Motor(bool inverted, int motor_pin_a, int motor_pin_b, uint8_t enc_pin_a, uint8_t enc_pin_b);
+
+        void tick(uint32_t delay);
+
+        /* Read-only stats */ 
+
+        int duty();
+        double power();
+        double speed();
+        double target_speed();
+        double dist();      // Total distance in cm
+        double tick_dist(); // Distance this tick in cm
+        int32_t raw_dist(); // Read the raw encoder value in ticks
+        double get_saved();
+
+        /* Affect the motor */
+
+        double save_dist();
+        void encoder_reset();
+        void set_power(double power);
+        void set_target_speed(double speed);
+
+    private:
+        int pin_a;
+        int pin_b;
+
+        bool inverted;
+
+        PIDController speed_controller;
+        double _power;
+        RingBuf _speeds;
+        double _target_speed;
+
+        int32_t raw_enc_value;
+        int32_t prev_raw_enc_value;
+        double saved_dist;
+        Encoder encoder;
+};
+
+int power_to_duty(double power);

include/robot/pid.h

@@ -0,0 +1,46 @@
+#pragma once
+
+class PIDController
+{
+public:
+    // PIDController(double kp, double ki, double kd, double min, double max);
+    PIDController(double kp, double ki, double kd, double min, double max, double errorTolerance);
+
+    double Compute(double setpoint, double actual_value, double dt);
+    void Reset();
+
+    double kp, ki, kd;           // PID gains
+    double minOutput, maxOutput; // Output limits
+
+    double prev_error, prev_velocity_error; // Previous error
+    double errorTolerance; // Allowed error before returning 0
+    double integral;   // Integral accumulator
+
+protected:
+    virtual double getError(double setpoint, double actual_value) { return setpoint - actual_value; }
+};
+// Manually making the clamp function
+// template <typename T>
+// T clamp(T value, T minValue, T maxValue) {
+//     return (value < minValue) ? minValue : (value > maxValue) ? maxValue : value;
+// }
+
+class ContinuousPIDController : public PIDController
+{
+public:
+    ContinuousPIDController(double kp, double ki, double kd, double min, double max, double errorTolerance, double minInput, double maxInput)
+        : PIDController(kp, ki, kd, min, max, errorTolerance), minInput(minInput), maxInput(maxInput) {}
+protected:
+    double getError(double setpoint, double actual_value) override {
+        double error = setpoint - actual_value;
+        double range = maxInput - minInput;
+        if (error > range / 2) {
+            error -= range;
+        } else if (error < -range / 2) {
+            error += range;
+        }
+        return error;
+    }
+private:
+    double minInput, maxInput; // Input range for continuous wrapping
+};