-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathCLOSE LOOP_autocali.py
1492 lines (1270 loc) · 72.8 KB
/
CLOSE LOOP_autocali.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
####################################################################################
########### WELCOME! ###############################################################
# My name is FrED and I'm the brains of this operation,
# Congratulations on assembling the FrED device and setting up your remote desktop connection.
# I'll be your instructor guiding you along your learning experience,
# let's have some fun!
#
# Get ready to learn about:
# 1) Coding in Python
# 2) Using controls to improve diameter control of fiber optic cable
# 3) Varying extruder speed, spooling speed, and temperature to grow fiber optic cables
# of desired diameter
#
############ READ ME BEFORE STARTING, THIS IS THE FIRST PART #######################
# If you've opened this code, I'll assume you're ready for your first test!
# You can run this code using the green "Run" button in the bar above.
#
# The live video feed should come up of both the binary and live video,
# nothing will show until a fiber is in front of the camera.
# Your first run should start the spooling motor first, then the black block at the top
# labeled "HOT" will start heating up.
# Once heated to the desired temperature (first run will be 95 C, be careful!)
# the extruder motor will start running.
#
# Your first task is to insert the hot glue preform, careful as the glue will be hot as it comes out.
#
# When the glue starts falling as a continuous thread, you can loop it around the
# tensioners and attach it to the spool.
# Be patient as this sometimes takes a few tries to get it right :)
#
# At this point you can stop the code and adjust the hardware location,
# cooling fan position, and lighting conditions as necessary
# until you get a continuous spooling and a stable image.
# Make sure you take some time setting up this first run, everything else will be
# much more fun with a clear image and stable fiber growth!
#
##################### DO NOT CONTINUE UNTIL YOU DO THE FIRST PART #################
# Congratulations on getting your first fiber to spool! Your image looks great,
# and the binary image matches the live feed such that a stable diameter is being detected.
# Let's keep going to the fun part!
#
############### PYTHON AND CONTROLS INTERFACE (OPTIONAL READ)######################
# If you've written code in python, work in fiber optics (or have familiarity), or
# have a controls background you're already halfway there to getting the most out of me!
# If you don't then you're in the right place to learn.
#
# The code below is designed to walk you through step by step how the code is operating,
# directing you to the places where changes can be made to improve the quality of the fiber.
# Remember that I am a learning device, don't be afraid to add comments or ask questions!
#
# BRIEF DESCRIPTION OF INTERFACES:
# Running the program: The user interface that pops up after pressing the
# start button is designed to represent how the code is setup and
# allow the user to make controls and diameter changes without
# knowing python or altering the code itself.
#
# This Code: The code below is designed to walk you through step by step
# how the code is operating, directing you to the places where
# changes can be made to improve the quality of the fiber.
# Remember that I am a learning device, don't be afraid to add
# more code or play around with different conditions to see their
# effect on the fiber! Best coding practices for major edits is to
# copy this document and rename it with something unique that you'll
# remember.
#
# Physical FrED: The goal of the Fiber optic Extrusion Device is to spool hot glue from a larger
# diameter to a smaller one and achieve precise diameter control.
# This push-pull method is used in industry, usually using glass,
# to make fiber optic cables like those used in high speed internet applications.
# This diamter control is achieved by melting the hot
# glue preform rod - not so hot that it liquifies, and not so cold that it can't
# be spooled into thread. Then the extrude motor and spooling motor speeds are
# adjusted so that volumetric flow rate is conserved (think volume in the larger cylinder,
# or preform, input must match the volume in the thin cylinder, or fiber, coming out).
############################################################################################
############### BEGGINING OF USER CHANGEABLE CODE ##########################################
# In this section you'll find the code that allows you to change the initial hardware settings
# This includes rpm of the extruder motor, duty cycle of the spooling motor, and the temperature of the heater.
# ~~~~~~~ Extruder (Stepper) Motor Settings ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
direction = 0 # Clockwise = 1 or Anticlockwise = 0
microstepping = '1/2' # Enter '1' , '1/2', '1/4', '1/8', '1/16' or '1/32', with colons
rpm = 0.3 #revolutions per minute RPM
# ~~~~~~~ Heater Settings ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
targetTemp = 80 #degree celcius
# ~~~~~~~ Spooling (DC) Motor Settings ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
dcDuty = 10 #DC motor initialization
# ~~~~~~~ Fan Settings ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
fanDuty = 45 #duty cycle from 0 to 100%
# ~~~~~~~ Image Settings ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
binary_true = 1 # If binary_true = 1, use binary image. If 0, use gray image
##################### END OF USER CHANGEABLE CODE ##########################################
############################################################################################
# ONLY CHANGE CODE IF YOU ARE FAMILIAR WITH PYTHON AND HAVE AN UNDERSTANDING OF THE FUNCTIONS!
# Feel free to read along in the comments to see how the code works, change at your own risk
# To "play" with the code, try saving a copy of this document with a unique name and play around!
# However, you'll see that most of the controls interfaces can be accessed from the slider bars
# in the user interface.
# ~~~~~~ Import Libraries ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
import os
import cv2
import csv
import time
import math
import board
import busio
import atexit
import datetime
import digitalio
import contextlib
import threading
import numpy as np
import RPi.GPIO as GPIO
import adafruit_mcp3xxx.mcp3008 as MCP
import matplotlib.pyplot as plt
import matplotlib
matplotlib.use('Qt5Agg')
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.figure import Figure
import numpy as np
from statistics import mean
from statistics import stdev
import tkinter as tk
from tkinter import ttk
GPIO.setmode(GPIO.BCM)
from time import sleep
from gpiozero import Motor
from gpiozero import RotaryEncoder
from adafruit_mcp3xxx.analog_in import AnalogIn
import warnings
warnings.filterwarnings("ignore")
import sys
from PyQt5 import QtCore
from PyQt5.QtWidgets import QApplication, QMainWindow, QLabel, QSlider, QGridLayout, QWidget, QDoubleSpinBox, QPushButton, QMessageBox, QLineEdit, QCheckBox
from PyQt5.QtGui import QImage, QPixmap
from PyQt5.QtCore import QTimer, Qt, QThread, pyqtSignal
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
import matplotlib.pyplot as plt
from PyQt5.QtGui import QPixmap
#~~~~~~~~~~~~~~~~~~~~ Initialization ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#Parameters for motor calibration
diameter_coeff = 0.00782324
motor_slope = 0
motor_intercept = 0
Pd = 11
d_preform = 7
d_spool = 15.2
# Thermistor Constants
RT0 = 100000 # Ω
T0 = 298.15 # K
B = 3977 # K
VCC = 3.3 # Supply voltage
R = 10000 # R=10KΩ
#Stepper motor initialisation
SPR = 200 # Steps per Revolution, from Stepper Data Sheet
RESOLUTION = {'1': (0, 0, 0), # (M0, M1, M2)
'1/2': (1, 0, 0),
'1/4': (0, 1, 0),
'1/8': (1, 1, 0),
'1/16': (0, 0, 1),
'1/32': (1, 0, 1)} #microstepping settings
FACTOR = {'1': 1,
'1/2': 2,
'1/4': 4,
'1/8': 8,
'1/16': 6,
'1/32': 32} #microstepping settings
# delay = 60/rpm/SPR/FACTOR[microstepping]
# # # # # # # DC Motor Initialisation # # # # # # # # #
# DC Motor initialisation
ppr = 1176 # Pulses Per Revolution of the encoder
dcFreq = 2000 #DC motor PWM frequency
fanFreq = 1000 #Fan PWM frequency
tsample = 0.1 # Sampling period for code execution (s)
# Initializing previous values and starting main clock
tstart = time.perf_counter() #start internal clock
oldtime = 0 #old DC time
oldpos = 0 #old DC position
lasttime = 0 #old stepper time
# Initialize motor PID terms
# PID Parameters (these should be adjusted based on Ku and Tu)
Ku = 0.4 # ultimate gain (example value, adjust accordingly)
Tu = 0.9 # oscillation period (example value, adjust accordingly)
Kp = 0.6 * Ku
Ti = Tu / 2
Td = Tu / 8
Ki = Kp / Ti
Kd = Kp * Td
# setpoint_rpm = 50 # Desired speed
integral = 0
previous_error = 0
# Anti-windup limits
integral_max = 100
integral_min = -100
integral_d = 0
previous_error_d = 0
# Temperature PID constants
Kp_t = 1.4 # Proportional gain
Ki_t = 0.2 # Integral gain
Kd_t = 0.8 # Derivative gain
# Controller output limits
output_t_min = 0
output_t_max = 1
# Number of readings to average
num_readings = 5
temperature_readings = []
# Initialize PID variables
integral_t = 0
previous_error_t = 0
previous_time_t = time.perf_counter()
# Create instance of lists for saving to csv
CSVList = [] # stores data that will be saved as csv file
time_list = [] # stores time values
diameter_mm_list = [] # stores diameter values
diameter_setpoint_list = [] # stores diameter set point values
temperature_list = [] # stores temperature values
temp_set_point_list = [] # stores temperature set point values
temp_error_list = [] # stores temperature error values
kp_list = [] # stores kp values
ki_list = [] # stores kd values
kd_list = [] # stores kd values
dc_motor_set_speed_list = [] # stores dc motor set speed values
dc_motor_speed_list = [] # stores dc motor speed values
oscillation_ku_list = [] # stores oscilation ku values
period_tu_list = [] # stores oscillation tu values
pid_list = [] # stores PID values
extruder_speed_list = [] # stores extruder speed values
fan_speed_list = [] # stores fan speed values
counter = 0
count = 0
y=0 # counter variable for main loop
list_time2=[]
device_started = False
diameter_started = False
use_binary = False
################################################################
####### Start of Classes #######################################
######### READ AND PLOT DC MOTOR SPEED ################################
class DCMotorPlot(FigureCanvas):
def __init__(self, parent=None):
self.figure = Figure()
self.axes = self.figure.add_subplot(111)
super(DCMotorPlot, self).__init__(self.figure)
self.setParent(parent)
self.axes.set_title('DC Motor Plot')
self.axes.set_xlabel('Time')
self.axes.set_ylabel('Speed (RPM)')
self.line, = self.axes.plot([], [], lw=2, label='Speed')
self.line_set_point, = self.axes.plot([], [], lw=2, label='Target Speed')
self.axes.legend()
self.x_data = []
self.y_data = []
self.y_set_point_data = []
def update_plot(self, x, y, y_set_point):
self.x_data.append(x)
self.y_data.append(y)
self.y_set_point_data.append(y_set_point)
self.line.set_data(self.x_data, self.y_data)
self.line_set_point.set_data(self.x_data, self.y_set_point_data)
self.axes.relim()
self.axes.autoscale_view(True, True, True)
self.draw()
######### READ AND PLOT TEMPERATURE ################################
class TemperaturePlot(FigureCanvas):
def __init__(self, parent=None):
self.figure = Figure()
self.axes = self.figure.add_subplot(111)
super(TemperaturePlot, self).__init__(self.figure)
self.setParent(parent)
self.axes.set_title('Temperature Plot')
self.axes.set_xlabel('Time')
self.axes.set_ylabel('Temperature (°C)')
self.line, = self.axes.plot([], [], lw=2, label='Temperature')
self.line_set_point, = self.axes.plot([], [], lw=2, label='Target Temperature')
self.axes.legend()
self.x_data = []
self.y_data = []
self.y_set_point_data = []
def update_plot(self, x, y, y_set_point):
self.x_data.append(x)
self.y_data.append(y)
self.y_set_point_data.append(y_set_point)
self.line.set_data(self.x_data, self.y_data)
self.line_set_point.set_data(self.x_data, self.y_set_point_data)
self.axes.relim()
self.axes.autoscale_view(True, True, True)
self.draw()
############ READ AND PLOT FIBER DIAMETER ########################
class DiameterPlot(FigureCanvas):
def __init__(self, parent=None):
self.figure = Figure()
self.axes = self.figure.add_subplot(111)
super(DiameterPlot, self).__init__(self.figure)
self.setParent(parent)
self.axes.set_title('Diameter Plot')
self.axes.set_xlabel('Time')
self.axes.set_ylabel('Diameter (mm)')
self.line, = self.axes.plot([], [], lw=2, label='Diameter - Gray Filter')
self.line_set_point, = self.axes.plot([], [], lw=2, label='Target Diameter')
# self.line_binaryimg, = self.axes.plot([], [], lw=2, label='Diameter - Binary Filter')
self.axes.legend()
self.x_data = []
self.y_data = []
self.y_set_point_data = []
# self.y_binary_data = []
def update_plot(self, x, y, y_set_point): #, y_binary):
self.x_data.append(x)
self.y_data.append(y)
self.y_set_point_data.append(y_set_point)
# self.y_binary_data.append(y_binary)
self.line.set_data(self.x_data, self.y_data)
self.line_set_point.set_data(self.x_data, self.y_set_point_data)
# self.line_binaryimg.set_data(self.x_data, self.y_binary_data)
self.axes.relim()
self.axes.autoscale_view(True, True, True)
self.draw()
########### GPIO SETUP FOR HARDWARE CALLS ######################
class GPIOController:
def __init__(self):
global heaterPin
# GPIO Pin Definitions
self.extruderDirection = 16 # Extruder Direction Pin
self.extruderStep = 12 # Extruder Step Pin
self.encoderPin1 = 24 # DC Motor Encoder Pin 1
self.encoderPin2 = 23 # DC Motor Encoder Pin 2
self.dcmotorPin = 5 # DC Motor PWM Pin
heaterPin = 6 # Heater Pin
self.fanPin = 13 # Fan Pin
self.M0 = 17 # Microstepping Pin 1
self.M1 = 27 # Microstepping Pin 2
self.M2 = 22 # Microstepping Pin 3
# Initialize GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM) # Use Broadcom pin numbering
# Set up GPIO pins
GPIO.setup(heaterPin, GPIO.OUT)
GPIO.setup(self.extruderDirection, GPIO.OUT)
GPIO.setup(self.extruderStep, GPIO.OUT)
GPIO.setup(self.dcmotorPin, GPIO.OUT)
GPIO.setup(self.fanPin, GPIO.OUT)
GPIO.setup((self.M0, self.M1, self.M2), GPIO.OUT)
# Set initial state (if needed)
#GPIO.output(self.extruderDirection, GPIO.LOW) # Replace with your desired initial state
GPIO.output(self.extruderDirection, direction)
# Initialize DC motor encoder and SPI bus
self.initialize_encoder_and_spi()
# Stepper motor initialisation
self.SPR = 200 # Steps per Revolution, from Stepper Data Sheet
self.RESOLUTION = {'1': (0, 0, 0), # (M0, M1, M2)
'1/2': (1, 0, 0),
'1/4': (0, 1, 0),
'1/8': (1, 1, 0),
'1/16': (0, 0, 1),
'1/32': (1, 0, 1)} # microstepping settings
self.FACTOR = {'1': 1,
'1/2': 2,
'1/4': 4,
'1/8': 8,
'1/16': 16,
'1/32': 32} # microstepping settings
self.microstepping = '1/4' # Default microstepping mode
self.rpm = 0.6 # Default RPM
self.set_microstepping(self.microstepping)
def set_microstepping(self, mode):
mode_pins = (self.M0, self.M1, self.M2)
mode_values = self.RESOLUTION[mode]
for pin, value in zip(mode_pins, mode_values):
GPIO.output(pin, value)
self.microstepping = mode
self.update_delay()
def set_rpm(self, rpm):
self.rpm = rpm
self.update_delay()
def update_delay(self):
self.delay = 60 / self.rpm / self.SPR / self.FACTOR[self.microstepping]
def step(self, direction):
# Set direction
GPIO.output(self.extruderDirection, direction)
#
# # while True:
# GPIO.output(self.extruderStep, GPIO.HIGH)
# time.sleep(self.delay)
# GPIO.output(self.extruderStep, GPIO.LOW)
# time.sleep(self.delay)
# print(self.delay)
def initialize_encoder_and_spi(self):
global channel_0
global encoder
# Initialise DC motor encoder
encoder = RotaryEncoder(self.encoderPin1, self.encoderPin2, max_steps=0)
# Create the SPI bus
self.spi = busio.SPI(clock=board.SCK, MISO=board.MISO, MOSI=board.MOSI)
# Create the cs (chip select)
self.cs = digitalio.DigitalInOut(board.D8)
# Create the mcp object
self.mcp = MCP.MCP3008(self.spi, self.cs)
# Create analog inputs connected to the input pins on the MCP3008
channel_0 = AnalogIn(self.mcp, MCP.P0)
def start_fan(self, fan_freq, fan_duty):
self.fan_pwm = GPIO.PWM(self.fanPin, fan_freq)
self.fan_pwm.start(fan_duty)
def update_fan_duty(self, new_duty):
self.fan_pwm.ChangeDutyCycle(new_duty)
def stop_fan(self):
self.fan_pwm.stop()
def start_dc_motor(self, dc_freq, dc_duty):
global dc_motor_pwm
dc_motor_pwm = GPIO.PWM(self.dcmotorPin, dc_freq)
dc_motor_pwm.start(dc_duty)
def update_dc_duty(self, new_duty):
dc_motor_pwm.ChangeDutyCycle(new_duty)
def stop_dc_motor(self):
dc_motor_pwm.stop()
def start_stepper_motor(self, microstepping):
GPIO.output((self.M0, self.M1, self.M2), self.RESOLUTION[microstepping])
def start_devices(self, fan_freq, fan_duty, dc_freq, dc_duty, stepper_rpm):
self.start_fan(fan_freq, fan_duty)
self.start_dc_motor(dc_freq, dc_duty)
# self.set_rpm(stepper_rpm)
# self.step(1) # Start stepping in the clockwise direction
def cleanup(self):
GPIO.cleanup()
class PID_Controls():
global fiber_diameter
def __init__(self):
super(PID_Controls,self).__init__()
# self.connection = video_widget.line_value_updated.connect(self.handle_diam)
#~~~~~~ Get diameter and plot on graph ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Define a global variable to store the diameter value
self.diameter_value = 0.0
# Define a global variable to store the diameter set point
self.diameter_set_point = 0.0 # Set an initial value
# Connect the line_value_updated signal to the update_ slot
video_widget.line_value_updated.connect(self.update_line_value)
def update_line_value(self, line_value):
# Update the diameter_value with the new line_value
self.diameter_value = line_value
# Call the update_plot function with the current time and diameter value
current_time = time.time() # Get the current time
# diameter_plot.update_plot(current_time, self.diameter_value) #, self.diameter_set_point)
def temperature(self):
try:
#~~~~~~~~~~~ CONTROL TEMPERATURE ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
global previous_time_t
global temperature_readings
global integral_t
global previous_error_t
global Kp_t
global Ki_t
global Kd_t
global error_t
slider_value = tempslider.value()
Kp_t = kpslider.value()
Ki_t = kislider.value()
Kd_t = kdslider.value()
# Simple heater controls
targetTemp = slider_value
VR = (channel_0.voltage)
# with contextlib.suppress(ZeroDivisionError):
RT = (VCC - VR) * R / VR
ln = math.log(RT / RT0)
TX = (1 / ((ln / B) + (1 / T0)))
TX = TX - 273.15
current_time_t = time.perf_counter()
elapsed_time_t = current_time_t - previous_time_t
previous_time_t = current_time_t
temperature_readings.append(TX)
# Turn the heater off if there are not enough readings to prevent building up too much heat
GPIO.output(heaterPin, GPIO.LOW)
# Ensure we have the required number of readings
if len(temperature_readings) >= num_readings:
# Calculate the average temperature
average_temp = sum(temperature_readings) / num_readings
temperature_readings = [] # Reset the readings list
# Calculate error
error_t = targetTemp - 1 - average_temp
# Calculate integral
integral_t += error_t * elapsed_time_t
# Calculate derivative
derivative_t = (error_t - previous_error_t) / elapsed_time_t
previous_error_t = error_t
# Compute the PID output
PID_output_t = Kp_t * error_t + Ki_t * integral_t + Kd_t * derivative_t
# Apply anti-windup: Clamp the integral term if the PID output is saturated
if PID_output_t > output_t_max:
PID_output_t = output_t_max
integral_t -= error_t * elapsed_time_t # Remove the effect of this error integration
elif PID_output_t < output_t_min:
PID_output_t = output_t_min
integral_t -= error_t * elapsed_time_t # Remove the effect of this error integration
# Apply the PID output
if PID_output_t > 0:
GPIO.output(heaterPin, GPIO.HIGH)
else:
GPIO.output(heaterPin, GPIO.LOW)
else:
error_t =0
PID_output_t=0
current_time = time.time() # Get the current time
temperature_value = TX # Replace with your actual temperature value
temperature_set_point = slider_value # Replace with your desired temperature set point
temperature_plot.update_plot(current_time, temperature_value, temperature_set_point)
temp_error_list.append(error_t) # stores temperature error values
temperature_list.append(TX) # stores temperature values
temp_set_point_list.append(temperature_set_point) # stores temperature set point values
kp_list.append(Kp_t) # stores kp values
ki_list.append(Ki_t) # stores kd values
kd_list.append(Kd_t) # stores kd values
pid_list.append(PID_output_t) # stores PID values
except Exception as e:
print (f"\n An error occurred while updating temperature controls: {e}\n\n")
QMessageBox.information(app.activeWindow(),"Temperature control didn't update", "The temperature controls didn't update, please try restarting program.")
pass
finally:
pass
def dc_motor(self):
try:
global oldtime
global oldpos
global integral
global previous_error
global integral_d
global previous_error_d
global current_time
diameter_set_point = diameterslider.value()
stepper_rpm = extrslider.value()
rpm = extrslider.value()
delay = 60 / rpm / SPR / FACTOR[microstepping]
#Stepper motor control
GPIO.output(16, 1)#direction
GPIO.output(12, GPIO.HIGH)
time.sleep(delay)
GPIO.output(12, GPIO.LOW)
time.sleep(delay)
# PID Parameters for DC motor control
Ku = Gainslider.value()
Tu = Oscslider.value()
Kp = 0.6 * Ku
Ti = Tu / 2
Td = Tu / 8
Ki = Kp / Ti
Kd = Kp * Td
# PID Parameters for diameter control
Ku_d = gain_dslider.value()
Tu_d = osc_dslider.value()
Kp_d = 0.6 * Ku_d
Ti_d = Tu_d / 2
Td_d = Tu_d / 8
Ki_d = Kp_d / Ti_d
Kd_d = Kp_d * Td_d
current_time = time.perf_counter() - tstart
if len(diameter_mm_list)>=1:
diameter_value = diameter_mm_list[-1]
else:
diameter_value = 0
if diameter_started == True:
diameter_plot.update_plot(current_time, diameter_value, diameter_set_point)
if len(diameter_mm_list) >= 20:
# Get the last 10 items
last_measurements = diameter_mm_list[-20:]
ma_diameter = sum(last_measurements) / len(last_measurements)
error_d = diameter_set_point - ma_diameter
integral_d += error_d * 0.35
integral_d = max(min(integral_d, 0.5), -0.5)
derivative_d = (error_d - previous_error_d) / 0.35
output_d2s = Kp_d * error_d + Ki_d * integral_d + Kd_d * derivative_d
rpm_diff =diameter_to_spool_rpm(output_d2s, stepper_rpm)
#print(rpm_diff)
setpoint_rpm = min(max(rpm_diff, 0), 60)
previous_error_d = error_d
#print(setpoint_rpm)
else:
print("not enough:")
setpoint_rpm = diameter_to_spool_rpm(diameter_set_point, stepper_rpm)
else:
setpoint_rpm = 50 # Desired speed
# Start of DC motor PID control
# Calculation of DC motor speed-mitcop
dt = time.perf_counter() - oldtime
ds = encoder.steps - oldpos
rpm_DC = ds / ppr / dt * 60
# PID Control
error = setpoint_rpm - rpm_DC
integral += error * dt
integral = max(min(integral, integral_max), integral_min) # Constrain integral to prevent windup
derivative = (error - previous_error) / dt
# Compute PID output in RPM
output_rpm = Kp * error + Ki * integral + Kd * derivative
# Convert PID output from RPM to duty cycle
output_duty = rpm_to_duty_cycle(output_rpm)
output_duty = min(max(output_duty, 0), 100) # Constrain duty cycle between 0 and 100
# Update duty cycle
gpio_controller.update_dc_duty(output_duty)
# p.ChangeDutyCycle(output_duty)
previous_error = error
oldtime = time.perf_counter()
oldpos = encoder.steps
# Update fan duty cycle
gpio_controller.update_fan_duty(fanslider.value())
# End of DC motor PID control
dcslider_value = slider.value()
dcspeed_value = rpm_DC
dcspeed_set_point = setpoint_rpm
dcmotor_plot.update_plot(current_time, dcspeed_value, dcspeed_set_point)
time_list.append(time.time())# stores time values
diameter_setpoint_list.append(self.diameter_set_point) # stores diameter set point values
dc_motor_set_speed_list.append(dcspeed_set_point) # stores dc motor set speed values
dc_motor_speed_list.append(dcspeed_value) # stores dc motor speed values
oscillation_ku_list.append(Ku) # stores oscilation ku values
period_tu_list.append(Tu) # stores oscillation tu values
extruder_speed_list.append(extrslider.value()) # stores extruder speed values
fan_speed_list.append(fanDuty) # stores fan speed values
except Exception as e:
print (f"\n An error occurred while updating dc motor controls: {e}\n\n")
QMessageBox.information(app.activeWindow(),"DC motor control didn't update", "The spooling motor controls didn't update, please try restarting program.")
pass
finally:
pass
############ DISPLAY LIVE AND BINARY VIDEO #######################
class VideoWidget(QWidget):
line_value_updated = pyqtSignal(float) # Create a new signal
def __init__(self, parent=None):
super(VideoWidget, self).__init__(parent)
self.video_size = None
self.setup_ui()
self.setup_camera()
def setup_ui(self):
layout = QGridLayout()
self.video_label = QLabel()
layout.addWidget(self.video_label, 2, 5, 10, 5) # Add the VideoWidget to the layout
self.bvideo_label = QLabel()
layout.addWidget(self.bvideo_label, 11, 5, 5, 5) # Add the VideoWidget to the layout
self.binaryvideo_label = QLabel()
layout.addWidget(self.binaryvideo_label, 11, 8, 5, 5) # Add the VideoWidget to the layout
self.setLayout(layout)
def setup_camera(self):
self.cap = cv2.VideoCapture(0) # Replace 0 with the appropriate camera index
def show_frame(self):
ret, frame = self.cap.read()
if ret:
# Live video
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
# Display the frame with lines
img = QImage(frame, frame.shape[1], frame.shape[0], QImage.Format_RGB888)
pix = QPixmap.fromImage(img)
self.video_label.setPixmap(pix)
# Process gray image
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
# Process binary image
gray2 = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
_, binary = cv2.threshold(gray2, 0, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
if use_binary == False:
edges = cv2.Canny(gray, 100, 255, apertureSize=3)
else:
edges = cv2.Canny(binary, 100, 255, apertureSize=3)
# Get diameter from the binary image
line_value = self.read_line_value(edges)
# Plot lines on the frame
frame = self.plot_lines(frame, edges)
# Emit the line_value_updated signal with the new line_value
self.line_value_updated.emit(line_value)
# Update diameter plot
diameter_mm_list.append(round(float(line_value), 2)) # Stores diameter values
# if line_value != 0:
# diameter_plot.update_plot(current_time, line_value)
# Display the frame with lines
img = QImage(frame, frame.shape[1], frame.shape[0], QImage.Format_RGB888)
pix = QPixmap.fromImage(img)
self.video_label.setPixmap(pix)
# Display the gray image
img2 = QImage(gray, gray.shape[1], gray.shape[0], QImage.Format_Grayscale8)
pix2 = QPixmap.fromImage(img2)
self.bvideo_label.setPixmap(pix2)
# Binary Image
img3 = QImage(binary, binary.shape[1], binary.shape[0], QImage.Format_Grayscale8)
pix3 = QPixmap.fromImage(img3)
self.binaryvideo_label.setPixmap(pix3)
def calibrate_line_value(self):
ret, frame = self.cap.read()
if ret:
# Live video
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
# Display the frame with lines
img = QImage(frame, frame.shape[1], frame.shape[0], QImage.Format_RGB888)
pix = QPixmap.fromImage(img)
self.video_label.setPixmap(pix)
# Process gray image
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
edges = cv2.Canny(gray, 100, 255, apertureSize=3)
lines = cv2.HoughLines(edges, 1, np.pi / 180, 100)
if lines is not None and len(lines) > 1:
line_distances = []
for line in lines:
rho, theta = line[0]
line_distances.append((rho, theta))
if len(line_distances) == 0:
print("Empty")
width_of_wire_mm = 0
else:
max_distance = 0
extreme_line1 = None
extreme_line2 = None
# Calculate the distance between every pair of lines
for i in range(len(line_distances)):
for j in range(i + 1, len(line_distances)):
distance = abs(line_distances[i][0] - line_distances[j][0])
if distance > max_distance:
max_distance = distance
extreme_line1 = line_distances[i]
extreme_line2 = line_distances[j]
# Calculate the width of the wire in mm (example conversion factor, needs tuning)
width_of_wire_mm = max_distance
return width_of_wire_mm
else:
return None
return None
def read_line_value(self, edges):
global diameter_coeff
lines = cv2.HoughLines(edges, 1, np.pi / 180, 100)
if lines is not None and len(lines) > 1:
line_distances = []
for line in lines:
rho, theta = line[0]
line_distances.append((rho, theta))
if len(line_distances) == 0:
print("Empty")
width_of_wire_mm = 0
else:
max_distance = 0
extreme_line1 = None
extreme_line2 = None
# Calculate the distance between every pair of lines
for i in range(len(line_distances)):
for j in range(i + 1, len(line_distances)):
distance = abs(line_distances[i][0] - line_distances[j][0])
if distance > max_distance:
max_distance = distance
extreme_line1 = line_distances[i]
extreme_line2 = line_distances[j]
# Calculate the width of the wire in mm (example conversion factor, needs tuning)
width_of_wire_mm = max_distance * diameter_coeff
else:
width_of_wire_mm = 0
return width_of_wire_mm
def plot_lines(self, frame, edges):
lines = cv2.HoughLines(edges, 1, np.pi / 180, 110)
if lines is not None:
for line in lines:
rho, theta = line[0]
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
x1 = int(x0 + 1000 * (-b))
y1 = int(y0 + 1000 * (a))
x2 = int(x0 - 1000 * (-b))
y2 = int(y0 - 1000 * (a))
cv2.line(frame, (x1, y1), (x2, y2), (255, 0, 0), 2)
return frame
def closeEvent(self, event):
self.cap.release()
event.accept()
################################################################
####### End of Classes. Start Threading and GUI ################
########### HARDWARE CONTROL ###########################
def motor_control_thread():
# ~~~~~~~ Initialize variables ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
global iteration
iteration = 0
# ~~~~~~~ Start motors ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
try:
global gpio_controller
global control
# Talks to hardware
gpio_controller = GPIOController() # Initialize controller class
gpio_controller.start_devices(1000, 45, 1000, 45, 0.6)
# Makes decisions for the hardware, also updates graphs
control = PID_Controls()
control.temperature()
control.dc_motor()
except Exception as e:
print (f"\n An error occurred while starting motors: {e}\n\n")
QMessageBox.information(app.activeWindow(),"Motors didn't start", "The motors didn't start, please make sure everything is turned on and in working condition and then try restarting program.")
pass
finally:
pass
# ~~~~~~~ Continually update the motors ~~~~~~~~~~~~~~~~~~~
while True:
# This is where the controls and hardware are continuously run
try:
# print('boop')
iteration += 1
#Make controls decisions
MakeControllerDecisions()
# Control motor
control_motor(50)
except Exception as e:
print (f"\n An error occurred while updating motors: {e}\n\n")
QMessageBox.information(app.activeWindow(),"Motors didn't update", "The motors didn't update, please make sure everything is turned on and in working condition and then try restarting program.")
finally:
pass
def control_motor(duty):
# Stepper extruder motor
gpio_controller.set_rpm(extrslider.value())
gpio_controller.step(1) # Start stepping in the clockwise direction
def video_update():
video_widget.show_frame()
def MakeControllerDecisions():
if device_started == True:
control.temperature()
control.dc_motor()
else:
pass
#control.diameter_control()
# control.diameter()
def update_gui():
# print('gui')
# dc_label.setText(f"iter {iteration}")
video_update()
def gui_thread():
global app
app = QApplication([])#sys.argv)
global window
window = QWidget()
global dc_label
# central_widget = QWidget()
# ~~~~~~~~~~~ Initialize the Interface ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# # # # # # # First Column # # # # # # #
# Add title
title = QLabel("Diameter Close Loop Controls:")
title.setStyleSheet("font-size: 18px; font-weight: bold;")
# Add the "DC Spooling Motor" label
dc_label = QLabel("DC Spooling Motor")
dc_label.setStyleSheet("font-size: 16px; font-weight: bold;")
global dcmotor_plot
dcmotor_plot = DCMotorPlot()
# Add the "Temperature" label
temp_label = QLabel("Temperature")
temp_label.setStyleSheet("font-size: 16px; font-weight: bold;")
global temperature_plot
temperature_plot = TemperaturePlot()
# Add the "Diameter (mm)" label
dia_label = QLabel("Diameter")
dia_label.setStyleSheet("font-size: 16px; font-weight: bold;")
global diameter_plot
diameter_plot = DiameterPlot()
global binarycheck
binarycheck = QCheckBox("Binary")
binarycheck.setStyleSheet("font-size: 14px;")
binarycheck.stateChanged.connect(checkbox_state_changed)
# # # # # # # Third Column # # # # # #
# Add the "Spooling Motor Set Speed (RPM)" label
dc_set_label = QLabel("Spooling Motor Set Speed (RPM)")
dc_set_label.setStyleSheet("font-size: 16px; font-weight: bold;")
# Add DC set speed slider
global slider
slider = QSlider(Qt.Horizontal)
slider.setMinimum(20)
slider.setMaximum(60)
slider.setValue(50)
# slider.valueChanged.connect(slider_value_changed)
# Add a label to the slider
slider_value_label = QLabel(str(slider.value()))
# Add the "Controller Gain" label