3 files changed, 229 insertions, 0 deletions
diff --git a/debug/motordriver/PID_autotune/PID.py b/debug/motordriver/PID_autotune/PID.py
new file mode 100644
index 0000000..e24dbd6
--- /dev/null
+++ b/debug/motordriver/PID_autotune/PID.py
@@ -0,0 +1,131 @@
+import serial
+import time
+from simple_pid import PID
+import matplotlib.pyplot as plt
+from matplotlib.widgets import Slider, Button
+
+# Initialize serial communication
+arduino = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=.1)
+
+# Function to write to and read from Arduino
+def write_read(x):
+    arduino.write(bytes(x, 'utf-8'))
+    time.sleep(0.05)
+    data = arduino.readline().decode().strip()
+    return data
+
+# Real-time plotting setup
+fig, (ax1, ax2) = plt.subplots(2, 1)
+feedback_list = []
+control_list = []
+setpoint_list = []
+
+def clamp(feedback, min_value, max_value):
+    return max(min(feedback, max_value), min_value)
+
+def update_plot(feedback, control, setpoint):
+    feedback_clamped = clamp(feedback, 0, 1000)  # Clamp feedback value between 0 and 1000
+    feedback_list.append(feedback_clamped)
+    control_list.append(control)
+    setpoint_list.append(setpoint)
+
+    # Limit lists to 50 items for smoother plotting
+    if len(feedback_list) > 50:
+        feedback_list.pop(0)
+        control_list.pop(0)
+        setpoint_list.pop(0)
+
+    ax1.clear()
+    ax2.clear()
+
+    ax1.plot(feedback_list, label="Feedback")
+    ax1.plot(setpoint_list, label="Setpoint")
+    ax1.legend()
+
+    ax2.plot(control_list, label="Control")
+    ax2.legend()
+
+    ax1.set_ylabel('RPM')
+    ax2.set_ylabel('Control Signal')
+    ax2.set_xlabel('Time')
+
+    plt.tight_layout()
+    plt.pause(0.05)  # Pause to allow plot to update
+
+# Initialize PID controller with output limits
+initial_setpoint = 500  # Initial setpoint
+pid = PID(0, 0, 0, output_limits=(0, 1000))
+pid.setpoint = initial_setpoint
+
+# Initialize PID parameters (preserved across resets)
+preserved_parameters = {
+    'Kp': 0,
+    'Ki': 0,
+    'Kd': 0,
+    'setpoint': initial_setpoint
+}
+
+# Function to update PID parameters
+def update_pid(val):
+    pid.Kp = s_kp.val
+    pid.Ki = s_ki.val
+    pid.Kd = s_kd.val
+    preserved_parameters['Kp'] = s_kp.val
+    preserved_parameters['Ki'] = s_ki.val
+    preserved_parameters['Kd'] = s_kd.val
+    setpoint = s_setpoint.val
+    pid.setpoint = setpoint
+    preserved_parameters['setpoint'] = setpoint
+    print(f"Setpoint: {setpoint}, Kp: {pid.Kp}, Ki: {pid.Ki}, Kd: {pid.Kd}")
+
+# Create sliders for PID parameters and setpoint
+axcolor = 'lightgoldenrodyellow'
+ax_setpoint = plt.axes([0.15, 0.1, 0.65, 0.03], facecolor=axcolor)
+ax_kp = plt.axes([0.15, 0.15, 0.65, 0.03], facecolor=axcolor)
+ax_ki = plt.axes([0.15, 0.2, 0.65, 0.03], facecolor=axcolor)
+ax_kd = plt.axes([0.15, 0.25, 0.65, 0.03], facecolor=axcolor)
+
+s_setpoint = Slider(ax_setpoint, 'Setpoint', 0, 1000, valinit=initial_setpoint)
+s_kp = Slider(ax_kp, 'Kp', 0, 10, valinit=preserved_parameters['Kp'])
+s_ki = Slider(ax_ki, 'Ki', 0, 10, valinit=preserved_parameters['Ki'])
+s_kd = Slider(ax_kd, 'Kd', 0, 10, valinit=preserved_parameters['Kd'])
+
+s_setpoint.on_changed(update_pid)
+s_kp.on_changed(update_pid)
+s_ki.on_changed(update_pid)
+s_kd.on_changed(update_pid)
+
+# Function to reset algorithm and set speed to zero
+def reset_algorithm(event):
+    global feedback_list, control_list, setpoint_list
+    feedback_list.clear()
+    control_list.clear()
+    setpoint_list.clear()
+    pid.reset()
+    s_setpoint.set_val(preserved_parameters['setpoint'])
+    s_kp.set_val(preserved_parameters['Kp'])
+    s_ki.set_val(preserved_parameters['Ki'])
+    s_kd.set_val(preserved_parameters['Kd'])
+    print("Algorithm reset")
+    time.sleep(1)  # Delay of 1 second after reset
+
+# Create reset button
+ax_reset = plt.axes([0.8, 0.02, 0.1, 0.05])
+btn_reset = Button(ax_reset, 'Reset')
+btn_reset.on_clicked(reset_algorithm)
+
+# Function to update Arduino with clamped control signal
+def update_arduino_control(control):
+    control_clamped = clamp(control, 0, 1000)
+    write_read(str(control_clamped))
+
+# Control loop
+while True:
+    feedback = float(write_read(str(pid.setpoint)))
+    feedback_clamped = clamp(feedback, 0, 1000)  # Clamp feedback value between 0 and 1000
+    control = pid(feedback_clamped)
+    update_arduino_control(control)
+    update_plot(feedback_clamped, control, pid.setpoint)
+
+# Keep plot open
+plt.show()
diff --git a/debug/motordriver/PID_autotune/PID_autotune.ino b/debug/motordriver/PID_autotune/PID_autotune.ino
new file mode 100644
index 0000000..ab99c50
--- /dev/null
+++ b/debug/motordriver/PID_autotune/PID_autotune.ino
@@ -0,0 +1,82 @@
+int enA = 1;
+int enB = 2;
+int pwm1 = 3;
+int pwm2 = 4;
+int encoA = 5;
+int encoB = 6;
+
+int speed = 1; // speed 0-1000
+
+double sp = 0.00;
+double rpm = 0.00;
+volatile long encount = 0;
+unsigned long Ltime = 0;
+unsigned long Ctime = 0;
+unsigned long Ptime = 0;
+
+int mess;
+
+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);
+
+  Serial.begin(115200);
+  Serial.setTimeout(1);
+}
+
+void encoderISR() {
+  encount++;
+}
+
+void readenc() {
+  Ctime = micros();
+  Ptime = Ctime - Ltime;
+  Ltime = Ctime;
+  rpm = (encount / 211.2) * (60000000.0 / Ptime);
+  /*Serial.print("RPM: ");
+  Serial.print(rpm);
+  Serial.print("    Sample period:");
+  Serial.print(Ptime);
+  Serial.print("ms, encoder count:");
+  Serial.println(encount);
+  */
+
+  encount = 0;
+}
+void resetenc(){
+  Ctime = micros();
+  Ltime = Ctime;
+  encount = 0;
+}
+
+void move(int sped) {
+  sp = abs(sped)*2.55;
+  analogWrite(enA, sp);
+  digitalWrite(enB, LOW);
+  if (sped < 0) {
+    //Serial.print("back ");
+    digitalWrite(pwm1, LOW);
+    digitalWrite(pwm2, HIGH);
+  } else {
+    //Serial.print("forward ");
+    digitalWrite(pwm1, HIGH);
+    digitalWrite(pwm2, LOW);
+  } 
+  //Serial.println(sp);
+}
+
+void loop() {
+  while (!Serial.available());
+  mess = Serial.readString().toInt();
+  move(mess);
+  readenc();
+  Serial.print(rpm);
+}
diff --git a/debug/motordriver/PID_autotune/man_test.py b/debug/motordriver/PID_autotune/man_test.py
new file mode 100644
index 0000000..52c84e3
--- /dev/null
+++ b/debug/motordriver/PID_autotune/man_test.py
@@ -0,0 +1,16 @@
+import serial
+import time
+
+arduino = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=.1)
+
+def write_read(x):
+    arduino.write(bytes(x, 'utf-8'))
+    time.sleep(0.05)
+    data = arduino.readline()
+    return data
+
+while True:
+    num = input("Enter a number: ") # Taking input from user
+    value = write_read(num)
+    print(value) # printing the value
+