From 69144d872e466f46e9ce75445a94480588504547 Mon Sep 17 00:00:00 2001 From: Wout Rombouts Date: Sat, 13 May 2023 19:51:19 +0200 Subject: [PATCH] Final changes --- Software/install/requirements.txt | 1 + Software/threads/customthreads.py | 200 ++++++++++++++++++------------ Software/threads/main.py | 12 +- 3 files changed, 127 insertions(+), 86 deletions(-) diff --git a/Software/install/requirements.txt b/Software/install/requirements.txt index 7ae4494..11134fb 100644 --- a/Software/install/requirements.txt +++ b/Software/install/requirements.txt @@ -2,6 +2,7 @@ simple_pid adafruit-circuitpython-bno055 adafruit-circuitpython-pca9685 adafruit-circuitpython-motor +adafruit-circuitpython-hcsr04 pyjoystick jetson-stats inquirer diff --git a/Software/threads/customthreads.py b/Software/threads/customthreads.py index 70a0537..f4b9e68 100644 --- a/Software/threads/customthreads.py +++ b/Software/threads/customthreads.py @@ -20,6 +20,7 @@ from collections import deque from busio import I2C from adafruit_bno055 import BNO055_I2C +from adafruit_hcsr04 import HCSR04 import pyjoystick from pyjoystick.sdl2 import run_event_loop from jtop import jtop @@ -299,78 +300,86 @@ def run(self): time.sleep(0.5) continue - if camera.snap_image(1): - # Load a frame (or image) - video = camera.get_image() - - X_lb=0 - Y_lb=0 - X_rb=0 - Y_rb=0 + try: + + if camera.snap_image(1): + # Load a frame (or image) + video = camera.get_image() - h1= 0 - h2= 0 + X_lb=0 + Y_lb=0 + X_rb=0 + Y_rb=0 - #Specifying upper and lower ranges of color to detect in hsv format - lower = numpy.array([137, 0 ,17 ]) - upper = numpy.array([180, 255 ,255 ]) - lower_2 = numpy.array([0, 0 ,0 ]) - upper_2 = numpy.array([0, 0 ,0 ]) + h1= 0 + h2= 0 - hight, width, _ = video.shape #Get resolution + #Specifying upper and lower ranges of color to detect in hsv format + lower = numpy.array([160, 100 ,16 ]) + upper = numpy.array([180, 255 ,255 ]) + lower_2 = numpy.array([0, 137 ,68 ]) + upper_2 = numpy.array([5, 255 ,255]) - video_cropped = video[307:hight, 0:width] + hight, width, _ = video.shape #Get resolution - video_hsv = cv2.cvtColor(video_cropped, cv2.COLOR_BGR2HSV) #Converting BGR image to HSV format + video_cropped = video[353:hight, 0:width] - mask_video_1 = cv2.inRange(video_hsv, lower, upper) # Masking the image to find our color - mask_video_2 = cv2.inRange(video_hsv, lower_2, upper_2) + video_hsv = cv2.cvtColor(video_cropped, cv2.COLOR_BGR2HSV) #Converting BGR image to HSV format - mask_video = mask_video_1 | mask_video_2 + mask_video_1 = cv2.inRange(video_hsv, lower, upper) # Masking the image to find our color + mask_video_2 = cv2.inRange(video_hsv, lower_2, upper_2) - mask_leftBand = mask_video[0:hight, int(200):int(250)] #Crop right band - mask_rightBand = mask_video[0:hight, int(width-250):int(width-200)] #crop left band + mask_video = mask_video_1 | mask_video_2 - mask_contours_leftBand, hierarchy = cv2.findContours(mask_leftBand, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) #Finding contours in mask image (leftband) - mask_contours_rightBand, hierarchy = cv2.findContours(mask_rightBand, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) #Finding contours in mask image (rightband) + mask_leftBand = mask_video[0:hight, int(100):int(120)] #Crop right band + mask_rightBand = mask_video[0:hight, int(width-120):int(width-100)] #crop left band - # Finding position of all contours of the bands - if len(mask_contours_leftBand or mask_contours_rightBand) != 0: - for i_lb, k_rb in zip(mask_contours_leftBand, mask_contours_rightBand): - if cv2.contourArea(i_lb) > 500: - x1, y1, w1, h1 = cv2.boundingRect(i_lb) - X_lb = int(x1+(w1/2)) #calculate center of rectangle - Y_lb = int(y1+(h1/2)) - cv2.rectangle(video_cropped, (x1+200, y1), (x1 + w1+200, y1 + h1), (0, 0, 255), 3) #drawing rectangle + mask_contours_leftBand, hierarchy = cv2.findContours(mask_leftBand, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) #Finding contours in mask image (leftband) + mask_contours_rightBand, hierarchy = cv2.findContours(mask_rightBand, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) #Finding contours in mask image (rightband) + + # Finding position of all contours of the bands + if len(mask_contours_leftBand or mask_contours_rightBand) != 0: + for i_lb, k_rb in zip(mask_contours_leftBand, mask_contours_rightBand): + if cv2.contourArea(i_lb) > 500: + x1, y1, w1, h1 = cv2.boundingRect(i_lb) + cv2.rectangle(video_cropped, (x1+200, y1), (x1 + w1+200, y1 + h1), (0, 0, 255), 3) #drawing rectangle + + if cv2.contourArea(k_rb) > 500: + x2, y2, w2, h2 = cv2.boundingRect(k_rb) + cv2.rectangle(video_cropped, ((width-x2-250), y2), (((width-x2-250) + w2), (y2 + h2)), (255, 0, 0), 3) #drawing rectangle (formula needed, otherwise it will add it on the left side) + + diff = 1 + if h1 and h2 !=0: + if h1h2: + diff = h2/h1 + + if (diff > 0.9) and (diff < 1): + print("Go straight") + angle= 90 + if h1>h2: #h1>h2 + angle = 116.47*diff - 26.47 + print("Go right, with angle:", angle) + if h1 180: + angle = 180 - if cv2.contourArea(k_rb) > 500: - x2, y2, w2, h2 = cv2.boundingRect(k_rb) - X_rb = int(width-(x2+(w2/2))) #calculate center of rectangle - Y_rb = int(y2+(h2/2)) - cv2.rectangle(video_cropped, ((width-x2-250), y2), (((width-x2-250) + w2), (y2 + h2)), (255, 0, 0), 3) #drawing rectangle (formula needed, otherwise it will add it on the left side) - + # Append result to pipe + data = { + 'time': datetime.now(), + 'angle': angle + } + self.pipe.append(data) - diff = 1 - if h2 != 0: - diff = h1/h2 - if (diff > 0.9) and (diff < 1.1): - angle = 90 - print("Go straight") - if diff > 1.1: #h1>h2 - norm = 1-(h1-h2)/h1 - angle = 90*norm - print("Go right, with angle:", angle) - if diff < 0.9: #h1

