From 6302520df8c4c38d970a78bad0316ce2fbe8f902 Mon Sep 17 00:00:00 2001
From: mattmcw <matt@sixteenmillimeter.com>
Date: Mon, 21 Apr 2025 12:20:13 -0400
Subject: [PATCH] Fix comments in PID code

---
 ino/contact_printer/PIDMotorController.cpp | 77 ++++++++++++++++++++
 ino/contact_printer/PIDMotorController.h   | 84 ++++++++++++++++++++++
 2 files changed, 161 insertions(+)
 create mode 100644 ino/contact_printer/PIDMotorController.cpp
 create mode 100644 ino/contact_printer/PIDMotorController.h

diff --git a/ino/contact_printer/PIDMotorController.cpp b/ino/contact_printer/PIDMotorController.cpp
new file mode 100644
index 0000000..8b9e6a4
--- /dev/null
+++ b/ino/contact_printer/PIDMotorController.cpp
@@ -0,0 +1,77 @@
+#include "PIDMotorController.h"
+
+PIDMotorController::PIDMotorController(double kp, double ki, double kd, uint32_t sampleTimeMs)
+    : m_kp(kp), m_ki(ki), m_kd(kd),
+      m_targetRPM(0.0), m_lastError(0.0), m_integralSum(0.0),
+      m_sampleTimeMs(sampleTimeMs), m_firstUpdate(true) {
+}
+
+void PIDMotorController::setTargetRPM(double targetRPM) {
+    m_targetRPM = targetRPM;
+}
+
+double PIDMotorController::getTargetRPM() const {
+    return m_targetRPM;
+}
+
+uint16_t PIDMotorController::update(double currentRPM) {
+    unsigned long currentTime = millis();
+    
+    if (m_firstUpdate) {
+        m_firstUpdate = false;
+        m_lastTime = currentTime;
+        m_lastError = m_targetRPM - currentRPM;
+        return 0;
+    }
+    
+    unsigned long deltaTime = currentTime - m_lastTime;
+    
+    if (deltaTime >= m_sampleTimeMs) {
+        double error = m_targetRPM - currentRPM;
+        double pTerm = m_kp * error;
+        m_integralSum += error * deltaTime / 1000.0;
+        double iTerm = m_ki * m_integralSum;
+        double dTerm = 0.0;
+        if (deltaTime > 0) {
+            double derivativeError = (error - m_lastError) / (deltaTime / 1000.0);
+            dTerm = m_kd * derivativeError;
+        }
+        
+        double output = pTerm + iTerm + dTerm;
+        m_lastError = error;
+        m_lastTime = currentTime;
+        int16_t pwmValue = static_cast<int16_t>(output);
+        
+        return constrain(pwmValue, MIN_PWM, MAX_PWM);
+    }
+    
+    // If not enough time has passed, return the last calculated value
+    // Here we approximate by using the last error
+    double pTerm = m_kp * m_lastError;
+    double iTerm = m_ki * m_integralSum;
+    double output = pTerm + iTerm;
+    
+    return constrain(static_cast<int16_t>(output), MIN_PWM, MAX_PWM);
+}
+
+void PIDMotorController::setTunings(double kp, double ki, double kd) {
+    if (kp < 0 || ki < 0 || kd < 0) {
+        return;
+    }
+    
+    m_kp = kp;
+    m_ki = ki;
+    m_kd = kd;
+}
+
+void PIDMotorController::setSampleTime(uint32_t sampleTimeMs) {
+    if (sampleTimeMs > 0) {
+        m_sampleTimeMs = sampleTimeMs;
+    }
+}
+
+void PIDMotorController::reset() {
+    m_integralSum = 0.0;
+    m_lastError = 0.0;
+    m_firstUpdate = true;
+}
\ No newline at end of file
diff --git a/ino/contact_printer/PIDMotorController.h b/ino/contact_printer/PIDMotorController.h
new file mode 100644
index 0000000..c76e14b
--- /dev/null
+++ b/ino/contact_printer/PIDMotorController.h
@@ -0,0 +1,84 @@
+#ifndef PID_MOTOR_CONTROLLER_H
+#define PID_MOTOR_CONTROLLER_H
+
+#include <Arduino.h>
+#include <stdint.h>
+
+class PIDMotorController {
+public:
+    /**
+     * Constructor for PID Motor Controller
+     * 
+     * @param kp Proportional gain coefficient
+     * @param ki Integral gain coefficient
+     * @param kd Derivative gain coefficient
+     * @param sampleTimeMs Time between PID calculations in milliseconds
+     */
+    PIDMotorController(double kp = 1.0, double ki = 0.1, double kd = 0.01, uint32_t sampleTimeMs = 20);
+
+    /**
+     * Sets the desired motor speed
+     * 
+     * @param targetRPM Desired motor speed in RPM
+     */
+    void setTargetRPM(double targetRPM);
+
+    /**
+     * Gets the current target RPM
+     * 
+     * @return Current target RPM
+     */
+    double getTargetRPM() const;
+
+    /**
+     * Updates the PID controller with current encoder reading
+     * 
+     * @param currentRPM Current motor speed in RPM from encoder
+     * @return PWM value for motor (0-255)
+     */
+    uint16_t update(double currentRPM);
+
+    /**
+     * Set PID controller parameters
+     * 
+     * @param kp Proportional gain coefficient
+     * @param ki Integral gain coefficient
+     * @param kd Derivative gain coefficient
+     */
+    void setTunings(double kp, double ki, double kd);
+
+    /**
+     * Set sample time for PID calculations
+     * 
+     * @param sampleTimeMs Time between PID calculations in milliseconds
+     */
+    void setSampleTime(uint32_t sampleTimeMs);
+
+    /**
+     * Reset the PID controller (clear accumulated error)
+     */
+    void reset();
+
+private:
+    // PID const
+    double m_kp;           // Proportional gain
+    double m_ki;           // Integral gain
+    double m_kd;           // Derivative gain
+    
+    // PID var
+    double m_targetRPM;    // Desired RPM
+    double m_lastError;    // Last error value
+    double m_integralSum;  // Sum of all errors (integral)
+    
+    // Timing
+    uint32_t m_sampleTimeMs;  // Time between updates in milliseconds
+    unsigned long m_lastTime;  // Last time update was called in milliseconds
+    
+    // Output limits
+    const uint16_t MIN_PWM = 0;
+    const uint16_t MAX_PWM = 255;
+    
+    bool m_firstUpdate;
+};
+
+#endif
\ No newline at end of file