HU-Projects/pid-balancer
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: HU-Projects:d27de43dc821699f9558652add29243608b457db
Branches Tags
HU-Projects:master
..
compare: HU-Projects:c8a10d9deb41c6ee4610292889b39b582f7b2d8f
Branches Tags
HU-Projects:master

No commits in common. "d27de43dc821699f9558652add29243608b457db" and "c8a10d9deb41c6ee4610292889b39b582f7b2d8f" have entirely different histories.
d27de43dc8 ... c8a10d9deb

3 changed files with 128 additions and 210 deletions
Show Stats Expand all files Collapse all files

2
Examples/Adafruit_Servokit_servo_driver.py
View File

@ -21,7 +21,7 @@ kit.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
# The servo angle in degrees. Must be in the range 0 to actuation_range. # The servo angle in degrees. Must be in the range 0 to actuation_range.
# Is None when servo is disabled. # Is None when servo is disabled.
kit.servo[0].angle = 88 kit.servo[0].angle = 90

333
control_functions.py
View File

@ -1,41 +1,45 @@
from adafruit_servokit import ServoKit # Servo libraries for PWM driver board from adafruit_hcsr04 import HCSR04 as hcsr04 # Ultrasound sensor
import adafruit_pcf8591.pcf8591 as PCF # AD/DA converter board for potentiometer import board # General board pin mapper
from adafruit_pcf8591.analog_in import AnalogIn # Analogue in pin library from adafruit_servokit import ServoKit # Servo libraries for PWM driver board
from adafruit_pcf8591.analog_out import AnalogOut # Analogue out pin library import adafruit_pcf8591.pcf8591 as PCF # AD/DA converter board for potentiometer
from adafruit_hcsr04 import HCSR04 as hcsr04 # Ultrasound sensor from adafruit_pcf8591.analog_in import AnalogIn # Analogue in pin library
import board # General board pin mapper from adafruit_pcf8591.analog_out import AnalogOut # Analogue out pin library
import statistics as st # Mean and median calculations import statistics as st # Mean and median calculations
import csv # CSV handling import csv # CSV handling
from time import sleep # Sleep/pause from time import sleep # Sleep/pause
import pandas as pd # Pandas for data manipulation import pandas as pd
from datetime import datetime # Datetime for timestamps from datetime import datetime
import math # Math for particular calculations import busio
import matplotlib.pyplot as plt # Mathplotlib for graphs import adafruit_vl6180x
import math
# laser sensor controls.
# i2c = busio.I2C(board.SCL, board.SDA)
# laser = adafruit_vl6180x.VL6180X(i2c)
# Variables to control sensor # Variables to control sensor
TRIGGER_PIN = board.D4 # GPIO pin xx TRIGGER_PIN = board.D4 # GPIO pin xx
ECHO_PIN = board.D17 # GPIO pin xx ECHO_PIN = board.D17 # GPIO pin xx
PIN_TIMEOUT: float = 0.1 # Timeout for echo wait -- don't change PIN_TIMEOUT: float = 0.1 # Timeout for echo wait -- don't change
RUN_TIMEOUT: float = 0.0 # Sleep time in read_distance() function RUN_TIMEOUT: float = 0.0 # Sleep time in function
MIN_DISTANCE: int = 2 # Minimum sensor distance to be considered valid (1 on bar) MIN_DISTANCE: int = 6 # Minimum sensor distance to be considered valid (1 on bar)
MAX_DISTANCE: int = 36 # Maximum sensor distance to be considered valid (35 on bar) MAX_DISTANCE: int = 40 # Maximum sensor distance to be considered valid (35 on bar)
# Variables to control servo # Variables to control servo
KIT = ServoKit(channels=16) # Define the type of board (8, 16) KIT = ServoKit(channels=16) # Define the type of board (8, 16)
MIN_PULSE: int = 400 # Defines angle 80, for current PID setup MIN_PULSE: int = 400 # Defines angle 80, for current PID setup
MAX_PULSE: int = 2500 # Defines angle 100, for current PID setup MAX_PULSE: int = 2500 # Defines angle 100, for current PID setup
OFFSET: int = -2 # Correction nominal angle versus physical angle of the arm OFFSET: int = -1
KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE) KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
# Variables to control logging. # Variables to control logging.
LOG: bool = False # Log data to files LOG: bool = True # Log data to files
LOG_GRAPH: bool = True # Log graph creation SCREEN: bool = True # Log data to screen
SCREEN: bool = True # Log data to screen DEBUG: bool = False # More data to display
DEBUG: bool = False # More data to display TWIN_MODE: bool = True # Run in live or twin mode
TWIN_MODE: bool = True # Run in live or twin mode
# Control the number of samples for single distance measurement (average from sample burst) # Control the number of samples for single distance measurement (average from burst)
MAX_SAMPLES: int = 8 MAX_SAMPLES: int = 1
# Control the potentiometer # Control the potentiometer
# Description: # Description:
@ -60,78 +64,48 @@ pcf_out = AnalogOut(pcf, PCF.OUT)
pcf_out.value = PCF_VAL pcf_out.value = PCF_VAL
# Variables to control PID values (PID formula tweaks) # Variables to control PID values (PID formula tweaks)
p_value: float = 1.0 p_value: float = 0.5
i_value: float = 0.0 i_value: float = 0.01
d_value: float = 0.1 d_value: float = 0.0
# Initial variables, used in pid_calculations() # Initial variables, used in pid_calculations()
i_result: float = 0.0 i_result: float = 0.0
previous_time: float = 0.0 previous_time: float = 0.0
previous_error: float = 0.0 previous_error: float = 0.0
# Error sum array values # Error sum array
error_sum_max: int = 10 error_sum_max: int = 10
error_sum_array: list = [0] * error_sum_max error_sum_array: list = [0]*error_sum_max
error_sum_counter: int = 0 error_sum_counter: int = 0
# Digital twin parameters
previous_speed: float = 0.0 # Digital twin
previous_speed:float = 0.0
previous_position: float = 0.0 previous_position: float = 0.0
previous_angle: int = 90 previous_angle: int = 90
# a: acceleration
# g: gravity (9.81 m/s^2)
# theta: angle of the inclined plane
# u: coefficient of the friction between the cart and the inclined plane.
acceleration: float = 0.0
gravity: float = 9.81
friction: float = 0.05
delta_t: float = 0.2
# Maximum angle the servo can move away from steady position. With 10 the range is between 80 (-10) and 100 (+10), #maximum angle the servo can move away from steady position. With 10 the range is between 80 and 100, with steady at 90
# with steady at 90 (0) max_angle = 10
max_angle: int = 5
# Servo slower # servo slower
current_angle: int = 90 current_angle:int = 90
# Servo memory for boosting the cart if its stuck due to friction watch_variable: int = 0
servo_memory_1: int = 0
servo_memory_2: int = 0
memory_max: int = 5
# Current time of the system, used as base for file creation) # base time of the system
base_time: float = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) base_time: float = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
# Write base_time in file, to be used by other functions.
with open("pid-balancer_" + "time_file.txt", "w") as time_file:
time_file.write(datetime.strftime(datetime.now(), '%Y-%m-%d %H:%M:%S.%f')[:-3])
# Write data to any of the logfiles # Write data to any of the logfiles
def log_data(data_file: str, data_line: str, remark: str | None): def log_data(data_file: str, data_line: str, remark: str|None):
log_stamp: str = datetime.strftime(datetime.now(), '%Y-%m-%d %H:%M:%S.%f')[:-3] log_stamp: str = datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]
with open("pid-balancer_" + "time_file.txt", "r") as time_file: with open("pid-balancer_" + "time_file.txt", "r") as time_file:
file_stamp: str = time_file.readline() file_stamp: str = time_file.readline()
with open("pid-balancer_" + data_file + "_data_" + 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, delimiter=';', quoting=csv.QUOTE_MINIMAL) data_writer = csv.writer(data_file)
data_writer.writerow([log_stamp, data_line, remark]) data_writer.writerow([log_stamp,data_line, remark])
# Write data to any of the logfiles. This is specifically for one type of logfile that uses multiple data columns
def log_data2(data_file: str, data_line: str, data_line2: str | None):
log_stamp: str = datetime.strftime(datetime.now(), '%Y-%m-%d %H:%M:%S.%f')[:-3]
with open("pid-balancer_" + "time_file.txt", "r") as time_file:
file_stamp: str = time_file.readline()
with open("pid-balancer_" + data_file + "_data_" + file_stamp + ".csv", "a") as data_file:
data_writer = csv.writer(data_file, delimiter=';', quoting=csv.QUOTE_MINIMAL)
data_writer.writerow([log_stamp, data_line, data_line2])
# Function to read the SR05 ultrasound sensor data
def read_distance_sensor(): def read_distance_sensor():
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
@ -143,87 +117,78 @@ def read_distance_sensor():
with hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=PIN_TIMEOUT) as sonar: with hcsr04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN, timeout=PIN_TIMEOUT) as sonar:
samples: int = 0 samples: int = 0
max_samples: int = MAX_SAMPLES max_samples: int = MAX_SAMPLES
timestamp_last: float = 0.0 timestamp_last: float = 0.0
timestamp_first: float = 0.0 timestamp_first: float = 0.0
while samples != max_samples: while samples != max_samples:
sleep(RUN_TIMEOUT) # Fixes some sensor driver crashes sleep(RUN_TIMEOUT)
try: try:
distance: float = sonar.distance # Reading distance from the sonic sensor distance: float = sonar.distance # reading distance from the sonic sensor
# distance: float = laser.range * 10 # reading distance from the laser sensor
if MIN_DISTANCE < distance < MAX_DISTANCE: # Only process distances within expected range. if MIN_DISTANCE < distance < MAX_DISTANCE:
# This drops erroneous readings.
log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None log_data(data_file="sensor", data_line=str(distance), remark="") if LOG else None
print("Distance_in_range: ", distance) if SCREEN else None # For testing # print("Distance_in_range: ", distance) if SCREEN else None
if max_samples == 1: if max_samples == 1:
median_distance: float = distance median_distance = distance
mean_timestamp = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')[:-3]) mean_timestamp = float(datetime.strftime(datetime.now(),'%Y%m%d%H%M%S.%f')[:-3])
samples: int = samples + 1
print("Distance_in_range_rounded: ", round(distance, 4)) if SCREEN else None # For testing
else: else:
sample_array.append(distance) sample_array.append(distance)
if samples == 0: timestamp_first = float(datetime.strftime(datetime.now(), if samples == 0: timestamp_first = float(datetime.strftime(datetime.now(),
'%Y%m%d%H%M%S.%f')[:-3]) '%Y%m%d%H%M%S.%f')[:-3])
if samples == max_samples - 1: if samples == max_samples - 1:
timestamp_last = float(datetime.strftime(datetime.now(), timestamp_last = float(datetime.strftime(datetime.now(),
'%Y%m%d%H%M%S.%f')[:-3]) '%Y%m%d%H%M%S.%f')[:-3])
timestamp_first_float: float = float(timestamp_first) timestamp_first_float: float = float(timestamp_first)
timestamp_last_float: float = float(timestamp_last) timestamp_last_float: float = float(timestamp_last)
median_distance: float = st.median(sample_array) median_distance: float = st.median(sample_array)
mean_timestamp: float = st.mean([timestamp_first_float, timestamp_last_float]) mean_timestamp: float = st.mean([timestamp_first_float, timestamp_last_float])
if DEBUG: print("Distance_median: ", median_distance) if SCREEN else None
print("Distance_median: ", median_distance) print("Timestamp_mean: ", mean_timestamp) if DEBUG else None
print("Timestamp_mean: ", mean_timestamp) print("Distance_in_range: ", distance) if SCREEN else None
print("Distance_in_range: ", distance)
data_line = str(sample_array) + ',' + str(median_distance)
log_data(data_file="sensor_array", data_line=data_line, remark="")
data_line = str(sample_array) + ',' + str(median_distance)
log_data(data_file="sensor_array", data_line= data_line,
remark="") if LOG else None
print("Distance_in_range_rounded: ", round(distance, 4)) if SCREEN else None print("Distance_in_range_rounded: ", round(distance, 4)) if SCREEN else None
samples: int = samples + 1 samples: int = samples + 1
else: else:
log_data(data_file="sensor", data_line=str(distance), log_data(data_file="sensor", data_line=str(distance), remark="Distance_out_of_range") if LOG else None
remark="Distance_out_of_range") if LOG else None
print("Distance_out_of_range: ", round(distance, 4)) if SCREEN else None print("Distance_out_of_range: ", round(distance, 4)) if SCREEN else None
except RuntimeError: except RuntimeError:
log_data(data_file="sensor", data_line="999.999", remark="Timeout") if LOG and DEBUG else None log_data(data_file="sensor", data_line="999.999", remark="Timeout") if LOG and DEBUG else None
print("Distance_timed_out") if SCREEN else None print("Distance_timed_out") if SCREEN else None
# Function process time recorder
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
data_line = str(start_time - end_time) data_line = str(start_time - end_time)
log_data(data_file="function", data_line=data_line, remark="read_distance_sensor") if LOG else None log_data(data_file="function", data_line=data_line, remark="read_distance_sensor") if LOG else None
# Median distance and Mean time to log writer
data_line = str(median_distance)
data_line2 = str(mean_timestamp)
log_data2(data_file="median_sensor", data_line=data_line, data_line2=data_line2) if LOG_GRAPH else None
return median_distance, mean_timestamp return median_distance, mean_timestamp
def read_setpoint(): def read_setpoint():
# Read the resistance of the potentiometer and convert to centimeters for use with setpoint distance
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
while True: while True:
raw_value: int = pcf_in_0.value raw_value: int = pcf_in_0.value
scaled_value: float = (raw_value / PCF_VAL) * pcf_in_0.reference_voltage scaled_value: float = (raw_value / PCF_VAL) * pcf_in_0.reference_voltage
log_line = str(scaled_value) + ";" + str(raw_value) + ";" + str("angle") log_line = str(scaled_value) + "," + str(raw_value) + "," + str("angle")
log_data(data_file="potmeter", data_line=log_line, remark="") if LOG else None log_data(data_file="potmeter", data_line=log_line, remark="") if LOG else None
cm_rounded: int = int(round(scaled_value * POT_PCM, 0)) cm_rounded: int = int(round(scaled_value * POT_PCM, 0))
if DEBUG: if DEBUG:
print('Scaled_rounded = ', round(scaled_value, 4), ' CM_rounded= ', cm_rounded) print('Scaled_rounded = ' , round(scaled_value, 4), ' CM_rounded= ', cm_rounded)
print('Scaled_raw= ', scaled_value, ' CM_raw= ', int(scaled_value * POT_PCM)) print('Scaled_raw= ' , scaled_value, ' CM_raw= ', int(scaled_value * POT_PCM))
print('Setpoint in cm: ', cm_rounded) if SCREEN else None print('setpoing in cm: ', cm_rounded) if SCREEN else None
sleep(POT_INT) # Fix for driver crashes sleep(POT_INT)
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
data_line = str(start_time - end_time) data_line = str(start_time - end_time)
@ -231,69 +196,67 @@ def read_setpoint():
return cm_rounded return cm_rounded
def digital_twin(): def digital_twin():
# Digital model of the physical model. # a: acceleration
# g: gravity (9.81 m/s^2)
global previous_position, previous_speed, base_time # theta: angle of the inclined plane
# u: coefficient of the friction between the cart and the inclined plane.
acceleration: float = 0.0
global previous_position, previous_speed, base_time, watch_variable
gravity: float = 9.81
friction: float = 0.1
delta_t: float = 0.1
angle = (previous_angle - 90) angle = (previous_angle - 90)
acceleration = gravity * math.sin(math.radians(angle)) acceleration = gravity * math.sin(math.radians(angle))
friction_force = abs(friction * gravity * math.cos(math.radians(angle)) * delta_t) friction_force = friction * gravity * math.cos(math.radians(angle)) * delta_t
friction_force = abs(friction_force)
work_speed = previous_speed + acceleration * delta_t work_speed = previous_speed + acceleration * delta_t
watch_variable = watch_variable + 1
# To avoid the friction setting the work_speed to a negative value, forced the friction to be lower than the speed. if watch_variable >= 150:
if friction_force < work_speed * 0.8: print("breakpoint")
print("watch_variable", watch_variable)
if friction_force < work_speed:
if work_speed > 0: if work_speed > 0:
work_speed = work_speed - friction_force work_speed = work_speed - friction_force
elif work_speed < 0: elif work_speed < 0:
work_speed = work_speed + friction_force work_speed = work_speed + friction_force
else: else:
work_speed = work_speed work_speed = work_speed
current_speed: float = work_speed current_speed = work_speed
current_position: float = previous_position + (current_speed * delta_t)
if SCREEN: current_position = previous_position + (current_speed * delta_t)
print("Angle", angle)
print("Friction", friction)
print("Acceleration", acceleration) print("angle", angle)
print("Current speed", current_speed) print("friction", friction)
print("Current position", current_position) print("acceleration", acceleration)
print("") print("current speed", current_speed)
print("----------------------------------------------") print("current position", current_position)
print("") print("")
print("----------------")
print("")
base_time = base_time + delta_t base_time = base_time + delta_t
previous_speed = current_speed previous_speed = current_speed
previous_position = current_position previous_position = current_position
if LOG_GRAPH:
# PID position logging
data_line = str(current_position)
log_data(data_file="twin_current_position", data_line=data_line, remark="")
# PID acceleration logging
data_line = str(acceleration)
log_data(data_file="twin_acceleration", data_line=data_line, remark="")
# PID speed logging
data_line = str(current_speed)
log_data(data_file="twin_current_speed", data_line=data_line, remark="")
return current_position, base_time return current_position, base_time
def pid_calculations(): def pid_calculations():
# Do all the PID calculations and return the new angle for the servo
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
global i_result, previous_time, previous_error # Can not be annotated with :float, because variables are global. global i_result, previous_time, previous_error # Can not be annotated with :float, because variables are global.
global error_sum_counter, error_sum_array # counter for error_sum_array and error_sum_array itself global error_sum_counter, error_sum_array # counter for error_sum_array and error_sum_array itself
global previous_angle global previous_angle
offset_value: int = 0 offset_value: int = 0
if TWIN_MODE: if TWIN_MODE:
measurement, measurement_time = digital_twin() measurement, measurement_time = digital_twin()
else: else:
@ -309,23 +272,24 @@ def pid_calculations():
error_sum_array[error_sum_counter] = (error * (measurement_time - previous_time)) error_sum_array[error_sum_counter] = (error * (measurement_time - previous_time))
p_result = p_value * error p_result = p_value * error
i_result = i_value * sum(error_sum_array) i_result = i_value * sum(error_sum_array)
d_result = d_value * ((error - previous_error) / (measurement_time - previous_time)) d_result = d_value * ((error - previous_error) / (measurement_time - previous_time))
pid_result = offset_value + p_result + i_result + d_result pid_result = offset_value + p_result + i_result + d_result
previous_error = error previous_error = error
previous_time = measurement_time previous_time = measurement_time
# Code to set the max angles. Or set the angle to a specific number = pid_result * max movement + correction
if pid_result >= max_angle: #function to set the 2 max angles. Or set the angle to a specific number = pid_result * max movement + correction
if pid_result >= max_angle: # if PID result is greater than 1, set to 1. 1 = max upward angle
output_angle = (90 + max_angle) output_angle = (90 + max_angle)
elif pid_result <= -max_angle: elif pid_result <= -max_angle: # if PID result is greater than 1, set to 1. 1 = max downward angle
output_angle = (90 - max_angle) output_angle = (90-max_angle)
elif -max_angle < pid_result < max_angle: elif -max_angle < pid_result < max_angle:
output_angle = pid_result + 90 output_angle = pid_result + 90
else: else:
output_angle = 90 output_angle = 90
log_line = str(p_result) + ";" + str(i_result) + ";" + str(d_result) + ";" + str(pid_result) log_line = str(p_result) + "," + str(i_result) + "," + str(d_result) + "," + str(pid_result)
log_data(data_file="pid", data_line=log_line, remark="") if LOG else None log_data(data_file="pid", data_line=log_line, remark="") if LOG else None
if DEBUG: if DEBUG:
@ -334,12 +298,12 @@ def pid_calculations():
print("I_result: ", i_result) print("I_result: ", i_result)
print("PID_result: ", pid_result) print("PID_result: ", pid_result)
if error_sum_counter <= error_sum_max - 2: # Correction tweak for error sum if error_sum_counter <= error_sum_max-2:
error_sum_counter = error_sum_counter + 1 error_sum_counter = error_sum_counter + 1
else: else:
error_sum_counter = 0 error_sum_counter = 0
print("error sum counter", error_sum_counter) if DEBUG else None print("error sum counter", error_sum_counter) if DEBUG else None
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
data_line = str(start_time - end_time) data_line = str(start_time - end_time)
@ -348,37 +312,23 @@ def pid_calculations():
output_angle = round(output_angle) output_angle = round(output_angle)
previous_angle = output_angle previous_angle = output_angle
# PID angle logging
data_line = str(output_angle)
log_data(data_file="pid_output_angle", data_line=data_line, remark="") if LOG_GRAPH and TWIN_MODE == False else None
log_data(data_file="pid_output_angle_twin", data_line=data_line,
remark="") if LOG_GRAPH and TWIN_MODE == True else None
return output_angle return output_angle
def control_server_angle(angle): def control_server_angle(angle):
# Tell the servo to set its position
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
print("Current angle: ", angle) if SCREEN else None
servo_angle = angle + OFFSET servo_angle = angle + OFFSET
print("Offset angle: ", servo_angle) if SCREEN else None KIT.servo[0].angle = servo_angle # Set angle
KIT.servo[0].angle = servo_angle # Send angle instruction to the servo
log_line = str(angle) log_line = str(angle)
log_data(data_file="servo", data_line=log_line, remark="") if LOG else None log_data(data_file="servo", data_line=log_line, remark="") if LOG else None
print("angle: ", servo_angle) if SCREEN else None
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
data_line = str(start_time - end_time) data_line = str(start_time - end_time)
log_data(data_file="function", data_line=data_line, remark="control_server_angle") if LOG else None log_data(data_file="function", data_line=data_line, remark="control_server_angle") if LOG else None
def servo_slower(): def servo_slower():
# This function restricts the servo to +/- 5 degrees in order to prevent launching the cart
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f')) start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
global current_angle global current_angle
@ -398,39 +348,10 @@ def servo_slower():
return servo_angle return servo_angle
def graph_plotter(file_name):
# Creates the graphs with Pandas and Mathplotlib using the logiles as input. It must be run manually.
plt.rcParams['figure.figsize'] = [12, 8] # Set the size of the plot canvas
picture_name = file_name + '.png' # User the name of the logfile as input for the graphical image
file_name_plotter = file_name + ".csv" # Use the logfile as input
# Run one set of the graph code.
# df = pd.read_csv(file_name_plotter,delimiter=';', header=None, skiprows=0, decimal=".", names=['Timestamp', 'Distance', 'Timestamp2','Remarks'])
# df = df.drop(columns = ['Timestamp2'])
df = pd.read_csv(file_name_plotter, delimiter=';', header=None, skiprows=0, decimal=".",
names=['Timestamp', 'Distance', 'Remarks'])
df = df.drop(columns=['Remarks'])
plt.figure(figsize=(30, 60))
df.plot(x='Timestamp', y='Distance')
plt.savefig(picture_name)
plt.show()
# -------------------- Main ----------------------------------
try: try:
KIT.servo[0].angle = 90 KIT.servo[0].angle = 90
# graph_plotter("pid-balancer_pid_output_angle_twin_data_2025-01-17 14:29:29.624")
# graph_plotter("pid-balancer_twin_acceleration_data_2025-01-17 14:29:29.624")
# graph_plotter("pid-balancer_twin_current_position_data_2025-01-17 14:29:29.624")
# graph_plotter("pid-balancer_twin_current_speed_data_2025-01-17 14:29:29.624")
while True: while True:
# digital_twin()
control_server_angle(pid_calculations()) control_server_angle(pid_calculations())
print("------------------------------------------\n")
except RuntimeError: except RuntimeError:
print("What's up?!") print("bbbb")

3
picture_generator.py
View File

@ -1,3 +0,0 @@
import pandas as pd
import matplotlib.pyplot as plt