500: - x1, y1, w1, h1 = cv2.boundingRect(i_lb) - cv2.rectangle(video_cropped, (x1+200, y1), (x1 + w1+200, y1 + h1), (0, 0, 255), 3) #drawing rectangle - - if cv2.contourArea(k_rb) > 500: - x2, y2, w2, h2 = cv2.boundingRect(k_rb) - X_rb = int(width-(x2+(w2/2))) #calculate center of rectangle - Y_rb = int(y2+(h2/2)) - cv2.rectangle(video_cropped, ((width-x2-250), y2), (((width-x2-250) + w2), (y2 + h2)), (255, 0, 0), 3) #drawing rectangle (formula needed, otherwise it will add it on the left side) - - angle1 = 1269.3 * math.exp(-0.008 * h1) - angle2 = 1269.3 * math.exp(-0.008 * h2) - - angle = (angle1 + angle2) / 2 - #angle = 2*10^-9 * h1^4 - 6*10^-6 * h1^3 + 0.0071*h1^2 - 3.6433*h1 + 760.07 + if len(mask_contours_leftBand) != 0: + for i_lb in mask_contours_leftBand: + x1, y1, w1, h1 = cv2.boundingRect(i_lb) + cv2.rectangle(video_cropped, (x1+200, y1), (x1 + w1+200, y1 + h1), (0, 0, 255), 3) #drawing rectangle + + angle = -0.2705*h1 + 204.54 if angle < 0: angle = 0 @@ -543,4 +539,48 @@ def __save(self): writer = csv.writer(file, delimiter="|") writer.writerow(list(pipe[0].keys())) for item in pipe: - writer.writerow(item.values()) \ No newline at end of file + writer.writerow(item.values()) + + +class DistanceProcessingThread(Thread): + def __init__(self, pipe, stop_event, init_event, pause_event): + super(DistanceProcessingThread, self).__init__(name="DistanceProcessingThread") + + self.pipe: deque = pipe + self.stop_event: Event = stop_event + self.init_event: Event = init_event + self.pause_event: Event = pause_event + + def run(self): + try: + logging.getLogger() + + logging.info("Initialising Distance Processing...") + + with HCSR04(trigger_pin=board.D16, echo_pin=board.D19) as sonar: + + self.init_event.set() + + logging.info("Distance Processing initialised.") + while not self.stop_event.is_set(): + if self.pause_event.is_set(): + time.sleep(0.5) + continue + + print(sonar.distance) + + """ + # Append result to pipe + data = { + 'time': datetime.now(), + 'angle': None + } + self.pipe.append(data) + """ + + except Exception as exception: + logging.error(exception) + traceback.print_exc() + self.stop_event.set() + finally: + logging.info("Stopped Distance Processing") \ No newline at end of file diff --git a/Software/threads/main.py b/Software/threads/main.py index f04f9b7..d0d20ce 100644 --- a/Software/threads/main.py +++ b/Software/threads/main.py @@ -15,7 +15,7 @@ from simple_pid import PID from pyjoystick.sdl2 import run_event_loop -from customthreads import IMUThread, EncoderThread, StatsThread, SideImageProcessingThread, DataCollectionThread, ImageProcessingThread +from customthreads import IMUThread, EncoderThread, StatsThread, DistanceProcessingThread, LowImageProcessingThread, SideImageProcessingThread, DataCollectionThread, ImageProcessingThread class McQueen: def __init__(self): @@ -67,7 +67,7 @@ def __init__(self): self.servo_pid.sample_time = 0.1 # Max safe speed = 0.3, Slow = 0.1, AVG = 0.2 self.motor_pid = PID(1, 0, 0, setpoint=0.2) - self.motor_pid.output_limits = (0, 0.2) + self.motor_pid.output_limits = (0, 0) self.motor_pid.sample_time = 0.1 logging.info("Starting controller") @@ -119,7 +119,7 @@ def process_control_loops(self): self.__cycle_loop_steering() def __cycle_loop_motor(self): - self.actuator_motor.throttle = 0.15 #self.motor_pid(self.velocity) + self.actuator_motor.throttle = 0#self.motor_pid(self.velocity) def __cycle_loop_steering(self): if len(self.pipe_imageprocessing) > 0: @@ -269,7 +269,7 @@ def threads_init(self): self.init_event_stats = Event() self.init_event_imageprocessing = Event() self.init_event_datacollection = Event() - self.init_events.append(self.init_event_imu) + #self.init_events.append(self.init_event_imu) self.init_events.append(self.init_event_encoder) self.init_events.append(self.init_event_stats) self.init_events.append(self.init_event_imageprocessing) @@ -290,10 +290,10 @@ def threads_init(self): } self.threads = [] - self.threads.append(IMUThread(self.pipe_imu, self.stop_event, self.init_event_imu, self.pause_event)) + #self.threads.append(IMUThread(self.pipe_imu, self.stop_event, self.init_event_imu, self.pause_event)) self.threads.append(EncoderThread(self.pipe_encoder, self.stop_event, self.init_event_encoder, self.pause_event)) self.threads.append(StatsThread(self.pipe_stats, self.stop_event, self.init_event_stats, self.pause_event)) - self.threads.append(SideImageProcessingThread(self.pipe_imageprocessing, self.stop_event, self.init_event_imageprocessing, self.pause_event)) + self.threads.append(DistanceProcessingThread(self.pipe_imageprocessing, self.stop_event, self.init_event_imageprocessing, self.pause_event)) self.threads.append(DataCollectionThread(None, self.stop_event, self.init_event_datacollection, self.pause_event, self.pipes)) def threads_start(self):