this version works
This commit is contained in:
parent
f3a02c8db3
commit
c8a10d9deb
@ -11,10 +11,11 @@ import pandas as pd
|
||||
from datetime import datetime
|
||||
import busio
|
||||
import adafruit_vl6180x
|
||||
import math
|
||||
|
||||
# laser sensor controls.
|
||||
i2c = busio.I2C(board.SCL, board.SDA)
|
||||
laser = adafruit_vl6180x.VL6180X(i2c)
|
||||
# i2c = busio.I2C(board.SCL, board.SDA)
|
||||
# laser = adafruit_vl6180x.VL6180X(i2c)
|
||||
|
||||
# Variables to control sensor
|
||||
TRIGGER_PIN = board.D4 # GPIO pin xx
|
||||
@ -35,7 +36,7 @@ KIT.servo[0].set_pulse_width_range(MIN_PULSE, MAX_PULSE)
|
||||
LOG: bool = True # Log data to files
|
||||
SCREEN: bool = True # Log data to screen
|
||||
DEBUG: bool = False # More data to display
|
||||
TWIN_MODE: bool = False # 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 burst)
|
||||
MAX_SAMPLES: int = 1
|
||||
@ -80,15 +81,20 @@ error_sum_counter: int = 0
|
||||
|
||||
# Digital twin
|
||||
previous_speed:float = 0.0
|
||||
start_loop = True
|
||||
previous_measurement: float = 0.0
|
||||
previous_position: float = 0.0
|
||||
previous_angle: int = 90
|
||||
|
||||
#maximum angle the servo can move away from steady position. With 10 the range is between 80 and 100, with steady at 90
|
||||
max_angle = 6
|
||||
max_angle = 10
|
||||
|
||||
# servo slower
|
||||
current_angle:int = 90
|
||||
|
||||
watch_variable: int = 0
|
||||
|
||||
# base time of the system
|
||||
base_time: float = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
|
||||
|
||||
# Write data to any of the logfiles
|
||||
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]
|
||||
@ -190,34 +196,66 @@ def read_setpoint():
|
||||
|
||||
return cm_rounded
|
||||
|
||||
def calculate_acceleration():
|
||||
def digital_twin():
|
||||
# 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
|
||||
global previous_position, previous_speed, base_time, watch_variable
|
||||
gravity: float = 9.81
|
||||
friction: float = 0.1
|
||||
delta_t: float = 0.1
|
||||
|
||||
print("calc is active")
|
||||
angle = (previous_angle - 90)
|
||||
acceleration = gravity * math.sin(math.radians(angle))
|
||||
friction_force = friction * gravity * math.cos(math.radians(angle)) * delta_t
|
||||
|
||||
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
|
||||
friction_force = abs(friction_force)
|
||||
|
||||
position_1, timestamp_1 = read_distance_sensor()
|
||||
position_2, timestamp_2 = read_distance_sensor()
|
||||
position_3, timestamp_3 = read_distance_sensor()
|
||||
work_speed = previous_speed + acceleration * delta_t
|
||||
watch_variable = watch_variable + 1
|
||||
|
||||
initial_velocity: float = (position_2 - position_1) / (timestamp_2 - timestamp_1)
|
||||
final_velocity: float = ((position_3 - position_2) / (timestamp_3 - timestamp_2))
|
||||
acceleration: float = (final_velocity - initial_velocity) / (timestamp_3 - timestamp_1)
|
||||
if watch_variable >= 150:
|
||||
print("breakpoint")
|
||||
|
||||
print(initial_velocity, " ", final_velocity, " ", acceleration) if SCREEN else None
|
||||
print("watch_variable", watch_variable)
|
||||
if friction_force < work_speed:
|
||||
if work_speed > 0:
|
||||
work_speed = work_speed - friction_force
|
||||
elif work_speed < 0:
|
||||
work_speed = work_speed + friction_force
|
||||
else:
|
||||
work_speed = work_speed
|
||||
|
||||
data_line: str = str(position_1) + ',' + str(position_2) + ',' + str(position_3) + ',' + str(initial_velocity) + ',' + str(final_velocity) + ',' + str(acceleration)
|
||||
log_data(data_file="acceleration", data_line=data_line, remark="") if LOG else None
|
||||
current_speed = work_speed
|
||||
|
||||
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
|
||||
data_line = str(start_time - end_time)
|
||||
log_data(data_file="function", data_line=data_line, remark="calculate_acceleration") if LOG else None
|
||||
current_position = previous_position + (current_speed * delta_t)
|
||||
|
||||
|
||||
print("angle", angle)
|
||||
print("friction", friction)
|
||||
print("acceleration", acceleration)
|
||||
print("current speed", current_speed)
|
||||
print("current position", current_position)
|
||||
print("")
|
||||
print("----------------")
|
||||
print("")
|
||||
|
||||
base_time = base_time + delta_t
|
||||
|
||||
|
||||
previous_speed = current_speed
|
||||
previous_position = current_position
|
||||
|
||||
return current_position, base_time
|
||||
|
||||
def pid_calculations():
|
||||
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 error_sum_counter, error_sum_array # counter for error_sum_array and error_sum_array itself
|
||||
global previous_angle
|
||||
offset_value: int = 0
|
||||
if TWIN_MODE:
|
||||
measurement, measurement_time = digital_twin()
|
||||
@ -272,6 +310,7 @@ def pid_calculations():
|
||||
log_data(data_file="function", data_line=data_line, remark="pid_calculations") if LOG else None
|
||||
|
||||
output_angle = round(output_angle)
|
||||
previous_angle = output_angle
|
||||
|
||||
return output_angle
|
||||
|
||||
@ -289,36 +328,6 @@ def control_server_angle(angle):
|
||||
data_line = str(start_time - end_time)
|
||||
log_data(data_file="function", data_line=data_line, remark="control_server_angle") if LOG else None
|
||||
|
||||
def digital_twin(pid_angle):
|
||||
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
|
||||
|
||||
global start_loop
|
||||
measurement_time = float(datetime.strftime(datetime.now(),'%Y%m%d%H%M%S.%f')[:-3])
|
||||
|
||||
if start_loop:
|
||||
delta_t = measurement_time - (measurement_time - 0.002)
|
||||
start_loop = False
|
||||
else:
|
||||
delta_t = measurement_time - previous_time
|
||||
|
||||
twin_data = pd.read_csv('twin_data_file.csv')
|
||||
twin_data.set_index('Arm angle', inplace=True)
|
||||
acceleration = twin_data.loc[pid_angle, 'Acceleration']
|
||||
|
||||
# previous acceleration to speed.
|
||||
new_speed = previous_speed + (acceleration*delta_t)
|
||||
measurement = new_speed * delta_t + previous_measurement
|
||||
|
||||
print(measurement)
|
||||
print(new_speed)
|
||||
print(previous_speed)
|
||||
|
||||
end_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
|
||||
data_line = str(start_time - end_time)
|
||||
log_data(data_file="function", data_line=data_line, remark="digital_twin") if LOG else None
|
||||
|
||||
return measurement, measurement_time
|
||||
|
||||
def servo_slower():
|
||||
start_time = float(datetime.strftime(datetime.now(), '%Y%m%d%H%M%S.%f'))
|
||||
|
||||
@ -340,11 +349,9 @@ def servo_slower():
|
||||
return servo_angle
|
||||
|
||||
try:
|
||||
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])
|
||||
KIT.servo[0].angle = 90
|
||||
while True:
|
||||
calculate_acceleration()
|
||||
# control_server_angle(pid_calculations())
|
||||
# digital_twin()
|
||||
control_server_angle(pid_calculations())
|
||||
except RuntimeError:
|
||||
print("bbbb")
|
||||
Loading…
x
Reference in New Issue
Block a user