class Controller(): mode = None button = 0 def __init__(self): self.last_text = '' self.bot = Robot() interval = 0.0 self.w = 320 self.h = 240 self.camera = Camera(self.w, self.h) super().__init__() def run(self): self.show('Started') cv.namedWindow('image', cv.WND_PROP_FULLSCREEN) cv.setWindowProperty('image', cv.WND_PROP_FULLSCREEN, cv.WINDOW_FULLSCREEN) menu_img = self.loadMenuImage('/home/pi/Pictures/Menu.jpg') notfound_img = self.loadMenuImage('/home/pi/Pictures/Joystick.jpg') camera_img = self.loadMenuImage('/home/pi/Pictures/Camera.jpg') remote_img = self.loadMenuImage( '/home/pi/Pictures/Remote-Controlled.jpg') lava_img = self.loadMenuImage('/home/pi/Pictures/Lava-Palava.jpg') mine_img = self.loadMenuImage('/home/pi/Pictures/Minesweeper.jpg') maze_img = self.loadMenuImage('/home/pi/Pictures/Maze.jpg') exit_img = self.loadMenuImage('/home/pi/Pictures/Exit.jpg') halt_img = self.loadMenuImage('/home/pi/Pictures/Halt.jpg') test_img = self.loadMenuImage('/home/pi/Pictures/Testing.jpg') track_img = self.loadMenuImage('/home/pi/Pictures/Track.jpg') reboot_img = self.loadMenuImage('/home/pi/Pictures/Reboot.jpg') gear = 2 menu = 1 MIN_MENU = 1 MAX_MENU = 8 running = True timing = 20000 while running: try: self.show('Press CTRL+C to quit') self.showMenuImage(menu_img) cv.waitKey(1) with ControllerResource() as joystick: if joystick.connected: # Set previous menu item prev = 0 menu = 1 # Loop indefinitely while running: presses = joystick.check_presses() if presses.select: # Ensure we have the camera attached for this challenges if (menu == 2 or menu == 3 or menu == 4) and ( self.camera.hasCamera == False): self.showMenuImage(camera_img) elif menu == 7: self.testing(test_img) prev = 7 else: self.showMenuImage(menu_img) running = self.doMenu( menu, joystick, gear, test_img) if menu == 1: self.showMenuImage(remote_img) if menu == 2: self.showMenuImage(lava_img) if menu == 3: self.showMenuImage(mine_img) if menu == 4: self.showMenuImage(maze_img) if menu == 5: self.showMenuImage(exit_img) if menu == 6: self.showMenuImage(halt_img) if menu == 7: self.showMenuImage(test_img) if menu == 8: self.showMenuImage(reboot_img) prev = 0 if joystick.presses.dright: menu += 1 if menu > MAX_MENU: menu = MIN_MENU if joystick.presses.dleft: menu -= 1 if menu < MIN_MENU: menu = MAX_MENU if prev != menu: #print(" Menu = {}".format(menu)) if menu == 1: self.showMenuImage(remote_img) if menu == 2: self.showMenuImage(lava_img) if menu == 3: self.showMenuImage(mine_img) if menu == 4: self.showMenuImage(maze_img) if menu == 5: self.showMenuImage(exit_img) if menu == 6: self.showMenuImage(halt_img) if menu == 7: self.showMenuImage(test_img) if menu == 8: self.showMenuImage(reboot_img) prev = menu # Select menu option # time.sleep(INTERVAL) except IOError: LOGGER.info('Unable to find joystick') self.showMenuImage(notfound_img) time.sleep(4.0) except KeyboardInterrupt: # CTRL+C exit, disable all drives self.bot.move(0, 0) self.show('Motors off') break cv.destroyAllWindows() def doMenu(self, menu, joystick, gear, image): if menu == 1: self.remoteNoCamera(joystick, gear) return True if menu == 2: self.straight(joystick, gear) return True if menu == 3: self.mine(joystick, gear) return True if menu == 4: self.maze(joystick, gear, image) return True if menu == 5: return False if menu == 6: self.shutdown() # We don't expect to get here return False if menu == 7: self.testing(image) return True if menu == 8: self.reboot() # We don't expect to get here return False def reboot(self): self.show("Reboot") os.system("sudo reboot") def shutdown(self): self.show("Shutdown") os.system("sudo halt") def testing(self, image): image = self.loadMenuImage('/home/pi/Pictures/Testing.jpg') temp = self.bot.temperature() volts = self.bot.battery() cv.putText(image, temp, (50, 60), cv.FONT_HERSHEY_TRIPLEX, 2, (0, 0, 255), 5) cv.putText(image, volts, (50, 120), cv.FONT_HERSHEY_TRIPLEX, 2, (0, 0, 255), 5) self.showMenuImage(image) # Pi Noon, Zombie Shoot, Temple of Doom, Eco Disaster def remoteNoCamera(self, joystick, gear): self.show("Remote no Camera mode") self.bot.servo_off() count = 0 while True: presses = joystick.check_presses() if presses.home: #self.show('HOME pressed since last check') running = False break count += 1 left_drive = joystick.ly right_drive = joystick.ry self.bot.move(left_drive, right_drive, gear) if joystick.presses.cross: self.bot.trigger(90) time.sleep(0.25) if joystick.presses.triangle: self.bot.trigger(-90) time.sleep(0.25) if joystick.presses.dup: self.bot.tilt(30) time.sleep(0.25) if joystick.presses.ddown: self.bot.tilt(-30) time.sleep(0.25) self.bot.servo_off() prev = gear if joystick.presses.l1: gear += 0.5 if joystick.presses.r1: gear -= 0.5 if gear < 1: gear = 1 if gear > 5: gear = 5 if gear != prev: print(" Gear = {}".format(gear)) self.bot.move(left_drive, right_drive, gear) self.bot.stop() def remoteWithCamera(self, joystick, gear): self.show("Remote with Camera mode") if self.camera.hasCamera == False: return rawCapture = PiRGBArray(self.camera, size=(self.w, self.h)) time.sleep(0.1) count = 0 for frame in self.camera.CaptureContinous(rawCapture): presses = joystick.check_presses() if presses.home: #self.show('HOME pressed since last check') running = False break image = frame.array heading, roll, pitch = self.bot.readEuler() distance = self.bot.getDistance() count = 0 text = "gear={0} : angle={1:5.2} : mm={2}".format( gear, heading, distance) cv.putText(image, text, (10, 20), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255)) count += 1 left_drive = joystick.ly right_drive = joystick.ry self.bot.move(left_drive, right_drive, gear) if joystick.presses.cross: self.bot.trigger(90) if joystick.presses.triangle: self.bot.trigger(-90) if joystick.presses.dup: self.bot.tilt(-10) if joystick.presses.ddown: self.bot.tilt(10) if joystick.presses.dright: self.bot.pan(-5) if joystick.presses.dleft: self.bot.pan(5) prev = gear if joystick.presses.l1: gear += 0.5 if joystick.presses.r1: gear -= 0.5 if gear < 1: gear = 1 if gear > 5: gear = 5 #if gear != prev: # print(" Gear = {}".format(gear)) self.showMenuImage(image) rawCapture.truncate(0) self.bot.move(left_drive, right_drive, gear) self.bot.stop() def maze(self, joystick, gear, image): self.show("Escape Route mode") image = self.loadMenuImage('/home/pi/Pictures/Track.jpg') #if self.camera.hasCamera == False: # return #rawCapture = PiRGBArray( self.camera, size=(self.w, self.h)) #time.sleep(0.1) # State 0 # Travel until 276 # State 1 # Turn to 90 # State 2 # Travel until 174 # State 3 # Turn to 180 # State 4 # Travel until 276 # State 5 # Turn to 90 # State 6 # Travel until 174 # State 7 # Turn to 0 # State 8 # Travel to 276 # State 9 # Turn to 90 # State 10 # Travel until 100 left_drive = 1 right_drive = 1 firsttime = True offset = 0 gear = 2 state = 0 running = False count = 0 turn_speed = 0.8 target = 0.0 adjust = 0.0 while True: # for frame in self.camera.CaptureContinous(rawCapture): presses = joystick.check_presses() if presses.home: running = False break if presses.start: running = not running #image = frame.array # Read the Euler angles for heading, roll, pitch (all in degrees). heading, roll, pitch = self.bot.readEuler() distance = self.bot.getDistance() # If heading just less than 360 then create an offset if firsttime and heading > 270: offset = 360 - heading firsttime = False # heading += offset # Target 0/360 if state == 0 and distance > 276: adjust, left_drive, right_drive = self.getAdjustedDrive( 0.0, heading, distance) elif state == 0 and distance <= 276: # Red 438, 434 - 208, 434 cv.line(image, (438, 434), (208, 434), (0, 0, 255), 5) left_drive = 0 right_drive = 0 state = 1 # Turn Right - Target 90 elif state == 1 and (heading > 270 or heading < 90): left_drive = turn_speed right_drive = -turn_speed elif state == 1 and (heading >= 90 and heading < 180): left_drive = 0 right_drive = 0 cv.circle(image, (208, 434), 8, (255, 0, 0), 8) state = 2 elif state == 2 and distance > 174: adjust, left_drive, right_drive = self.getAdjustedDrive( 90.0, heading, distance) elif state == 2 and distance <= 174: # 208, 434 - 208, 298 cv.line(image, (208, 434), (208, 298), (0, 0, 255), 5) left_drive = 0 right_drive = 0 state = 3 # Turn Right - Target 180 elif state == 3 and heading < 180: left_drive = turn_speed right_drive = -turn_speed elif state == 3 and heading >= 180: left_drive = 0 right_drive = 0 cv.circle(image, (208, 298), 8, (255, 0, 0), 8) state = 4 elif state == 4 and distance > 100: adjust, left_drive, right_drive = self.getAdjustedDrive( 180.0, heading, distance) elif state == 4 and distance <= 100: # 208, 298 - 404, 298 cv.line(image, (208, 298), (404, 298), (0, 0, 255), 5) left_drive = 0 right_drive = 0 state = 5 # Turn Left - Target 90 elif state == 5 and heading > 90: left_drive = -turn_speed right_drive = turn_speed elif state == 5 and heading <= 90: left_drive = 0 right_drive = 0 cv.circle(image, (404, 298), 8, (255, 0, 0), 8) state = 6 elif state == 6 and distance > 174: adjust, left_drive, right_drive = self.getAdjustedDrive( 90.0, heading, distance) elif state == 6 and distance <= 174: # 404, 298 - 404, 142 cv.line(image, (404, 298), (404, 142), (0, 0, 255), 5) left_drive = 0 right_drive = 0 state = 7 # Turn Left - Target 0/360 elif state == 7 and (heading >= 0 and heading < 90): left_drive = -turn_speed right_drive = turn_speed elif state == 7 and (heading >= 270 and heading <= 360): left_drive = 0 right_drive = 0 cv.circle(image, (404, 142), 8, (255, 0, 0), 8) state = 8 elif state == 8 and distance > 276: adjust, left_drive, right_drive = self.getAdjustedDrive( 0.0, heading, distance) elif state == 8 and distance <= 276: # 404, 142 - 220, 142 cv.line(image, (404, 142), (220, 142), (0, 0, 255), 5) left_drive = 0 right_drive = 0 state = 9 # Turn Right - Target 90+ diff elif state == 9 and (heading > 270 or heading < 90): left_drive = turn_speed right_drive = -turn_speed elif state == 9 and heading >= 90: left_drive = 0 right_drive = 0 cv.circle(image, (220, 142), 8, (255, 0, 0), 8) state = 10 elif state == 10: # 220, 142 - 220, 56 cv.line(image, (220, 142), (220, 56), (0, 0, 255), 5) adjust, left_drive, right_drive = self.getAdjustedDrive( 90.0, heading, distance) drive = "FWD" if left_drive < right_drive: drive = "LFT" elif left_drive > right_drive: drive = "RGT" cv.rectangle(image, (0, 0), (640, 50), (0, 0, 0), -1) text = "st={0} {1:.2f}({2:.2f}) {3}mm {4}".format( state, heading, adjust, distance, drive) cv.putText(image, text, (10, 30), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255)) self.showMenuImage(image) #rawCapture.truncate(0) if running: self.bot.move(left_drive, right_drive, gear) else: self.bot.stop() time.sleep(self.bot.getInterval()) self.bot.stop() cv.namedWindow('image', cv.WND_PROP_FULLSCREEN) cv.setWindowProperty('image', cv.WND_PROP_FULLSCREEN, cv.WINDOW_FULLSCREEN) cv.waitKey(10) def mine(self, joystick, gear): self.show("Mine Sweeper mode") if self.camera.hasCamera == False: return hw = int(self.w // 2) hh = int(self.h // 2) rawCapture = PiRGBArray(self.camera, size=(self.w, self.h)) time.sleep(0.1) showImage = 0 colour = np.array([0, 0, 0]) lower = np.array([110, 38, 34]) upper = np.array([130, 255, 255]) #colour_arrays = COLOURS["red"] #lower = np.array(colour_arrays[0], dtype = "uint8") #upper = np.array(colour_arrays[1], dtype = "uint8") brightness = self.camera.Brightness running = False mode = 0 states = ["Hunting", "Driving", "Sleeping", "Reversing"] xPos = 0 yPos = 0 angle = 45 diff = 10 gear = 2 state = 0 frameNo = 1.0 start = seconds() for frame in self.camera.CaptureContinous(rawCapture): presses = joystick.check_presses() if presses.home: running = False break if presses.start: xPos = -1 yPos = -1 angle = 45 diff = 0 mode = 0 running = True if joystick.presses.dup: self.bot.tilt(-10) time.sleep(0.5) elif joystick.presses.ddown: self.bot.tilt(10) time.sleep(0.5) elif joystick.presses.dright: self.bot.pan(-5) time.sleep(0.5) elif joystick.presses.dleft: self.bot.pan(5) time.sleep(0.5) else: self.bot.servo_off() yPos = self.h - int(((joystick.ly + 1) * self.h) / 2) xPos = int(((joystick.lx + 1) * self.w) / 2) yPos = min(self.h - 1, yPos) xPos = min(self.w - 1, xPos) # Take image image = frame.array imgHSV = cv.cvtColor( image, cv.COLOR_RGB2HSV ) # Convert the captured frame from RGB to HSV ( with blue in the red position ) # Get heading and distance heading, roll, pitch = self.bot.readEuler() distance = self.bot.getDistance() brightness = self.camera.SetBrightness( int(((joystick.ry + 1) * 100) / 2)) if running == False: left_drive = 0 right_drive = 0 if presses.square: showImage = 2 if presses.triangle: showImage = 1 if presses.cross: showImage = 0 if presses.circle: colour = imgHSV[yPos, xPos] print("xPos,yPos: {0},{1} Colour: {2}".format( xPos, yPos, colour)) lower[0] = colour[0] - 10 upper[0] = colour[0] + 10 lower[1] = 50 # colour[1] - 10 upper[1] = 255 # colour[1] + 10 lower[2] = 50 # colour[2] - 10 upper[2] = 255 # colour[2] + 10 if presses.l1: lower[0] = lower[0] + 1 upper[0] = upper[0] - 1 if presses.r1: lower[0] = lower[0] - 1 upper[0] = upper[0] + 1 if showImage == 1: imgMask = cv.inRange(imgHSV, lower, upper) # kernel = np.ones((5,5), np.uint8) # imgMask = cv.erode(imgMask, kernel, iterations=2) # imgMask = cv.dilate(imgMask, kernel, iterations=4) imgMask = cv.erode( imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask = cv.dilate( imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask = cv.dilate( imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask = cv.erode( imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask, contours_blk, hierarchy_blk = cv.findContours( imgMask, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE) contours_blk_len = len(contours_blk) xPos = -1 yPos = -1 area = 0 if contours_blk_len > 0: for con_num in range(contours_blk_len): area = cv.contourArea(contours_blk[con_num]) print("Index: {0} Area: {1}".format( con_num, area)) if area > 256: cnt = contours_blk[con_num] #x,y,w,h = cv.boundingRect(cnt) #cv.rectangle(imgMask,(x,y),(x+w,y+h),(0,255,0),2) M = cv.moments(cnt) xPos = int(M["m10"] / M["m00"]) yPos = int(M["m01"] / M["m00"]) cv.circle(imgMask, (xPos, yPos), 3, (0, 0, 255), -1) break text = "{0} {1} {2}".format(lower, upper, area) cv.putText(imgMask, text, (10, 20), cv.FONT_HERSHEY_PLAIN, 0.8, (255, 255, 255), 1) self.showMenuImage(imgMask) elif showImage == 2: colour = imgHSV[yPos, xPos] text = "{0}".format(colour) cv.putText(imgHSV, text, (10, 20), cv.FONT_HERSHEY_PLAIN, 0.8, (0, 0, 255), 1) # Draw a circle at the joystick position cv.circle(imgHSV, (xPos, yPos), 3, (0, 0, 0), -1) self.showMenuImage(imgHSV) else: colour = imgHSV[yPos, xPos] text = "{0}".format(colour) cv.putText(image, text, (10, 20), cv.FONT_HERSHEY_PLAIN, 0.8, (0, 0, 255), 1) # Draw a circle at the joystick position cv.circle(image, (xPos, yPos), 3, (255, 255, 255), -1) self.showMenuImage(image) if running == True: # Threshold the HSV image to get only red colors imgMask = cv.inRange(imgHSV, lower, upper) # kernel = np.ones((5,5), np.uint8) # imgMask = cv.erode(imgMask, kernel, iterations=2) # imgMask = cv.dilate(imgMask, kernel, iterations=4) imgMask = cv.erode( imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask = cv.dilate( imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask = cv.dilate( imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask = cv.erode( imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask, contours_blk, hierarchy_blk = cv.findContours( imgMask, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE) contours_blk_len = len(contours_blk) xPos = -1 yPos = -1 if contours_blk_len > 0: for con_num in range(contours_blk_len): area = cv.contourArea(contours_blk[con_num]) print("Index: {0} Area: {1}".format(con_num, area)) if area > 256: cnt = contours_blk[con_num] #x,y,w,h = cv.boundingRect(cnt) #cv.rectangle(imgMask,(x,y),(x+w,y+h),(0,255,0),2) M = cv.moments(cnt) xPos = int(M["m10"] / M["m00"]) yPos = int(M["m01"] / M["m00"]) cv.circle(imgMask, (xPos, yPos), 3, (0, 0, 255), -1) break # State = 0 - Spin on the spot with the camera at set position # State = 1 - Found target and moving towards # State = 2 - Sleep # State = 3 - Reverse slightly # states = ["Hunting","Driving","Sleeping","Reversing"] #print("xPos: {0} yPos: {1}".format(xPos, yPos)) # Can't see red, start turning if state == 0 and (xPos == -1 and yPos == -1): left_drive = 0.75 right_drive = -0.75 # Found red elif state == 0 and (xPos != -1 and yPos != -1): left_drive = 0 right_drive = 0 state = 1 # Drive towards red elif state == 1 and (xPos != -1 and yPos != -1): error = int(xPos - hw) left_drive = 1.0 if error < -4: left_drive = left_drive - (-error / 170) right_drive = 1.0 if error > 4: right_drive = right_drive - (error / 170) # Driven over it elif state == 1 and xPos == -1 and yPos == -1: #self.bot.tilt_angle(angle) #angle = angle + diff #if angle > 90: # diff = -10 #if angle < -90: # diff = 10 state = 2 # sleep assuming we are over a red square elif state == 2: self.bot.stop() time.sleep(1) left_drive = 0 right_drive = 0 state = 3 # Reverse a little bit and revert to seeing elif state == 3: left_drive = -0.5 right_drive = -0.5 state = 0 self.bot.move(left_drive, right_drive, gear) text = "{0} {1} {2} {3} {4} {5}".format( heading, distance, states[state], xPos, yPos, gear) cv.putText(imgMask, text, (10, 20), cv.FONT_HERSHEY_PLAIN, 0.8, (255, 255, 255), 1) self.showMenuImage(imgMask) frameNo += 1 rawCapture.truncate(0) self.bot.stop() self.bot.servo_off() end = seconds() print("Frames: {0} in {1} seconds or {2} frames per second".format( frameNo, end - start, frameNo / (end - start))) def straight(self, joystick, gear): self.show("Lava Palava mode") if self.camera.hasCamera == False: return hw = int(self.w // 2) hh = int(self.h // 2) rawCapture = PiRGBArray(self.camera, size=(self.w, self.h)) time.sleep(0.1) running = False self.bot.servo_off() blackbox = None frameNo = 1.0 start = seconds() for frame in self.camera.CaptureContinous(rawCapture): presses = joystick.check_presses() if presses.home: running = False break if presses.start: running = True if joystick.presses.l1: gear += 0.5 if joystick.presses.r1: gear -= 0.5 if gear < 1: gear = 1 if gear > 5: gear = 5 image = frame.array # Actually white line # Blackline = cv.inRange(image, (200,200,200), (255,255,255)) # Use for white lines Blackline = cv.inRange(image, (0, 0, 0), (60, 60, 60)) # Use for black lines kernel = np.ones((3, 3), np.uint8) Blackline = cv.erode(Blackline, kernel, iterations=5) Blackline = cv.dilate(Blackline, kernel, iterations=9) img_blk, contours_blk, hierarchy_blk = cv.findContours( Blackline.copy(), cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE) contours_blk_len = len(contours_blk) if contours_blk_len > 0: blackbox = cv.minAreaRect(contours_blk[0]) else: canditates = [] off_bottom = 0 for con_num in range(contours_blk_len): blackbox = cv.minAreaRect(contours_blk[con_num]) (x_min, y_min), (w_min, h_min), ang = blackbox box = cv.boxPoints(blackbox) (x_box, y_box) = box[0] if y_box > (h - 2): off_bottom += 1 canditates.append((y_box, con_num, x_min, y_min)) canditates = sorted(canditates) if off_bottom > 1: canditates_off_bottom = [] for con_num in range((contours_blk_len - off_bottom), contours_blk_len): (y_highest, con_highest, x_min, y_min) = canditates[con_num] total_distance = (abs(x_min - x_last)**2 + abs(y_min - y_last)**2)**0.5 canditates_off_bottom.append( (total_distance, con_highest)) canditates_off_bottom = sorted(canditates_off_bottom) (total_distance, con_highest) = canditates_off_bottom[0] blackbox = cv.minAreaRect(contours_blk[con_highest]) elif contours_blk_len > 1: (y_highest, con_highest, x_min, y_min) = canditates[contours_blk_len - 1] blackbox = cv.minAreaRect(contours_blk[con_highest]) if blackbox is not None: (x_min, y_min), (w_min, h_min), ang = blackbox if ang < -45: ang = 90 + ang if w_min < h_min and ang > 0: ang = (90 - ang) * -1 if w_min > h_min and ang < 0: ang = 90 + ang setpoint = hw error = int(x_min - setpoint) ang = int(ang) box = cv.boxPoints(blackbox) box = np.int0(box) cv.drawContours(image, [box], 0, (0, 0, 255), 3) cv.line(image, (int(x_min), 200), (int(x_min), 250), (255, 0, 0), 3) left_drive = 1.0 if error < -4: left_drive = left_drive - (-error / 170) right_drive = 1.0 if error > 4: right_drive = right_drive - (error / 170) text = "g = {0} : l = {1:.2} : r = {2:.2}".format( gear, left_drive, right_drive) cv.putText(image, text, (10, 20), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255)) if running: self.bot.turn(error, gear) frameNo += 1 self.showMenuImage(image) rawCapture.truncate(0) self.bot.stop() end = seconds() print("Frames: {0} in {1} seconds or {2} frames per second".format( frameNo, end - start, frameNo / (end - start))) def getAdjustedDrive(self, target, heading, dist): diff = 0.0 if target == 0.0 and heading > 180 and heading < 360: diff = 360.0 - heading else: diff = target - heading diff = diff / 45.0 left_drive = 1 right_drive = 1 if diff < 0: left_drive = left_drive + diff elif diff > 0: right_drive = right_drive - diff print( 'target={0:.2f} heading={1:.2f} diff={2:.2f} ldr={3:.2f} rdr={4:.2f} {5:.2f}' .format(target, heading, diff, left_drive, right_drive, dist)) return diff, left_drive, right_drive def showMenuImage(self, menu_img): cv.imshow('image', menu_img) cv.waitKey(1) def loadMenuImage(self, fileName): img = cv.imread(fileName, cv.IMREAD_COLOR) img = cv.resize(img, (640, 480), interpolation=cv.INTER_CUBIC) # was 1280 960 return img def writePNG(self, filename, y_data): imgSize = (32, 32) # Use the PIL raw decoder to read the data. # the 'F;16' informs the raw decoder that we are reading # a little endian, unsigned integer 16 bit data. #Image.fromstring('L', imgSize, y_data, 'L', 'F;16') img = Image.fromarray(y_data) img.save(filename) def show(self, text): # Only log changed values if text != self.last_text: print(text) LOGGER.debug(text) self.last_text = text
def main(): global running, button w = 320 h = 240 camera = Camera(w, h) camera.Rotate(270) cv.namedWindow('image', cv.WND_PROP_FULLSCREEN) cv.setWindowProperty('image', cv.WND_PROP_FULLSCREEN, cv.WINDOW_FULLSCREEN) cv.setMouseCallback("image", click) rawCapture = PiRGBArray(camera, size=(w, h)) time.sleep(0.1) lower = np.array([110, 50, 50]) upper = np.array([130, 255, 255]) showMask = True for frame in camera.CaptureContinous(rawCapture): if running == False: break if button == 1: cv.destroyAllWindows() os.system("sudo halt") if button == 2: break if button == 3: showMask = not showMask button = 0 # Narrow the H range if button == 4: lower[0] = min(lower[0] + 1, 170) upper[0] = max(upper[0] - 1, 0) button = 0 # Expand the H range if button == 5: lower[0] = max(lower[0] - 1, 0) upper[0] = min(upper[0] + 1, 170) button = 0 # Narrow the S range if button == 6: lower[1] = min(lower[1] + 1, 255) upper[1] = max(upper[1] - 1, 0) button = 0 # Expand the S range if button == 7: lower[1] = max(lower[1] - 1, 0) upper[1] = min(upper[1] + 1, 255) button = 0 # Narrow the V range if button == 8: lower[2] = min(lower[2] + 1, 255) upper[2] = max(upper[2] - 1, 0) button = 0 # Expand the V range if button == 9: lower[2] = max(lower[2] - 1, 0) upper[2] = min(upper[2] + 1, 255) button = 0 raw = frame.array imgHSV = cv.cvtColor( raw, cv.COLOR_RGB2HSV ) # Convert the captured frame from RGB to HSV ( with blue in the red position ) imgMask = cv.inRange(imgHSV, lower, upper) #kernel = np.ones((3,3), np.uint8) #imgMask = cv.erode(imgMask, kernel, iterations=2) #imgMask = cv.dilate(imgMask, kernel, iterations=4) imgMask = cv.erode(imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask = cv.dilate(imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask = cv.dilate(imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) imgMask = cv.erode(imgMask, cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))) if showMask: image = imgMask else: image = raw cv.rectangle(image, (290, 0), (320, 20), (255, 255, 255), -1) cv.putText(image, "X", (298, 18), cv.FONT_HERSHEY_PLAIN, 1, (0, 0, 0)) cv.rectangle(image, (0, 220), (40, 240), (255, 255, 255), -1) cv.putText(image, "-H", (8, 236), cv.FONT_HERSHEY_PLAIN, 1, (0, 0, 0)) cv.rectangle(image, (50, 220), (90, 240), (255, 255, 255), -1) cv.putText(image, "+H", (58, 236), cv.FONT_HERSHEY_PLAIN, 1, (0, 0, 0)) cv.rectangle(image, (100, 220), (150, 240), (255, 255, 255), -1) cv.putText(image, "-S", (108, 236), cv.FONT_HERSHEY_PLAIN, 1, (0, 0, 0)) cv.rectangle(image, (160, 220), (200, 240), (255, 255, 255), -1) cv.putText(image, "+S", (168, 236), cv.FONT_HERSHEY_PLAIN, 1, (0, 0, 0)) cv.rectangle(image, (210, 220), (250, 240), (255, 255, 255), -1) cv.putText(image, "-V", (218, 236), cv.FONT_HERSHEY_PLAIN, 1, (0, 0, 0)) cv.rectangle(image, (260, 220), (300, 240), (255, 255, 255), -1) cv.putText(image, "+V", (268, 236), cv.FONT_HERSHEY_PLAIN, 1, (0, 0, 0)) cv.rectangle(image, (310, 220), (350, 240), (255, 255, 255), -1) cv.putText(image, "+", (311, 236), cv.FONT_HERSHEY_PLAIN, 1, (0, 0, 0)) text = "{0} {1}".format(lower, upper) cv.putText(image, text, (10, 20), cv.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1) showMenuImage(image) rawCapture.truncate(0) cv.destroyAllWindows()