diff --git a/.idea/misc.xml b/.idea/misc.xml
index e158a33..116cb80 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -3,5 +3,5 @@
-
+
\ No newline at end of file
diff --git a/control_functions.py b/control_functions.py
index 4cbe1d1..2239d47 100644
--- a/control_functions.py
+++ b/control_functions.py
@@ -1,6 +1,6 @@
-from adafruit_hcsr04 import HCSR04 as hcsr04 # PWM driver board for servo
-import board # PWM driver board for servo
-from adafruit_servokit import ServoKit # Servo libraries
+from adafruit_hcsr04 import HCSR04 as hcsr04 # Ultrasound sensor
+import board # General board pin mapper
+from adafruit_servokit import ServoKit # Servo libraries for PWM driver board
import adafruit_pcf8591.pcf8591 as PCF # AD/DA converter board for potentiometer
from adafruit_pcf8591.analog_in import AnalogIn # Analogue in pin library
from adafruit_pcf8591.analog_out import AnalogOut # Analogue out pin library
@@ -14,9 +14,6 @@ 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
ECHO_PIN = board.D17 # GPIO pin xx
@@ -35,6 +32,15 @@ LOG: bool = True # Log data to files
SCREEN: bool = True # Log data to screen
DEBUG: bool = False # More data to display
+# Control the number of samples for single measurement
+MAX_SAMPLES = 10
+
+# Control the number of samples for the potentiometer
+PCF_VALUE = 65535
+POT_MAX = 65280
+POT_MIN = 256
+POT_INTERVAL = 0.1
+
# Variables to assist PID calculations
current_time: float = 0
integral: float = 0
@@ -42,42 +48,36 @@ time_prev: float = -1e-6
error_prev: float = 0
# Variables to control PID values (PID formula tweaks)
-p_value: float = 2
-i_value: float = 0
-d_value: float = 0
+p_value : float = 2.0
+i_value: float = 0.0
+d_value: float = 0.0
# Initial variables, used in pid_calculations()
-i_result: float = 0
-previous_time: float
-previous_error: float
+i_result: float = 0.0
+previous_time: float = 0.0
+previous_error: float = 0.0
# 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()
+def log_data(file_stamp: str, data_file: str, data_line: float, remark: str|None):
+ log_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
- with open("pid-balancer_" + data_file + "_data_" + fixed_file_stamp + ".csv", "a") as data_file:
+ with open("pid-balancer_" + data_file + "_data_" + 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):
+def read_distance_sensor(file_stamp):
- with (hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar):
+ # Do a burst (MAX_SAMPLES) of measurements, filter out the obvious wrong ones (too short or to long distance)
+ # Return the mean timestamp and median distance.
+ with hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=TIMEOUT) as sonar:
samples: int = 0
- max_samples: int = 10
+ max_samples: int = MAX_SAMPLES
timestamp_last: float = 0.0
timestamp_first: float = 0.0
while samples != max_samples:
@@ -85,12 +85,14 @@ def read_distance_sensor(fixed_file_stamp):
distance: float = sonar.distance
if MIN_DISTANCE < distance < MAX_DISTANCE:
- log_data(fixed_file_stamp,"sensor", distance, None) if LOG else None
+ log_data(file_stamp,"sensor", distance, None) if LOG else None
print("Distance: ", 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()
+ if samples == 0: timestamp_first = float(datetime.strftime(datetime.now(),
+ '%Y%m%d%H%M%S.%f')[:-3])
+ if samples == max_samples - 1: timestamp_last = float(datetime.strftime(datetime.now(),
+ '%Y%m%d%H%M%S.%f')[:-3])
timestamp_first_float: float = float(timestamp_first)
timestamp_last_float: float = float(timestamp_last)
@@ -101,45 +103,37 @@ def read_distance_sensor(fixed_file_stamp):
print(mean_timestamp) if SCREEN else None
else:
- log_data(fixed_file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG else None
+ log_data(file_stamp,"sensor", distance,"Ignored") if LOG and DEBUG 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
+ log_data(file_stamp, "sensor", 999.999, "Timeout") if LOG and DEBUG 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)
+ pcf_out.value = PCF_VALUE
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
+ raw_value: float = pcf_in_0.value
+ scaled_value: float = (raw_value / PCF_VALUE) * pcf_in_0.reference_voltage
+ # Calculate angle in reference to raw pot values
+ angle: float = ((180 - 0) / (POT_MAX - POT_MIN)) * (raw_value - POT_MIN)
- print("Pin 0: %0.2fV" % (scaled_value))
- print("")
- time.sleep(1)
+ if SCREEN:
+ print('pin 0 ', pcf.read(0))
+ print('raw_value ',raw_value)
+ print("pin 0: %0.2fV" % scaled_value)
+ print(angle)
+ time.sleep(POT_INTERVAL)
+ send_data_to_servo(set_angle=angle)
def calculate_velocity():
...
@@ -148,32 +142,34 @@ def pid_calculations(setpoint):
global i_result, previous_time, previous_error
offset_value: int = 320
- measurement, current_time = read_distance_sensor
+ measurement, measurement_time = read_distance_sensor()
error: float = setpoint - measurement
error_sum: float = 0.0
if previous_time is None:
- 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.
+ previous_error = 0.0
+ previous_time = current_time
+ i_result = 0.0
+ error_sum = error * 0.008 # sensor sampling number approximation.
error_sum: float = error_sum + (error * (current_time - previous_time))
- p_result: float = p_value * error
- i_result: float = i_value * error_sum
- d_result: float = d_value * ((error - previous_error) / (current_time - previous_time))
- pid_result: float = offset_value + p_result + i_result + d_result
- previous_error: float = error
- previous_time: float = current_time
+ p_result = p_value * error
+ i_result = i_value * error_sum
+ d_result = d_value * ((error - previous_error) / (measurement_time - previous_time))
+ pid_result = offset_value + p_result + i_result + d_result
+ previous_error = error
+ previous_time = measurement_time
return pid_result
-def calculate_new_servo_pos():
+def calculate_new_servo_position():
...
-def send_data_to_servo():
+def send_data_to_servo(set_angle):
- KIT.servo[0].angle = 180 # Set angle
+ KIT.servo[0].angle = set_angle # Set angle
+read_distance_sensor(file_stamp=123)
+read_setpoint()
\ No newline at end of file
diff --git a/main.py b/main.py
index 2c1382d..5cca24a 100644
--- a/main.py
+++ b/main.py
@@ -1,17 +1,9 @@
+from datetime import datetime
import control_functions as cf
-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
-from adafruit_hcsr04 import HCSR04 as hcsr04
-_, fixed_file_stamp = cf.time_stamper()
+file_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%I%M')
-
-cf.read_distance_sensor(fixed_file_stamp)
+cf.read_distance_sensor(file_stamp)
diff --git a/plotter_functions.py b/plotter_functions.py
index df4dc39..b9925de 100644
--- a/plotter_functions.py
+++ b/plotter_functions.py
@@ -1,7 +1,40 @@
-def read_data_file():
- pass
+import pandas as pd
+import matplotlib.pyplot as plt
+# Variables to control logging.
+LOG: bool = True # Log data to files
+SCREEN: bool = True # Log data to screen
+DEBUG: bool = False # More data to display
-def plot_graphs():
- pass
+def read_data_file(data_file):
+ data_frame = pd.read_csv(data_file)
+ first_row_time = data_frame['Timestamp'].iloc[1]
+ last_row_time = data_frame['Timestamp'].iloc[-1]
+ first_row_value = data_frame['Value'].iloc[1]
+ last_row_value = data_frame['Value'].iloc[-1]
+ mean_value = data_frame['Value'].mean()
+ median_value = data_frame['Value'].median()
+ sum_value = data_frame['Value'].sum()
+ if SCREEN:
+ print('first_row_value ',first_row_value)
+ print('last_row_value ',last_row_value)
+ print('first_row_time ', first_row_time)
+ print('last_row_time ', last_row_time)
+ print('elapsed_time ', (last_row_time - first_row_time))
+ print('mean_value ', mean_value)
+ print('median_value ', median_value)
+ print('sum_value ', sum_value)
+
+ return data_frame
+
+def plot_data_frame(data_file):
+
+ data_frame = read_data_file(data_file)
+ plt.plot(data_frame['Timestamp'], data_frame['Value'])
+ # plt.savefig(data_file + '.png')
+ # img = plt.imread(data_file + '.png')
+ # plt.imshow(img)
+ plt.show()
+
+ plot_data_frame(data_file = 'pid-balancer_twin_test_data.csv')
\ No newline at end of file