.idea/misc.xml

@@ -1,7 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="Black">
-    <option name="sdkName" value="Python 3.11 (pid-balancer)" />
-  </component>
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.11 (pid-balancer)" project-jdk-type="Python SDK" />
-</project>

control_functions.py

@@ -1,172 +1,75 @@
-from adafruit_hcsr04 import HCSR04 as hcsr04        # PWM driver board for servo
+from adafruit_hcsr04 import HCSR04 as hcsr04
-import board                                        # PWM driver board for servo
+import board
-from adafruit_servokit import ServoKit              # Servo libraries
+import data_transfer_functions as dt
-import adafruit_pcf8591.pcf8591 as PCF              # AD/DA converter board for potentiometer
+from adafruit_servokit import ServoKit
-from adafruit_pcf8591.analog_in import AnalogIn     # Analogue in pin library
+import numpy as np
-from adafruit_pcf8591.analog_out import AnalogOut   # Analogue out pin library
+import matplotlib.pyplot as plt
-import adafruit_pcf8591 as pcf8591                  # AD/DA converter board for potentiometer
+from scipy.integrate import odeint
-import numpy as np                                  # Number handling
+import numpy as np
-import pandas as pd                                 # Data handling
+import matplotlib.pyplot as plt
-import matplotlib.pyplot as plt                     # Plotter handling
-from scipy.integrate import odeint                  # Integral calculations
-import statistics as st                             # Mean and median calculations
-import csv                                          # CSV handling
-from datetime import datetime                       # Date and time formatting
-import time                                         # Time formatting
 
-from serial.tools.list_ports_osx import kCFNumberSInt8Type
-
-######################################## Variables (start) ##################################
 # Variables to control sensor
-TRIGGER_PIN = board.D4      # GPIO pin xx
+TRIGGER_PIN = board.D4
-ECHO_PIN = board.D17        # GPIO pin xx
+ECHO_PIN = board.D17
-TIMEOUT: float = 0.1        # Timout for echo wait
+TIMEOUT = 0.1
-MIN_DISTANCE: int = 4       # Minimum sensor distance to considered valid
+MIN_DISTANCE = 4
-MAX_DISTANCE: int = 40      # Maximum sensor distance to considered valid
+MAX_DISTANCE = 40
-
-# Variables to control servo
-KIT = ServoKit(channels=16) # Define the type of board (8, 16)
-MIN_PULSE = 500             # Defines angle 0
-MAX_PULSE = 2500            # Defines angle 180
-KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
 
 # Variables to control logging.
-LOG: bool = True            # Log data to files
+LOG = True
-SCREEN: bool = True         # Log data to screen
+SCREEN = True
-DEBUG: bool = False         # More data to display
 
-# Variables to assist PID calculations
+# PID measurements
-current_time: float = 0
+time = 0
-integral: float = 0
+integral = 0
-time_prev: float  = -1e-6
+time_prev = -1e-6
-error_prev: float = 0
+e_prev = 0
 
-# Variables to control PID values (PID formula tweaks)
-p_value: float = 2
-i_value: float = 0
-d_value: float = 0
-
-# Initial variables, used in pid_calculations()
-i_result: float = 0
-previous_time: float
-previous_error: float
-
-# Init array, used in read_distance_sensor()
-sample_array: list = []
-
-######################################## Variables (end) ##################################
 
 def initial():
     ...
 
-# Create timestamp strings for logs and screen
-def time_stamper():
-    log_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
-    file_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M')
-    return log_timestamp, file_timestamp
-
-# Write data to any of the logfiles
-def log_data(fixed_file_stamp: str, data_file: str, data_line: float, remark: str|None):
-    log_stamp, _ = time_stamper()
-
-    with open("pid-balancer_" + data_file + "_data_" + fixed_file_stamp + ".csv", "a") as data_file:
-        data_writer  = csv.writer(data_file)
-        data_writer.writerow([log_stamp,data_line, remark])
 
 def read_distance_sensor(fixed_file_stamp):
 
     with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar):
-        samples: int = 0
+
-        max_samples: int = 10
+        while True:
-        timestamp_last: float = 0.0
-        timestamp_first: float = 0.0
-        while samples != max_samples:
             try:
-                distance: float = sonar.distance
+                distance = sonar.distance
                 if MIN_DISTANCE < distance < MAX_DISTANCE:
 
-                    log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None
+                    dt.log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None
-                    print("Distance: ", distance) if SCREEN else None
+                    print("Distance: ", sonar.distance) if SCREEN else None
-
-                    sample_array.append(distance)
-                    if samples == 0: timestamp_first, _ = time_stamper()
-                    if samples == max_samples - 1: timestamp_last, _ = time_stamper()
-
-                    timestamp_first_float: float = float(timestamp_first)
-                    timestamp_last_float: float = float(timestamp_last)
-                    samples: int = samples + 1
-                    median_distance: list = st.median(sample_array)
-                    mean_timestamp: float  = st.mean([timestamp_first_float, timestamp_last_float])
-                    print(median_distance) if SCREEN else None
-                    print(mean_timestamp) if SCREEN else None
-
                 else:
