1 files changed, 116 insertions, 0 deletions
diff --git a/debug/motordriver/PID/PID.ino b/debug/motordriver/PID/PID.ino
new file mode 100644
index 0000000..313b440
--- /dev/null
+++ b/debug/motordriver/PID/PID.ino
@@ -0,0 +1,116 @@
+#include <TimerThree.h>
+#include "ArduPID.h"
+
+ArduPID myController;
+
+double input;
+double output;
+double setpoint = 950; // Desired RPM
+double p = 40; // PID P-term
+double i = 2;  // PID I-term
+double d = 0; // PID D-term
+
+int enA = 1;  // PWM pin
+int enB = 2;  // GND
+int pwm1 = 3; // Motor control pin 1
+int pwm2 = 4; // Motor control pin 2
+int encoA = 5; // Encoder pin A
+int encoB = 6; // Encoder pin B
+
+double sp = 0.00;
+double rpm = 0.00;
+volatile long encount = 0;
+unsigned long Ltime = 0;
+unsigned long Ctime = 0;
+unsigned long Ptime = 0;
+
+volatile int PWMval = 0;   // PWM value 0-1000
+
+void setup() {
+  pinMode(enA, OUTPUT);
+  pinMode(enB, OUTPUT);
+  pinMode(pwm1, OUTPUT);
+  pinMode(pwm2, OUTPUT);
+
+  pinMode(encoA, INPUT_PULLUP);
+  pinMode(encoB, INPUT_PULLUP);
+
+  attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE);
+
+  Timer3.initialize(800); // 800 microseconds or 1.25kHz
+  Timer3.attachInterrupt(updatePWM); 
+
+  Serial.begin(9600);
+
+  // PID settings
+  myController.begin(&input, &output, &setpoint, p, i, d);
+  myController.setOutputLimits(0, 1000); // Output limits for PWM control
+  myController.setBias(500); // Starting bias for PWM
+  myController.setWindUpLimits(-50, 50);
+
+  myController.start();
+}
+
+void updatePWM() {
+  static int counter = 0;
+
+  if (counter < PWMval) {
+    digitalWrite(enA, HIGH);
+  } else {
+    digitalWrite(enA, LOW);
+  }
+
+  counter++;
+  if (counter >= 1000) {
+    counter = 0;
+  }
+}
+
+void mcpsoftpwm(int value) {
+  if (value < 0) value = 0;
+  if (value > 1000) value = 1000;
+
+  PWMval = value; 
+}
+
+void encoderISR() {
+  encount++;
+}
+
+void readenc() {
+  Ctime = micros();
+  Ptime = Ctime - Ltime;
+  Ltime = Ctime;
+  rpm = (encount / 211.2) * (60000000.0 / Ptime); // Adjust the encoder counts per revolution here if necessary
+  encount = 0;
+}
+
+void resetenc() {
+  Ctime = micros();
+  Ltime = Ctime;
+  encount = 0;
+}
+
+void move(int sped) {
+  sp = abs(sped) * 0.1;
+  mcpsoftpwm(sp);
+  digitalWrite(enB, LOW);
+  if (sped < 0) {
+    digitalWrite(pwm1, LOW);
+    digitalWrite(pwm2, HIGH);
+  } else {
+    digitalWrite(pwm1, HIGH);
+    digitalWrite(pwm2, LOW);
+  }
+}
+
+void loop() {
+  readenc();
+  input = rpm;
+  myController.compute();
+  myController.debug(&Serial, "myController", PRINT_INPUT);
+  
+  move(output); // Replace with plant control signal
+  delay(100);
+}