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
