├── Python code ├── Old files │ ├── folderfile │ ├── circle_trajectory.py │ ├── convert_to_polar.py │ ├── first_try.py │ ├── Old_Motorcontrol.py │ └── old_revision.py ├── Test_Tools │ ├── still-image_test.py │ ├── livevideofeed.py │ ├── HoughTransform_test │ └── GUI_Test └── Working Files │ └── main.py ├── ping-pong.jpg ├── STL_Files ├── Arm1.STL ├── Arm2.stl ├── Base.stl └── Servo_bracket_fit_test.stl ├── 3MF Files ├── PLATEAU.3MF ├── coupler.3MF ├── stencil.3MF ├── Arm_proto1.3MF ├── Arm_proto2.3MF ├── test_link.3MF ├── Arm2_proto1.3MF ├── Arm2_proto2.3MF ├── Arm2_proto3.3MF ├── Arm_proto3_1.3MF ├── Arm_proto3_2.3MF ├── Arm_proto3_3.3MF ├── Base_proto1.3MF ├── Base_proto2.3MF ├── Arm2_proto2 2.3MF ├── Arm2_proto2 3.3MF ├── picamera_test.3MF ├── picamera_test2.3MF ├── Servo_bracket_proto1.3MF └── Servo_bracket_proto2.3MF ├── Complete_assembly.jpg └── README.md /Python code/Old files/folderfile: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /ping-pong.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/ping-pong.jpg -------------------------------------------------------------------------------- /STL_Files/Arm1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/STL_Files/Arm1.STL -------------------------------------------------------------------------------- /STL_Files/Arm2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/STL_Files/Arm2.stl -------------------------------------------------------------------------------- /STL_Files/Base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/STL_Files/Base.stl -------------------------------------------------------------------------------- /3MF Files/PLATEAU.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/PLATEAU.3MF -------------------------------------------------------------------------------- /3MF Files/coupler.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/coupler.3MF -------------------------------------------------------------------------------- /3MF Files/stencil.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/stencil.3MF -------------------------------------------------------------------------------- /Complete_assembly.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/Complete_assembly.jpg -------------------------------------------------------------------------------- /3MF Files/Arm_proto1.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Arm_proto1.3MF -------------------------------------------------------------------------------- /3MF Files/Arm_proto2.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Arm_proto2.3MF -------------------------------------------------------------------------------- /3MF Files/test_link.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/test_link.3MF -------------------------------------------------------------------------------- /3MF Files/Arm2_proto1.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Arm2_proto1.3MF -------------------------------------------------------------------------------- /3MF Files/Arm2_proto2.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Arm2_proto2.3MF -------------------------------------------------------------------------------- /3MF Files/Arm2_proto3.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Arm2_proto3.3MF -------------------------------------------------------------------------------- /3MF Files/Arm_proto3_1.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Arm_proto3_1.3MF -------------------------------------------------------------------------------- /3MF Files/Arm_proto3_2.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Arm_proto3_2.3MF -------------------------------------------------------------------------------- /3MF Files/Arm_proto3_3.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Arm_proto3_3.3MF -------------------------------------------------------------------------------- /3MF Files/Base_proto1.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Base_proto1.3MF -------------------------------------------------------------------------------- /3MF Files/Base_proto2.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Base_proto2.3MF -------------------------------------------------------------------------------- /3MF Files/Arm2_proto2 2.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Arm2_proto2 2.3MF -------------------------------------------------------------------------------- /3MF Files/Arm2_proto2 3.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Arm2_proto2 3.3MF -------------------------------------------------------------------------------- /3MF Files/picamera_test.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/picamera_test.3MF -------------------------------------------------------------------------------- /3MF Files/picamera_test2.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/picamera_test2.3MF -------------------------------------------------------------------------------- /3MF Files/Servo_bracket_proto1.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Servo_bracket_proto1.3MF -------------------------------------------------------------------------------- /3MF Files/Servo_bracket_proto2.3MF: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/3MF Files/Servo_bracket_proto2.3MF -------------------------------------------------------------------------------- /STL_Files/Servo_bracket_fit_test.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicohmje/PID-ballonplate/HEAD/STL_Files/Servo_bracket_fit_test.stl -------------------------------------------------------------------------------- /Python code/Test_Tools/still-image_test.py: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | 3 | cap = cv.VideoCapture(0) 4 | ret, frame = cap.read() 5 | if ret: 6 | cv.imwrite('imagetest.jpeg', frame) 7 | cap.release() -------------------------------------------------------------------------------- /Python code/Test_Tools/livevideofeed.py: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | 3 | cap = cv.VideoCapture(0) 4 | cap.set(3,640) 5 | cap.set(4,480) 6 | 7 | while True: 8 | ret, frame = cap.read() 9 | 10 | cv.imshow("frame",frame) 11 | k = cv.waitKey(5) 12 | if k == 27: 13 | break 14 | cap.release() 15 | cv.destroyAllWindows 16 | -------------------------------------------------------------------------------- /Python code/Old files/circle_trajectory.py: -------------------------------------------------------------------------------- 1 | import json 2 | import numpy as np 3 | 4 | def CircleTrajCoord(): 5 | global circle_coord_x, circle_coord_y 6 | circle_coord_x = [] 7 | circle_coord_y = [] 8 | radius = 150 9 | x = np.arange(0,481) 10 | y = np.arange(0,481) 11 | center = 240 12 | for i in x: 13 | for j in y: 14 | if ((i-center)**2 + (j-center)**2 == 150**2): 15 | print(i,j) 16 | 17 | CircleTrajCoord() 18 | 19 | 240,198 ,150 ,120 ,96 ,90 ,96 ,120 ,150 ,198 ,240 ,282 ,330 ,360 ,384 ,390 ,384 ,360 ,330 ,282 20 | 21 | 90,96,120,150,198,240,282,330,360,384,360,330,282,240,198,150,120,96 22 | -------------------------------------------------------------------------------- /Python code/Old files/convert_to_polar.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | center_x = 250 3 | center_y = 240 4 | 5 | x = 300 6 | y = 300 7 | 8 | def CartesianToPolar(x,y): 9 | center_x = 250 10 | center_y = 240 11 | r = np.sqrt((center_x - x)**2 + (center_y - y)**2) 12 | theta_1 = np.arccos((x-center_x) / r) 13 | theta_2 = np.arcsin((y-center_y)/ r) 14 | 15 | if x > center_x: 16 | if y > center_y: 17 | theta = theta_1 18 | else: 19 | theta = 2*np.pi - abs(theta_2) 20 | else: 21 | if y > center_y: 22 | theta = theta_1 23 | else: 24 | theta = np.pi + abs(theta_2) 25 | 26 | theta = (180 / np.pi) * theta 27 | return(r,theta) 28 | 29 | print(CartesianToPolar(x,y)) -------------------------------------------------------------------------------- /Python code/Test_Tools/HoughTransform_test: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | import numpy as np 3 | 4 | cap = cv.VideoCapture('imagetest.jpeg') 5 | ret, frame = cap.read() 6 | 7 | #cv.imshow("Og image", frame) 8 | hsv = cv.cvtColor(frame, cv.COLOR_BGR2HSV) 9 | lower_value = np.array([00,125,90]) 10 | higher_value = np.array([50,255,255]) 11 | mask = cv.inRange(hsv, lower_value, higher_value) 12 | circles = cv.HoughCircles(mask, cv.HOUGH_GRADIENT, 2, 500, param1=300,param2=30,minRadius=30,maxRadius=40) 13 | if type(circles) == np.ndarray and circles.size != 0: 14 | print("john") 15 | circles = np.uint16(np.around(circles)) 16 | for i in circles[0,:]: 17 | # draw the outer circle 18 | cv.circle(frame,(i[0],i[1]),i[2],(0,255,0),2) 19 | # draw the center of the circle 20 | cv.circle(frame,(i[0],i[1]),2,(0,0,255),3) 21 | cv.imshow('detected circles', frame) 22 | k = cv.waitKey(0) 23 | cap.release() 24 | -------------------------------------------------------------------------------- /Python code/Old files/first_try.py: -------------------------------------------------------------------------------- 1 | #149, 5 2 | #149,423 3 | #624, 432 4 | #640, 0 5 | import time 6 | import numpy as np 7 | import cv2 as cv 8 | from adafruit_servokit import ServoKit 9 | kit = ServoKit(channels = 16) 10 | kit.servo[0].set_pulse_width_range(500, 2500) #Very important 11 | kit.servo[1].set_pulse_width_range(500, 2500) #Very important 12 | 13 | cap = cv.VideoCapture(0) 14 | cap.set(3,640) 15 | cap.set(4,480) 16 | 17 | radii = [] 18 | start = time.time() 19 | for f in np.arange(0,200,1): 20 | ret, frame = cap.read() 21 | frame = frame[5:435, 165:635, :] 22 | blurredframe = cv.medianBlur(frame, 3) 23 | hsv = cv.cvtColor(blurredframe, cv.COLOR_BGR2HSV) 24 | lower_value1 = np.array([00,100,20]) 25 | higher_value1 = np.array([30,255,255]) 26 | mask = cv.inRange(hsv, lower_value1, higher_value1) 27 | #lower_value2 = np.array([160,100,20]) 28 | #higher_value2 = np.array([179,255,255]) 29 | #mask2 = cv.inRange(hsv, lower_value2, higher_value2) 30 | #mask = cv.bitwise_or(mask1, mask2) 31 | res = cv.bitwise_and(frame, frame, mask) 32 | circles = cv.HoughCircles(mask, cv.HOUGH_GRADIENT, 2, 500, param1=300,param2=10,minRadius=25,maxRadius=60) 33 | if type(circles) == np.ndarray and circles.size != 0: 34 | circles = np.uint16(np.around(circles)) 35 | for i in circles[0,:]: 36 | print(i[1]) 37 | kit.servo[0].angle = ((i[1] - 30) / 410) * 180 38 | cv.imshow("Feed", res) 39 | k = cv.waitKey(2) 40 | if k == 27: 41 | break 42 | stop = time.time() 43 | print(200 / (stop - start)) 44 | cap.release() 45 | cv.destroyAllWindows() -------------------------------------------------------------------------------- /Python code/Old files/Old_Motorcontrol.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from adafruit_servokit import ServoKit 3 | import time 4 | 5 | 6 | kit = ServoKit(channels = 16) 7 | kit.servo[0].set_pulse_width_range(500, 2500) #Very important 8 | kit.servo[1].set_pulse_width_range(500, 2500) #Very important 9 | kit.servo[2].set_pulse_width_range(500, 2500) #Very important 10 | 11 | def motor_coeff(theta): 12 | motor_pos = np.array([90,330,210]) 13 | erreur = np.array([0,0,0]) 14 | erreur = (theta - motor_pos + 180 + 360) % 360 - 180 15 | 16 | dir = np.ones(3) 17 | dir[abs(erreur)>90] = - 1 #If dir is -1, the motors should lower | Else, they should go up 18 | 19 | coeffs = np.ones(3) 20 | if theta > 180: 21 | theta_inverse = theta - 180 22 | else: 23 | theta_inverse = theta + 180 24 | 25 | coeffs[dir<0] = 1 - abs((theta - motor_pos[dir < 0] + 180 + 360) % 360 - 180) / 90 26 | coeffs[dir>0] = 1 - abs((theta_inverse - motor_pos[dir > 0] + 180 + 360) % 360 - 180) / 90 27 | 28 | coeffs = coeffs * dir 29 | 30 | return(coeffs) 31 | 32 | def moveMotors(theta, radius): 33 | intensity = (radius / 150) * 25 34 | print(intensity) 35 | initial_angle = np.array([120,120,110]) 36 | coeffs = motor_coeff(theta) 37 | motor_angles = (coeffs * intensity * -1) + initial_angle 38 | print(motor_angles) 39 | if max(motor_angles)>160: 40 | print("angle too high") 41 | return 42 | if min(motor_angles)<80: 43 | print("angle too low") 44 | return 45 | for i in np.arange(0,3): 46 | kit.servo[i].angle = motor_angles[i] 47 | 48 | def initialPos(): 49 | kit.servo[0].angle = 120 50 | kit.servo[1].angle = 120 51 | kit.servo[2].angle = 110 52 | 53 | moveMotors(266, 160) 54 | 55 | time.sleep(10) 56 | initialPos() 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /Python code/Test_Tools/GUI_Test: -------------------------------------------------------------------------------- 1 | from webbrowser import get 2 | from adafruit_servokit import ServoKit 3 | import tkinter as tk 4 | from tkinter import ttk 5 | 6 | # root window 7 | root = tk.Tk() 8 | root.geometry('300x200') 9 | root.resizable(False, False) 10 | root.title('Slider Demo') 11 | 12 | 13 | root.columnconfigure(0, weight=1) 14 | root.columnconfigure(1, weight=3) 15 | 16 | 17 | # slider current value 18 | current_value = tk.DoubleVar() 19 | 20 | 21 | def get_current_value(): 22 | return '{: .2f}'.format(current_value.get()) 23 | 24 | 25 | def slider_changed(event): 26 | value_label.configure(text=get_current_value()) 27 | 28 | 29 | # label for the slider 30 | slider_label = ttk.Label( 31 | root, 32 | text='Slider:' 33 | ) 34 | 35 | slider_label.grid( 36 | column=0, 37 | row=0, 38 | sticky='w' 39 | ) 40 | 41 | 42 | def set_angle(value): 43 | angle = slider.get() 44 | angle = int(angle) 45 | kit.servo[0].angle = angle 46 | 47 | 48 | # slider 49 | slider = ttk.Scale( 50 | root, 51 | from_=0, 52 | to=180, 53 | orient='horizontal', # vertical 54 | command=set_angle, 55 | variable=current_value 56 | ) 57 | 58 | slider.grid( 59 | column=1, 60 | row=0, 61 | sticky='we' 62 | ) 63 | 64 | # current value label 65 | current_value_label = ttk.Label( 66 | root, 67 | text='Current Value:' 68 | ) 69 | 70 | current_value_label.grid( 71 | row=1, 72 | columnspan=2, 73 | sticky='n', 74 | ipadx=10, 75 | ipady=10 76 | ) 77 | 78 | # value label 79 | value_label = ttk.Label( 80 | root, 81 | text=get_current_value() 82 | ) 83 | value_label.grid( 84 | row=2, 85 | columnspan=2, 86 | sticky='n' 87 | ) 88 | 89 | kit = ServoKit(channels = 16) 90 | kit.servo[0].set_pulse_width_range(500, 2500) #Very important 91 | kit.servo[1].set_pulse_width_range(500, 2500) #Very important 92 | 93 | 94 | root.mainloop() 95 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Using a PID algo and Computer Vision to stabilize a ping-pong ball on a moving plane 2 | 3 | ![the complete assembly](https://github.com/nicohmje/PID-ballonplate/blob/main/Complete_assembly.jpg?raw=true) 4 | 5 | A relatively straightforward project using a Raspberry Pi, three servos and a 3D printed structure. 6 | 7 | [SEE IT IN ACTION!](https://www.reddit.com/r/3Dprinting/comments/vbh1qf/3d_printed_all_the_parts_almost_for_this_fun/) 8 | 9 | ## Principle behind the project: 10 | 11 | 1. A Pi camera tracks the position of the ball, using OpenCV. A mask is first applied separating the orange color of the ball from the rest. The mask is then eroded twice, blurred, and finally a 'smallest enclosing circle' command returns the radius and the center position. 12 | 13 | 2. This information is then filtered by radius. If it is part of the range of radii that the ball is expected to have, the program calculates the error between the position of the ball and the center of the plate. Three errors total are calculated, one for each servos' line of action. 14 | 15 | 3. These three errors are then input in the three PID Controllers, including past errors and past derivatives. The PID Controllers then output a change of angle for each servo, which is first normalized against computing time to account for small variations, and then sent using AdaFruit's servo library to the motors. 16 | 17 | ## Improvements / Failures 18 | 19 | My current platform is unstable at best, and requires constant recalibration (Done through the GUI); a more stable platform could be obtained with a better interface between the arms. Ball bearings could be used to further smoothen the motion of the platform, and more drastically three more servos could be added for a full on Stewart platform. At this scale however with this level of precision, the current platform is suitable enough. 20 | 21 | One could also use Dynamixel Servos for a more accurate position control, although this would significantly increase the price of the whole assembly. 22 | 23 | ## Parts list : 24 | 25 | - Raspberry Pi 4b 8 gb 26 | - Pi Camera V2 27 | - Adafruit Camera cable extension 28 | - Three DIYMORE Full metal servos 29 | - Waveshare Servo HAT 30 | - Pimoroni Fan Shim 31 | - Three Dexter Cardan Joints (Universal joints) 32 | 33 | The rest of the parts are 3D printed. 34 | 35 | Part selection really doesn't matter that much apart from the RPi and the PiCamera. Use what is available to you for a fair price, and adapt the parts to your needs. 36 | 37 | ## Want to replicate this project yourself ? 38 | 39 | Great idea! If you need any help, feel free to shoot me a message on reddit (/u/parisiancyclist) or Instagram (@nicohmje). You can also directly contact me through GitHub. 40 | 41 | The STL Files folder includes the necessary parts for this project. These should be adapted to your parts and specific setup, and should only serve as a reference. The 3MF Files folder includes the prototypes too, if you want a deeper, in-depth look at the projects evolution. 42 | 43 | Happy building! 44 | 45 | -------------------------------------------------------------------------------- /Python code/Old files/old_revision.py: -------------------------------------------------------------------------------- 1 | from cmath import asin 2 | from re import X 3 | import numpy as np 4 | import cv2 as cv 5 | import time 6 | import tkinter as tk 7 | from adafruit_servokit import ServoKit 8 | 9 | kit = ServoKit(channels = 16) 10 | kit.servo[0].set_pulse_width_range(500, 2500) #Very important 11 | kit.servo[1].set_pulse_width_range(500, 2500) #Very important 12 | kit.servo[2].set_pulse_width_range(500, 2500) #Very important 13 | 14 | center_x = 240 15 | center_y = 240 16 | prevX = 0 17 | prevY = 0 18 | X_integral = 0 19 | Y_integral = 0 20 | X_derivative = 0 21 | Y_derivative = 0 22 | 23 | 24 | def motor_coeff(theta): 25 | motor_pos = np.array([90,330,210]) 26 | erreur = np.array([0,0,0]) 27 | erreur = (theta - motor_pos + 180 + 360) % 360 - 180 28 | 29 | dir = np.ones(3) 30 | dir[abs(erreur)>90] = - 1 #If dir is -1, the motors should lower | Else, they should go up 31 | 32 | coeffs = np.ones(3) 33 | if theta > 180: 34 | theta_inverse = theta - 180 35 | else: 36 | theta_inverse = theta + 180 37 | 38 | coeffs[dir<0] = 1 - abs((theta - motor_pos[dir < 0] + 180 + 360) % 360 - 180) / 90 39 | coeffs[dir>0] = 1 - abs((theta_inverse - motor_pos[dir > 0] + 180 + 360) % 360 - 180) / 90 40 | 41 | coeffs = coeffs * dir 42 | 43 | return(coeffs) 44 | 45 | def moveMotors(theta, radius): 46 | intensity = (radius / 200) * 30 47 | initial_angle = np.array([120,120,110]) 48 | coeffs = motor_coeff(theta) 49 | motor_angles = (coeffs * intensity * -1) + initial_angle 50 | 51 | if max(motor_angles)>170: 52 | motor_angles[np.argmax(motor_angles)] = 170 53 | print("angle too high") 54 | return 55 | if min(motor_angles)<80: 56 | motor_angles[np.argmin(motor_angles)] = 80 57 | print("angle too low") 58 | return 59 | for i in np.arange(0,3): 60 | kit.servo[i].angle = motor_angles[i] 61 | 62 | def initialPos(): 63 | kit.servo[0].angle = 120 64 | kit.servo[1].angle = 120 65 | kit.servo[2].angle = 110 66 | 67 | cap = cv.VideoCapture(0) 68 | 69 | def CartesianToPolar(x,y, xcenter, ycenter): 70 | 71 | r = np.sqrt((xcenter - x)**2 + (ycenter - y)**2) 72 | theta_1 = np.arccos((x-xcenter) / r) 73 | theta_2 = np.arcsin((y-ycenter)/ r) 74 | 75 | if x > xcenter: 76 | if y > ycenter: 77 | theta = theta_1 78 | else: 79 | theta = 2*np.pi - abs(theta_2) 80 | else: 81 | if y > ycenter: 82 | theta = theta_1 83 | else: 84 | theta = np.pi + abs(theta_2) 85 | 86 | theta = (180 / np.pi) * theta 87 | return(r,theta) 88 | 89 | 90 | 91 | def ballFinder(): 92 | _, frame = cap.read() 93 | frame = frame[:, 93:550, :] 94 | hsv = cv.cvtColor(frame, cv.COLOR_BGR2HSV) 95 | lower_value = np.array([00,100,100]) 96 | higher_value = np.array([50,255,255]) 97 | mask = cv.inRange(hsv, lower_value, higher_value) 98 | contours, _ = cv.findContours(mask, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE) 99 | result = (0,0) 100 | for cnt in contours: 101 | area = cv.contourArea(cnt) 102 | if area > 1500: 103 | (x,y), radius = cv.minEnclosingCircle(cnt) 104 | x = int(x) 105 | y = 480 - int(y) 106 | radius = int(radius) 107 | if radius > 20: 108 | result = (x,y) 109 | return(result) 110 | 111 | def CenterCalibration(): 112 | for i in np.arange(0,150): 113 | _, frame = cap.read() 114 | frame = frame[:,93:550,:] 115 | cv.circle(frame,(center_x,center_y),3,[255,130,130],3) 116 | cv.imshow("Calibration", frame) 117 | cv.waitKey(1) 118 | 119 | 120 | 121 | def pid(x,y): 122 | global center_x 123 | global center_y 124 | global Kp, Ki, Kd 125 | global X_integral, Y_integral, X_derivative, Y_derivative 126 | Kp = 0.4 127 | Ki = 0.0025 128 | Kd = 5 129 | if abs(Ki*X_integral) > 400: 130 | Ki = 0 131 | if abs(Ki*Y_integral) > 400: 132 | Ki = 0 133 | pid_X = Kp * (center_x - x) + Ki * X_integral + Kd * X_derivative 134 | pid_Y = Kp * (center_y - y) + Ki * Y_integral + Kd * Y_derivative 135 | 136 | 137 | return (pid_X, pid_Y) 138 | 139 | initialPos() 140 | CenterCalibration() 141 | cv.destroyAllWindows() 142 | while True: 143 | x, y = ballFinder() 144 | #x,y = 250, 400 145 | #start = time.time() 146 | if x != y != 0: 147 | futureX, futureY = pid(x,y) 148 | radius, theta = CartesianToPolar(futureX, futureY, 0,0) 149 | moveMotors(theta, radius) 150 | X_derivative = prevX - x 151 | Y_derivative = prevY - y 152 | X_integral += (center_x - x) 153 | Y_integral += (center_y - y) 154 | prevX = x 155 | prevY = y 156 | 157 | #print(theta, radius) 158 | else : 159 | initialPos() 160 | X_integral = 0 161 | Y_integral = 0 162 | #stop = time.time() 163 | #print(stop - start) 164 | 165 | cap.release() 166 | 167 | -------------------------------------------------------------------------------- /Python code/Working Files/main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | import time 4 | import tkinter as tk 5 | from adafruit_servokit import ServoKit 6 | 7 | #Initialize the three servos 8 | kit = ServoKit(channels = 16) 9 | kit.servo[0].set_pulse_width_range(500, 2500) #Very important 10 | kit.servo[1].set_pulse_width_range(500, 2500) #Very important 11 | kit.servo[2].set_pulse_width_range(500, 2500) #Very important 12 | 13 | #Center coordinates 14 | center_x = 250 15 | center_y = 250 16 | 17 | prevX = 0 18 | prevY = 0 19 | 20 | #Min and max angles of each motor 21 | min_motor = np.array([85,85,77]) 22 | max_motor = np.array([175,175,165]) 23 | 24 | #Sets the angle of the motors to the chosen values 25 | def set_angle(): 26 | global initial_angle 27 | initial_angle[0] = e0.get() 28 | initial_angle[1] = e1.get() 29 | initial_angle[2] = e2.get() 30 | initialPos() 31 | 32 | initial_angle = np.array([124,120,111]) #These depend on how level the surface is 33 | 34 | #Initial position of the motors 35 | def initialPos(): 36 | kit.servo[0].angle = int(e0.get()) 37 | kit.servo[1].angle = int(e1.get()) 38 | kit.servo[2].angle = int(e2.get()) 39 | 40 | #Initialize the capture 41 | cap = cv.VideoCapture(0) 42 | 43 | #Moves the motor according to the instructions given by the PID 44 | def move_motors(pid): 45 | for i in np.arange(0,3,1): 46 | angle = initial_angle[i] - (pid[i]/150.0 * 20) 47 | angle = np.floor(angle) 48 | if angle > max_motor[i]: 49 | print("angle limit high") 50 | angle = max_motor[i] 51 | if angle < min_motor[i]: 52 | print("angle limit low") 53 | angle = min_motor[i] 54 | kit.servo[i].angle = angle 55 | 56 | #Finds the ball's center 57 | def ballFinder(): 58 | ret, frame = cap.read() 59 | result = (0,0) 60 | if ret: 61 | frame = frame[:, 93:550, :] 62 | hsv = cv.cvtColor(frame, cv.COLOR_BGR2HSV) 63 | lower_value = np.array([00,80,100]) 64 | higher_value = np.array([50,255,255]) 65 | mask = cv.inRange(hsv, lower_value, higher_value) 66 | mask = cv.blur(mask,(6,6)) 67 | mask = cv.erode(mask, None, iterations=2) 68 | mask = cv.dilate(mask, None, iterations=2) 69 | contours, _ = cv.findContours(mask, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE) 70 | for cnt in contours: 71 | area = cv.contourArea(cnt) 72 | if area > 1500: 73 | (x,y), radius = cv.minEnclosingCircle(cnt) 74 | x = int(x) 75 | y = 480 - int(y) #In images, y=0 is on top, not on the bottom 76 | radius = int(radius) 77 | if radius > 20: 78 | result = (x,y) 79 | return(result) 80 | 81 | #This allows you to properly place the plate beneath the camera 82 | def CenterCalibration(): 83 | for i in np.arange(0,150): 84 | ret, frame = cap.read() 85 | if ret: 86 | frame = frame[:,93:550,:] 87 | cv.circle(frame,(center_x,center_y),3,[255,130,130],3) 88 | cv.imshow("Calibration", frame) 89 | cv.waitKey(1) 90 | else: 91 | pass 92 | 93 | 94 | started = False 95 | #GUI and looping 96 | def start_program(): 97 | global started,i 98 | set_values() 99 | reset_values() 100 | if started: 101 | start["text"] = "Start" 102 | started = False 103 | else: 104 | start["text"] = "Stop" 105 | started = True 106 | 107 | errors = np.zeros(3) 108 | pasterrors = np.zeros(3) 109 | #Calculates the error for each line of action of each servo 110 | def get_errors(x,y): 111 | global center_x, center_y, errors, pasterrors 112 | print(center_x, center_y) 113 | pasterrors = np.copy(errors) 114 | errors[0] = y - center_y 115 | errors[1] = (np.sqrt(3)*(x-center_x) + (center_y - y))/2.0 116 | errors[2] = (np.sqrt(3)*(center_x - x) + (center_y - y))/2. 117 | #errors[abs(errors - pasterrors) > 40] = pasterrors[abs(errors - pasterrors) > 40] 118 | #print(abs(errors - pasterrors)) 119 | print(errors[0]) 120 | 121 | lastderivative = 0 122 | lastlastderivative = 0 123 | integral = 0 124 | derivative = 0 125 | pid = 0 126 | #Calculates the PID and normalizes it based on computation time 127 | def pid_control(compT): 128 | global errors, pasterrors 129 | global lastderivative, lastlastderivative, derivative 130 | global Kp, Ki, Kd, Ka 131 | global integral 132 | global lastpid 133 | global computation_time 134 | #output == Kp * error + Ki * integral + Kd * derivative 135 | global pid 136 | if np.all(errors > 10): 137 | integral += errors * 0.5 138 | else: 139 | integral = 0 140 | Kd_normalized = np.copy(Kd) / (compT/np.mean(computation_time)) 141 | Ka_normalized = np.copy(Ka) * (compT/np.mean(computation_time)) 142 | Kp_normalized = np.copy(Kp) 143 | Kp_normalized[np.abs(errors) < 20] = Kp_normalized[np.abs(errors) < 20] * 0.5 144 | lastlastderivative = np.copy(lastderivative) 145 | lastderivative = np.copy(derivative) 146 | derivative = (errors - pasterrors) 147 | #print("SUM",np.sum(abs(derivative))) 148 | if np.sum(abs(derivative)) < 10: 149 | Kd_normalized = Kd_normalized * (0.1*(np.sum(abs(derivative)))) 150 | elif np.sum(abs(derivative)) > 20: 151 | Kd_normalized = Kd_normalized * 1.1 152 | if np.all(np.abs(derivative) < 0.5): 153 | Kd_normalized = 0 154 | lastpid = pid 155 | pid = Kp_normalized * errors + Ki * integral + Kd_normalized * (((5*derivative) + (3*lastderivative) + (2*lastlastderivative))/10.0) + Ka_normalized * (((derivative - lastderivative) + (lastderivative - lastlastderivative))/2.0) 156 | return pid 157 | 158 | 159 | Kp = np.zeros(3) 160 | Kd = np.zeros(3) 161 | Ka = np.zeros(3) 162 | Ki = np.zeros(3) 163 | 164 | #Resets the values, if the ball leaves the plate 165 | def reset_values(): 166 | global derivative, lastderivative, lastlastderivative, i 167 | global pasterrors, errors, pid 168 | derivative = 0 169 | lastderivative = 0 170 | lastlastderivative = 0 171 | pid = 0 172 | i = 0 173 | print('successfully reset all values') 174 | 175 | #sets the coefficients 176 | def set_values(): 177 | global Kp, Ki, Kd, Ka 178 | i = int(motor_selector.get()) 179 | if i == 3: 180 | Kp[:] = p_slider.get() / 10.0 181 | Ki[:] = i_slider.get() / 10.0 182 | Kd[:] = d_slider.get() 183 | Ka[:] = a_slider.get() 184 | else: 185 | Kp[i] = p_slider.get() / 10.0 186 | Ki[i] = i_slider.get() / 10.0 187 | Kd[i] = d_slider.get() 188 | Ka[i] = a_slider.get() 189 | print(Kp, Ki, Kd, Ka) 190 | 191 | t = 0 192 | #Refreshes the 'ball position' graph 193 | def refresh(x, y): 194 | global t 195 | graphWindow.deiconify() 196 | graphCanvas.create_oval(x,y,x,y, fill="#b20000", width=4) 197 | graphCanvas.create_line(0,240,480,240, fill="#0069b5", width=2) 198 | graphCanvas.create_line(240,0,240,480, fill="#0069b5", width=2) 199 | if t >= 480: 200 | t = 0 201 | graphCanvas.delete("all") 202 | graphCanvas.create_line(0,240,480,240, fill="#0069b5", width=2) 203 | graphCanvas.create_line(240,0,240,480, fill="#0069b5", width=2) 204 | graphCanvas.create_oval(x,y,x+1,y, fill="#b20000") 205 | t += 4 206 | 207 | computation_time = [] 208 | #Main loop 209 | def main(): 210 | global prevX, prevY, computation_time 211 | global center_x, center_y 212 | start_time = time.time() 213 | if started: 214 | x, y = ballFinder() 215 | #print("posiiton", x,y) 216 | if x != y != 0: 217 | detected = True 218 | get_errors(x,y) 219 | compT = (time.time() - start_time) 220 | computation_time.append(compT) 221 | pid = pid_control(compT) 222 | #print(pid[0]) 223 | #print(compT / np.mean(computation_time)) 224 | 225 | move_motors(pid) 226 | prevX = x 227 | prevY = y 228 | else: 229 | initialPos() 230 | refresh(x, y) 231 | lmain.after(1,main) 232 | 233 | 234 | #GUI PART 235 | 236 | window = tk.Tk() 237 | window.geometry("820x500") 238 | window.title("Test ?") 239 | 240 | p_slider = tk.Scale(window, from_=0, to=15, orient="horizontal", label="Proportionnal", length=500, tickinterval=2.5, resolution=0.1) 241 | p_slider.set(3) 242 | p_slider.pack() 243 | i_slider = tk.Scale(window, from_=0, to=1, orient="horizontal", label="Integral", length=500, tickinterval=0.25, resolution=0.005) 244 | i_slider.set(0) 245 | i_slider.pack() 246 | d_slider = tk.Scale(window, from_=0, to=10, orient="horizontal", label="Derivative", length=500, tickinterval=10, resolution=0.1) 247 | d_slider.set(6.2) 248 | d_slider.pack() 249 | a_slider = tk.Scale(window, from_=0, to=10, orient="horizontal", label="Double Derivative", length=500, tickinterval=10, resolution=0.1) 250 | a_slider.set(5.6) 251 | a_slider.pack() 252 | 253 | p_slider.place(x=250, y= 0) 254 | i_slider.place(x=250, y= 100) 255 | d_slider.place(x=250, y= 200) 256 | a_slider.place(x=250, y= 300) 257 | 258 | tk.Label(window, text="Motor0").place(x=00,y=20) 259 | tk.Label(window, text="Motor1").place(x=00,y=50) 260 | tk.Label(window, text="Motor2").place(x=00,y=80) 261 | 262 | 263 | lmain = tk.Label(window) 264 | lmain.pack() 265 | 266 | graphWindow = tk.Toplevel(window) 267 | graphWindow.title('Ball position') 268 | graphCanvas = tk.Canvas(graphWindow,width=480,height=480) 269 | graphCanvas.pack() 270 | 271 | motor0 = tk.StringVar(window) 272 | motor1 = tk.StringVar(window) 273 | motor2 = tk.StringVar(window) 274 | motor_select = tk.StringVar(window) 275 | motor0.set(str(initial_angle[0])) 276 | motor1.set(str(initial_angle[1])) 277 | motor2.set(str(initial_angle[2])) 278 | motor_select.set("3") 279 | 280 | e0 = tk.Spinbox(window, from_=100, to=200, command=set_angle, width=3, textvariable=motor0) 281 | e1 = tk.Spinbox(window, from_=100, to=200, command=set_angle, width=3, textvariable=motor1) 282 | e2 = tk.Spinbox(window, from_=100, to=200, command=set_angle, width=3, textvariable=motor2) 283 | 284 | motor_selector = tk.Spinbox(window, from_=0, to=3, width=2, textvariable=motor_select) 285 | 286 | motor_selector.place(x=100, y= 100) 287 | 288 | e0.place(x=50, y=20) 289 | e1.place(x=50, y = 50 ) 290 | e2.place(x=50, y = 80) 291 | 292 | 293 | 294 | start = tk.Button(window, text="Start" ,command=start_program) 295 | start.place(x=20, y=225) 296 | Breset = tk.Button(window, text="Reset", command=reset_values) 297 | Breset.place(x=20, y= 250) 298 | set = tk.Button(window, text='set values', command=set_values) 299 | set.place(x=400, y = 350 ) 300 | 301 | #END OF GUI 302 | 303 | #Final order of operations : 304 | initialPos() 305 | CenterCalibration() 306 | cv.destroyAllWindows() 307 | main() 308 | window.mainloop() 309 | 310 | --------------------------------------------------------------------------------