def display_train(): pygame.init() # fenetre = pygame.display.set_mode((width, height), pygame.FULLSCREEN) fenetre = pygame.display.set_mode((width, height), pygame.RESIZABLE) fenetre.fill(BLACK) delay = st.get_system_refresh_period() th = mv.save_data(True) for fr in range(0, len(st.frequencies)): for block in range(0, 15): current_time = pygame.time.get_ticks() switch_time = current_time + switch_delay current_display_time = pygame.time.get_ticks() display_time = current_display_time + delay while current_time <= switch_time: stop() current_time = pygame.time.get_ticks() current_display_time = pygame.time.get_ticks() if current_display_time >= display_time: display_time = current_display_time + delay st.train_switch(fr, fenetre) current_time = pygame.time.get_ticks() gaze_time = current_time + train_delay current_display_time = pygame.time.get_ticks() display_time = current_display_time + delay j = 0 while current_time <= gaze_time: mv.save_training = True mv.fileName = str(fr) + '_' + str(block) mv.label = fr stop() current_time = pygame.time.get_ticks() current_display_time = pygame.time.get_ticks() if current_display_time >= display_time: display_time = current_display_time + delay st.draw_stimuli(j, fenetre) j += 1 while not mv.saved: continue mv.saved = False mv.save_training = False mv.stop(th)
def main(): pygame.init() # fenetre = pygame.display.set_mode((width, height), pygame.FULLSCREEN) fenetre = pygame.display.set_mode((width, height), pygame.RESIZABLE) fenetre.fill(BLACK) x, y = get_start() player_position = (x + 32, y + 32) end_rect, player = maze.build_game(player_position, x, y) maze.draw(fenetre, end_rect, player) # time in millisecond from start program current_time = pygame.time.get_ticks() # how long to show or hide delay = st.get_system_refresh_period() move_delay = 7000 # time of next change change_time = current_time + delay j = 0 th = mv.save_data() maze.draw(fenetre, end_rect, player) win = False while not win: for event in pygame.event.get(): if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE): pygame.quit() sys.exit() win = maze.update(fenetre, end_rect) current_time = pygame.time.get_ticks() if current_time >= change_time: change_time = current_time + delay st.draw_stimuli(j, fenetre) j += 1 if win: mv.stop(th) win_message(fenetre)
def handler(signal, frame): move.stop() sys.exit(0)
cv2.putText(crop_img,str(ang), (10,40), cv2.FONT_HERSHEY_SIMPLEX,1, (0,0,255), 2) cv2.putText(crop_img,str(error), (10,320), cv2.FONT_HERSHEY_SIMPLEX,1, (255,0,0), 2) cv2.line(crop_img, (int(x_min), int(y_min)), (halfway, int(y_min)), (255,0,0), 3) Rad_Angle = math.radians(ang) <<<<<<< HEAD error_angle = math.radians(error/4) move.rotate(dxl_io,1000, dt*Rad_Angle) Correction(dxl_io, dt*error_angle) time.sleep(dt) ======= err = math.radians(error/4) rotate(dxl_io, 300,dt*err + dt*Rad_Angle) time.sleep(dt) >>>>>>> ce5c36cad41c5be65dfc3544a01fe047bc8741a7 print(error, ang) else: print("I don't see a line") Look_for_line(dxl_io) time.sleep(dt) #Display the resulting frame #cv2.imshow('frame',crop_img) if cv2.waitKey(20) & 0xFF == ord('k'): move.stop() break if cv2.waitKey(1) & 0xFF == ord('q'): break #Pas de temps t += dt
def main(): pid = PID(1, 0.1, 0.05, setpoint=0) rightSpeed = 0 leftSpeed = 0 direction = "r" track = "simple" if len(sys.argv) >= 2 and str(sys.argv[1]) == "left": direction = "l" if len(sys.argv) >= 3 and str(sys.argv[2]) == "hard": track = "hard" stream = io.BytesIO() if DEMO: # Create a window cv2.namedWindow(WINDOW_DISPLAY_IMAGE) # position the window cv2.moveWindow(WINDOW_DISPLAY_IMAGE, 0, 35) # Open connection to camera with picamera.PiCamera() as camera: # Set camera resolution camera.resolution = (RESOLUTION_X, RESOLUTION_Y) # Start loop while True: mod = 0 # Get the tick count so we can keep track of performance e1 = cv2.getTickCount() # Capture image from camera camera.capture(stream, format='jpeg', use_video_port=True) # Convert image from camera to a numpy array data = np.fromstring(stream.getvalue(), dtype=np.uint8) # Decode the numpy array image image = cv2.imdecode(data, cv2.CV_LOAD_IMAGE_COLOR) # Empty and return the in-memory stream to beginning stream.seek(0) stream.truncate(0) # Create other images grey_image = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY) display_image = cv2.copyMakeBorder(image, 0, 0, 0, 0, cv2.BORDER_REPLICATE) center_point = (SCAN_POS_X, SCAN_HEIGHT) if (track == "hard"): ret, thresh = cv2.threshold(grey_image, 127, 255, 0) thresh = thresh[100:240, 0:320] contours_right, hierarchy = cv2.findContours( thresh[0:140, 170:320], cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE) contours_left, hierarchy = cv2.findContours( thresh[0:140, 0:150], cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE) # If a interection is detected pull some evasive manouvers if len(contours_right) >= 1 and len(contours_left) >= 1: contour_right = max(contours_right, key=cv2.contourArea) contour_left = max(contours_left, key=cv2.contourArea) extent_right = cv2.contourArea(contour_right) extent_left = cv2.contourArea(contour_left) print(extent_left, extent_right) if extent_left >= 21000 / 5 and extent_right >= 21000 / 5: print( "More than 2 large contours, most likely an intersection, EVASIVE MANOUVERS !" ) cv2.rectangle(display_image, (0, 0), (160, 240), (0, 0, 0), -1) cv2.rectangle(display_image, (0, 0), (320, 80), (0, 0, 0), -1) cv2.rectangle(grey_image, (0, 0), (160, 240), (0, 0, 0), -1) cv2.rectangle(grey_image, (0, 0), (320, 80), (0, 0, 0), -1) mod = -1 if DEMO: croppedImg = image.copy() croppedImg_right = croppedImg[100:240, 170:320] croppedImg_left = croppedImg[100:240, 0:150] cv2.drawContours(croppedImg_right, contours_right, 0, (0, 0, 255), 2) cv2.drawContours(croppedImg_left, contours_left, 0, (0, 255, 0), 2) numpy_horizontal = np.hstack( (croppedImg_left, croppedImg_right)) else: numpy_horizontal = None # San a horizontal line based on the centre point # We could just use this data to work out how far off centre we are and steer accordingly. # Get a data array of all the falues along that line # scan_data is an array containing: # - pixel value scan_data = scanLine(grey_image, display_image, center_point, SCAN_RADIUS) # The center point we believe the line we are following intersects with our scan line. point_on_line = findLine(display_image, scan_data, SCAN_POS_X, SCAN_HEIGHT, SCAN_RADIUS) # Start scanning the arcs # This allows us to look ahead further ahead at the line and work out an angle to steer # From the intersection point of the line, scan in an arc to find the line # The scan data contains an array # - pixel value # - x co-ordinate # - y co-ordinate returnVal, scan_data = scanCircle(grey_image, display_image, point_on_line, SCAN_RADIUS_REG, -90) previous_point = point_on_line # in the same way ads the findLine, go through the data, find the mid point and return the co-ordinates. last_point = findInCircle(display_image, scan_data) cv2.line(display_image, (previous_point[0], previous_point[1]), (last_point[0], last_point[1]), (255, 255, 255), 1) actual_number_of_circles = 0 for scan_count in range(0, NUMBER_OF_CIRCLES + mod): returnVal, scan_data = scanCircle( grey_image, display_image, last_point, SCAN_RADIUS_REG, lineAngle(previous_point, last_point)) # Only work out the next itteration if our point is within the bounds of the image if returnVal == True: actual_number_of_circles += 1 previous_point = last_point last_point = findInCircle(display_image, scan_data) cv2.line(display_image, (previous_point[0], previous_point[1]), (last_point[0], last_point[1]), (255, 255, 255), 1) else: break # Draw a line from the centre point to the end point where we last found the line we are following cv2.line(display_image, (center_point[0], center_point[1]), (last_point[0], last_point[1]), (0, 0, 255), 1) # Display the image if DEMO: cv2.imshow(WINDOW_DISPLAY_IMAGE, display_image) if numpy_horizontal is not None: cv2.imshow('Contours', numpy_horizontal) # This is the maximum distance the end point of our search for a line can be from the centre point. line_scan_length = SCAN_RADIUS_REG * (actual_number_of_circles + 1) # This is the measured line length from the centre point line_length_from_center = lineLength(center_point, last_point) center_y_distance = center_point[1] - last_point[1] center_x_distance = center_point[0] - last_point[0] # Stop counting all work is done at this point and calculate how we are doing. e2 = cv2.getTickCount() bearing = lineAngle(center_point, last_point) * -1 - 90 returnString = "fps {} - bearing {} - x:{} y:{} look distance:{} distance from origin:{}".format( 1000 / ((e2 - e1) / cv2.getTickFrequency() * 1000), bearing, center_x_distance, center_y_distance, line_scan_length, line_length_from_center) print(returnString) if MOVE: if mod == -1 and bearing <= 5: print( "Derped out on intersection setting bearing to 45 degrees" ) bearing = 45 rightSpeed, leftSpeed, pid = move.move(bearing, rightSpeed, leftSpeed, pid) # Wait for ESC to end program c = cv2.waitKey(7) % 0x100 if c == 27: break move.stop() cv2.destroyAllWindows() return
line_sensor.wait_for_line(seek_time) if line_sensor.value < 0.5: ret = True break else: direction = not direction if direction: seek_count += 1 continue return ret parser.add_option("-s", "--speed", type="float", dest="speed") (options, args) = parser.parse_args() if options.speed != None: speed = options.speed line_sensor.when_line = lambda: move.forward() line_sensor.when_no_line = lambda: move.stop() signal.signal(signal.SIGINT, exit_gracefully) while True: move.forward(speed) line_sensor.wait_for_no_line() if not seek_line(): print("Can't find any line") break exit_gracefully()
#!/usr/bin/env python3 import move import signal from gpiozero import DistanceSensor min_distance = 0.15 sonic_sensor = DistanceSensor( echo=18, trigger=17, threshold_distance=min_distance) speed = 0.5 def exit_gracefully(): exit() sonic_sensor.when_in_range = lambda: move.stop() sonic_sensor.when_out_of_range = lambda: move.forward() signal.signal(signal.SIGINT, exit_gracefully) move.forward(speed) while True: sonic_sensor.wait_for_in_range() move.backward(speed) sonic_sensor.wait_for_out_of_range() move.right(speed, 0.75) exit_gracefully()