def run(self): # start camera camera = pi_camera_stream.setup_camera() # direction pid - how far from the middle X is. direction_pid = PIController(proportional_constant=0.0015, integral_constant=0.0000, windup_limit=400) # warm up and servo move time time.sleep(0.1) print("Setup Complete") # Main loop for frame in pi_camera_stream.start_stream(camera): (x, y), radius = self.process_frame(frame) print("Coordes: %dx%d" % (x, y)) if radius > 20: # The size is the first error radius_error = self.correct_radius - radius #speed_value = speed_pid.get_value(radius_error) # And the second error is the based on the center coordinate. direction_error = self.center - x direction_value = direction_pid.get_value(direction_error) print( "radius: %d, radius_error: %d direction_error: %d, direction_value: %.2f" % (radius, radius_error, direction_error, direction_value)) # Now produce left and right motor speeds self._motor.set_left(0.5 - direction_value) self._motor.set_right(0.5 + direction_value) time.sleep(0.1) else: self._motor.stop_all()
def frame_generator(): """This is our main video feed""" camera = pi_camera_stream.setup_camera() # allow the camera to warmup time.sleep(0.1) for frame in pi_camera_stream.start_stream(camera): encoded_bytes = pi_camera_stream.get_encoded_bytes_for_frame(frame) # Need to turn this into http multipart data. yield (b'--frame\r\n Content-Type: image/jpeg\r\n\r\n' + encoded_bytes + b'\r\n')
def controlled_image_server_behavior(): # Setup the camera camera = pi_camera_stream.setup_camera() # allow the camera to warmup time.sleep(0.1) # Send frames from camera to server for frame in pi_camera_stream.start_stream(camera): encoded_bytes = pi_camera_stream.get_encoded_bytes_for_frame(frame) put_output_image(encoded_bytes) # Check any control instructions instruction = get_control_instruction() if instruction == "exit": print("Stopping") return
def run(self): # Set pan and tilt to middle, then clear it. self.robot.set_pan(0) self.robot.set_tilt(0) # start camera camera = pi_camera_stream.setup_camera() # warm up and servo move time time.sleep(0.1) # Servo's will be in place - stop them for now. self.robot.servos.stop_all() print("Setup Complete") # Main loop for frame in pi_camera_stream.start_stream(camera): self.make_display(frame) self.process_control()
def controlled_image_server_behavior(rotation, display_queue, control_queue): # Setup the camera camera = setup_camera(rotation=rotation) # allow the camera to warmup time.sleep(0.1) # Send frames from camera to server for frame in start_stream(camera): encoded_bytes = get_encoded_bytes_for_frame(frame) put_output_image(encoded_bytes, display_queue=display_queue) # Check any control instructions instruction = get_control_instruction(control_queue=control_queue) if instruction == "exit": print("Stopping") return
def run(self): # Set pan and tilt to middle, then clear it. self.robot.set_pan(0) self.robot.set_tilt(0) # start camera camera = pi_camera_stream.setup_camera() # speed pid - based on the radius we get. speed_pid = PIController(proportional_constant=0.8, integral_constant=0.1, windup_limit=100) # direction pid - how far from the middle X is. direction_pid = PIController(proportional_constant=0.25, integral_constant=0.1, windup_limit=400) # warm up and servo move time time.sleep(0.1) # Servo's will be in place - stop them for now. self.robot.servos.stop_all() print("Setup Complete") # Main loop for frame in pi_camera_stream.start_stream(camera): (x, y), radius = self.process_frame(frame) self.process_control() if self.running and radius > 20: # The size is the first error radius_error = self.correct_radius - radius speed_value = speed_pid.get_value(radius_error) # And the second error is the based on the center coordinate. direction_error = self.center - x direction_value = direction_pid.get_value(direction_error) print( "radius: %d, radius_error: %d, speed_value: %.2f, direction_error: %d, direction_value: %.2f" % (radius, radius_error, speed_value, direction_error, direction_value)) # Now produce left and right motor speeds self.robot.set_left(speed_value - direction_value) self.robot.set_right(speed_value + direction_value) else: self.robot.stop_motors() if not self.running: speed_pid.reset() direction_pid.reset()
def run(self): # start camera camera = pi_camera_stream.setup_camera() # warm up time time.sleep(0.1) print("Setup Complete") # Main loop for frame in pi_camera_stream.start_stream(camera): (x, y, w, h) = self.process_frame(frame) self.process_control() if self.running and h > self.min_size: # Pan pan_error = self.center_x - (x + (w / 2)) pan_value = self.pan_pid.get_value(pan_error) self.robot.set_pan(int(pan_value)) # Tilt tilt_error = self.center_y - (y + (h / 2)) tilt_value = self.tilt_pid.get_value(tilt_error) self.robot.set_tilt(int(tilt_value)) print("x: %d, y: %d, pan_error: %d, tilt_error: %d, pan_value: %.2f, tilt_value: %.2f" % (x, y, pan_error, tilt_error, pan_value, tilt_value))
def __init__(self): Motor.__init__(self) # Read in the YAML config file with open(YAML_CONFIG_FILE, 'r') as yaml_data_file: self._config = yaml.safe_load(yaml_data_file) # Build up a list of the sensors self._tofSensors = {} for address in self._config['ToF']['sensors']: self._tofSensors[address["name"]] = SensorStruct(-1, 0) # Do we have any? if len(self._tofSensors) > 0: print("Setting up ToF monitor") # Work out the UDP Address to listen on udpAddress = (self._config["ToF"]["udp"]["address"], self._config["ToF"]["udp"]["port"]) # Launch the UDP/ToF monitor self._tofMonitor = UDPMonitorThread(udpAddress, self._tofSensors) self._tofMonitor.daemon = True self._tofMonitor.start() # Wait for the sensors to settle down. print("Waiting for ToF sensors to settle") firstSensor = list(self._tofSensors.keys())[0] while self._tofSensors[firstSensor].distance <= 0: time.sleep(0.1) # Start the camera # IMPROVE: Only trigger if needed? self._camera = pi_camera_stream.setup_camera() # Make sure we release everything at exit atexit.register(self.shutdown)
import cv2 import pi_camera_stream import numpy as np import time # start camera camera = pi_camera_stream.setup_camera() cv2.namedWindow("output", cv2.WINDOW_NORMAL) cv2.namedWindow("outputimg", cv2.WINDOW_NORMAL) # Green #lowerBound=np.array([60-20,100,50]) #upperBound=np.array([60+20,255,255]) # Red #lowerBound=np.array([160,100,50]) #upperBound=np.array([179,255,255]) # Blue lowerBound = np.array([100, 100, 50]) upperBound = np.array([120, 255, 255]) # Yello #lowerBound=np.array([10,100,50]) #upperBound=np.array([20,255,255]) # Whi40 sensitivity = 100 #lowerBound = np.array([0,0,255-sensitivity]) #upperBound = np.array([255,sensitivity,255])
def controlled_image_server_behavior(): #in last_state steht immer die Richtung in die zuletzt gefahren wurde global last_state #centre x und y beschreibt nachher die Bildmitte ,Default 0 global centre_x global centre_y #Default Wert "nach rechts" wird last_state zugewiesen last_state = "right" centre_x = 0. centre_y = 0. #elapsed_time ist die verstrichene Zeitspanne solange kein Objekt gefunden wurde. elapsed_time = 4.0 #correction_factor x und y ist eine Hilfe für Auflösungsänderung, in diesem Fall nicht benutzt correction_factor_x = 3.0 correction_factor_y = 2.5 #Die Verbindung zum Arduino Mega wird aufgebaut functions.open_connection_arduino() #Motoren werden auf LOW initialisiert functions.stop() #Hilfstimer start und end #start ist die Startzeit des Timers #in start wird eine aktuelle Zeitangabe geschrieben start = float(time.perf_counter()) #end ist das Ende des Timers #Startzeit + die gewünschte Zeitspanne die vorher in elapsed_time festgelegt wurde end = start + elapsed_time #Initalisierung der Kamera camera = pi_camera_stream.setup_camera() #kruzer sleep damit die Kamera sich "aufwärmen" kann time.sleep(0.1) #in der for Schleife werden einzelne Frames (Bilder) ausgewertet, verarbeitet #und im Nachhinein wieder ausgegeben. In diesem werden sie an den Flask-Server #weitergeleitet. Der über den Port 8000 aufrufbar ist. #Ebenfalls anhand der erfassten Positionswerte des aktuellen Bildes reagiert #das heißt es werden Funktionen aufgerufen um das Auto zu steuern for frame in pi_camera_stream.start_stream(camera): #Hue Saturation Value hsv1 = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) #das Bild wird maskiert und nur noch der Teil mit den voreingestellten Farben #in diesem Fall Pink,Rot wird zu sehen sein mask_red = pi_camera_stream.segment_colour(frame) #In diesem Teil werden die Kooardinaten des Bildes erfasst und in #die Hilfsvariablen geschrieben loct,area = pi_camera_stream.find_blob(mask_red) #die einzelnen Werte aus loct werden aufgesplittet um diese nachher zu verwenden x,y,w,h = loct #print Funktionen zu Debug-Zwecken ##print("x+w") ##print(x+w) #Der folgende Fall beschreibt wenn kein Objekt gefunden wurde if (w*h) < 10: ####<NO OBJECT FOUND>##### found = 0 #ein Timer wird gestartet um kurz zu warten ob das Objekt noch gefunden wird start = float(time.perf_counter()) #wenn die vorher eingestellte Zeit, in Default-Fall 4 Sekunden, die jetzt in end steht vertrichen ist, #wird in die zuletzt gefahrene Richtung gefahren. if end <= start: if last_state == "left": #Funktion kommuniziert mit dem Arduino um nach links zu fahren functions.turnleft() time.sleep(0.1) elif last_state == "right": #Funktion kommuniziert mit dem Arduino um nach rechts zu fahren functions.turnright() time.sleep(0.1) #Der folgende Fall beschreibt wenn ein Objekt gefunden wurde else: ####<OBJECT FOUND>#### found = 1 #Timer wird bei jedem Durchlauf neu gesetzt und das Ende wird durch die aufaddierte Zeit gesetzt. start = float(time.perf_counter()) end = float(time.perf_counter()) + elapsed_time #Die Mittelpunkte des Bildes werden ermittelt und in die Variablen centre x und y geschrieben. centre_x = x + ((w)/2) centre_y = y + ((h)/2) #Anhand der nun beschriebenen Variablen wird ein kleiner Kreis, #zur Visualisierung, in das Bild gezeichnet. cv2.circle(frame,(int(centre_x),int(centre_y)),10,(240,240,240),-1) #centre_x wird nocheinmal in 2 Häflten geteilt um nachher einfacher festzustellen in welcher #Hälfte sich das gefundene Objekt befindet. centre_x -= 240 #Der Bewegungsablauf wenn ein Objekt gefunden wurde if(found == 1 ): #wenn sich das Objekt in der äußeren linken Hälfte befindet, #wird nachjustiert, also nach links gefahren um die Objekt-Mitte in die Bild-Mitte zu bringen if(centre_x <= -100): functions.turnleft() last_state = "left" #wenn sich das Objekt in der äußeren rechten Hälfte befindet, #wird nachjustiert, also nach rechts gefahren um die Objekt-Mitte in die Bild-Mitte zu bringen elif(centre_x >= 100): functions.turnright() last_state = "right" #Solange die Außenkanten des Objektes die festgelegten Werte nicht überschreitet wird #geradeaus in Richtung des Objektes gefahren. elif(x >= 18 and x+w <=330): functions.forward() #wenn keiner der vorhergegangen Fälle eintritt wird angehalten (Stop) else: functions.stop() #wenn keiner der vorhergegangen Fälle eintritt wird angehalten (Stop) else: functions.stop() #Das nun erfasste Bild wird nun in die Variable encoded_bytes geschrieben encoded_bytes = pi_camera_stream.get_encoded_bytes_for_frame(frame) #das erfasste Bild wird nun in die Schlange (Queue) eingereiht #um nachher abgerufen zu werden und auf den Server als Stream ausgegeben zu werden put_output_image(encoded_bytes) #wenn eine Kontrollanweisung gesetzt wurde wie zum Beispiel "exit" wird darauf reagiert #in Falle von "exit" wird das Programm angehalten instruction = get_control_instruction() if instruction == "exit": print("Stopping") return