-                    log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG else None
+                    dt.log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG else None
                     print("Distance: ", distance) if SCREEN else None
 
             except RuntimeError:
-                log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG and DEBUG else None
+                dt.log_data(fixed_file_stamp, "sensor", 999.999, "Timeout") if LOG else None
                 print("Timeout") if SCREEN else None
 
-    return median_distance, mean_timestamp
-
 def read_setpoint():
-
+    ...
-
-    ############# AnalogOut & AnalogIn Example ##########################
-    #
-    # This example shows how to use the included AnalogIn and AnalogOut
-    # classes to set the internal DAC to output a voltage and then measure
-    # it with the first ADC channel.
-    #
-    # Wiring:
-    # Connect the DAC output to the first ADC channel, in addition to the
-    # normal power and I2C connections
-    #
-    #####################################################################
-    i2c = board.I2C()
-    pcf = PCF.PCF8591(i2c)
-
-    pcf_in_0 = AnalogIn(pcf, PCF.A0)
-    pcf_out = AnalogOut(pcf, PCF.OUT)
-
-    while True:
-        print("Setting out to ", 65535)
-        pcf_out.value = 65535
-        raw_value = pcf_in_0.value
-        scaled_value = (raw_value / 65535) * pcf_in_0.reference_voltage
-
-        print("Pin 0: %0.2fV" % (scaled_value))
-        print("")
-        time.sleep(1)
 
 
 def calculate_velocity():
-    ...
 
-def pid_calculations(setpoint):
 
-    global i_result, previous_time, previous_error
-    offset_value: int = 320
-    measurement, current_time = read_distance_sensor
-    error: float = setpoint - measurement
-    error_sum: float = 0.0
 
-    if previous_time is None:
+def PID_calculations(Kp, Ki, Kd, setpoint, measurement):
-        previous_error: float = 0.0
-        previous_time: float = current_time
-        i_result: float = 0.0
-        error_sum: float = error * 0.008 # sensor sampling number approximation.
 
-    error_sum: float = error_sum +  (error * (current_time - previous_time))
+    global cur_time, integral, prev_time, prev_error
-    p_result: float = p_value * error
+    offset_value = 320
-    i_result: float =  i_value * error_sum
+    # PID calculations
-    d_result: float = d_value * ((error - previous_error) / (current_time - previous_time))
+    error = setpoint - measurement
-    pid_result: float = offset_value + p_result + i_result + d_result
+    P = Kp * error
-    previous_error: float = error
+    integral = integral + Ki * error * (cur_time - prev_time)
-    previous_time: float = current_time
+    D = Kd * (error - prev_error) / (cur_time - prev_time)
-
+    PID_result = offset_value + P + integral + D
-    return pid_result
+    prev_error = error
+    prev_time = cur_time
+    return PID_result
 
 
 def calculate_new_servo_pos():
@@ -175,5 +78,8 @@ def calculate_new_servo_pos():
 
 def send_data_to_servo():
 
-    KIT.servo[0].angle = 180 # Set angle
+    kit = ServoKit(channels=16)
+    kit.servo[0].set_pulse_width_range(500, 2500)
+
+    kit.servo[0].angle = 180
 

data_transfer_functions.py

@@ -0,0 +1,16 @@
+import csv
+from datetime import datetime
+
+# Create timestamp strings for logs and screen
+def timestamper():
+    log_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
+    file_timestamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M')
+    return log_timestamp, file_timestamp
+
+# Write data to any of the logfiles
+def log_data(fixed_file_stamp: str, data_file: str, data_line: float, remark: str|None):
+    log_stamp, _ = timestamper()
+
+    with open("pid-balancer_" + data_file + "_data_" + fixed_file_stamp + ".csv", "a") as data_file:
+        data_writer = csv.writer(data_file)
+        data_writer.writerow([log_stamp,data_line, remark])

main.py

@@ -1,14 +1,14 @@
 import control_functions as cf
+import data_transfer_functions as dt
 import plotter_functions as pf
 import numpy as np
 import matplotlib.pyplot as plt
 from scipy.integrate import odeint
 import numpy as np
 import matplotlib.pyplot as plt
-import statistics as st
+import data_transfer_functions as dt
-from adafruit_hcsr04 import HCSR04 as hcsr04
 
-_, fixed_file_stamp = cf.time_stamper()
+_, fixed_file_stamp = dt.timestamper()
 
 
 cf.read_distance_sensor(fixed_file_stamp)