Maneuvers

.gitignore

@@ -0,0 +1 @@
+images/

README.md

@@ -1,2 +1,7 @@
 # RaspberryDrive
 Use a Raspberry Pi 3 to drive an RC Car
+
+## Libraries
+Python 2
+SimpleCV
+PiCamera

analyzeblob.py

@@ -0,0 +1,49 @@
+import SimpleCV
+import cv2
+import time
+
+class AnalyzeBlob(object):
+
+	def __init__(self,img,blob):
+		self.vertical_boundary = 0.3
+		self.width_boundary = 0.4
+		self.img = img
+		self.blob = blob
+		self.angle_to_turn = 30
+
+	def isBlobBlocking(self):
+		if (self.__heightBelowThreshold() and self.__widthSufficientToBlock()):
+				return True
+
+	def isBlobDetectedOnLeft(self):
+		if (self.__anglingFromLeft() and self.__largeObjectStartingNearLeft()):
+			return True
+
+	def isBlobDetectedOnRight(self):
+		if (self.__anglingFromRight() and self.__largeObjectStartingNearRight()):
+			return True
+
+	def __anglingFromRight(self):
+		return self.blob.angle() < self.angle_to_turn
+
+	def __largeObjectStartingNearRight(self):
+		return (self.blob.maxX() > self.img.size()[0] * 0.95) and (self.blob.width() > self.img.size()[0] * 0.8) and (self.blob.maxY() > self.img.size()[1] * 0.3)
+
+	def isBlobBlockingMoreRight(self):
+		self.blob.angle() > 0
+
+	def __anglingFromLeft(self):
+		print self.blob.angle()
+		return self.blob.angle() > 90 - self.angle_to_turn
+
+	def __largeObjectStartingNearLeft(self):
+		return (self.blob.maxX() < self.img.size()[0] * 0.05) and (self.blob.width() > self.img.size()[0] * 0.8) and (self.blob.maxY() > self.img.size()[1] * 0.3)
+
+	def __heightBelowThreshold(self):
+		return self.blob.maxY() > self.img.size()[1] * self.vertical_boundary
+
+	def __widthSufficientToBlock(self):
+		print self.blob.width() > self.img.size()[0] * self.width_boundary
+		return self.blob.width() > self.img.size()[0] * self.width_boundary
+
+

analyzeimage.py

@@ -0,0 +1,63 @@
+import SimpleCV
+import cv2
+import time
+from analyzeblob import *
+from car_maneuvers import *
+from PIL import Image
+
+class AnalyzeImage(object):
+
+  def __init__(self, image,connection):
+    self.default_command = "stop"
+    self.scvImg = SimpleCV.Image(image)
+    self.segmented_black_white = self.scvImg.stretch(160,161)
+    self.black_white_blobs = self.segmented_black_white.findBlobs(minsize=100)
+    self.car = CarManeuvers(connection)
+
+  def runBlobFinder(self):
+    if (self.black_white_blobs and (len(self.black_white_blobs) > 0)):
+      self.analyzeBlobs()
+    else:
+      self.car.forward()
+  # print x
+	# print blob
+
+  def analyzeBlobs(self):
+    blobs = self.scvImg.findBlobs(minsize = 100)
+    # if blobs:
+    #   for blob in blobs:
+    #     if blob.area() > 100:
+    #       blob.draw(color=(128,0,0))
+    #   self.scvImg.show()
+    #   self.scvImg.show()
+    #   time.sleep(2)
+    #check if blocked
+    for blob in self.black_white_blobs:
+      print blob
+      analyzed_blob = AnalyzeBlob(self.scvImg,blob)
+
+      if analyzed_blob.isBlobBlocking():
+        print "true"
+        if analyzed_blob.isBlobBlockingMoreRight():
+          self.car.back_up_and_then_drive_left()
+        else:
+          self.car.back_up_and_then_drive_right()
+        return
+
+      elif analyzed_blob.isBlobDetectedOnRight():
+        # if analyzed_blob.blockedOnRight():
+        self.car.left()
+        return
+      elif analyzed_blob.isBlobDetectedOnLeft():
+        self.car.right()
+        return
+    #otherwise go forward
+    self.car.forward()
+
+#  detect blobs
+#  if blobs
+#    check left and check rightt
+#     is blocking?
+#         back up
+#       else
+#         turn (in correct direction)

car_maneuvers.py

@@ -0,0 +1,29 @@
+from connect import *
+
+class CarManeuvers(object):
+
+  def __init__(self,connection):
+    self.connection = connection
+
+  def forward(self):
+    print "executing forward"
+    send_command(self.connection, "forward", "0.5")
+
+  def right(self):
+    print "executing right"
+    send_command(self.connection, "right", "0.5")
+
+  def left(self):
+    print "executing left"
+    send_command(self.connection, "left", "0.5")
+
+  def back_up_then_right(self):
+    print "executing back up and drive right"
+    send_command(self.connection, "backwards", "0.75")
+    send_command(self.connection, "right", "0.5")
+
+  def back_up_then_left(self):
+    print "executing back up and drive left"
+    send_command(self.connection, "backwards", "0.75")
+    send_command(self.connection, "left", "0.5")
+

connect.py

@@ -0,0 +1,24 @@
+import socket
+
+def new_connection():
+    host = '192.168.2.4'
+    port = 9000
+    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    s.connect((host, port))
+    return s;
+
+def send_command(connection,command,time):
+    connection.send('' + command + ' ; ' + time)
+    return receive_confirmation(connection)
+
+def receive_confirmation(connection):
+  received = connection.recv(20)
+  return received;
+
+def send_end_connection(connection):
+  connection.send('quit')
+  return
+
+# example:
+# con = new_conntection
+# send_command(con,"forward",0.5)

controllers/controller.py

@@ -0,0 +1,58 @@
+import os
+import sys
+from time import sleep
+sys.path.insert(0, os.getcwd())
+import analyzeblob
+import analyzeimage
+from analyzeimage import *
+from analyzeblob import *
+from get_images_from_pi import get_image, valid_image
+from connect import new_connection, send_command, receive_confirmation,send_end_connection
+from start_camera import start_camera
+from start_server import start_server
+import threading
+
+def start_server_thread():
+	server_thread = threading.Thread(target=start_server)
+	server_thread.start()
+
+def start_camera_thread():
+	camera_thread = threading.Thread(target=start_camera)
+	camera_thread.start()
+
+def start_all():
+  start_server_thread()
+  start_camera_thread()
+  sleep(7)
+  s = new_connection()
+  return s
+  #start the server listening on contr pi
+  #start the camera taking pictures on camera pi
+  # Sleep to make sure server connects
+  #start the server connection on this current server
+
+def end_all(server):
+  send_end_connection(server)
+  server.close()
+  #End-all
+  #Camera exits automatically automatically
+  #Close the Server Connection on the Pi-Controller End
+  #Kill the server listening process
+
+def run(server):
+  count = 0
+  while (count < 20):
+    instruction = "forward"
+    #retrieve image.
+    get_image(count)
+    if valid_image(os.getcwd() + "/gregTest.jpg"):
+      #Blog Detection
+      analyzed_image = AnalyzeImage(os.getcwd() + "/gregTest.jpg")
+      analyzed_image.runBlobFinder()
+    count += 1
+
+server = start_all()
+run(server)
+end_all(server)
+#exit out of controller
+exit()

driver_code_test.py

@@ -0,0 +1,38 @@
+import SimpleCV
+from SimpleCV import Image
+import cv2
+import time
+from start_camera import start_camera
+import threading
+
+camera_thread = threading.Thread(target=start_camera)
+camera_thread.start()
+from get_images_from_pi import get_image, valid_image
+
+count = 0
+while (count < 10):
+  get_image(count)
+  count += 1
+
+exit()
+
+# image = Image('images/9.jpg')
+# # p = image.getPalette()
+# # rgb = PIL.ImageColor.getrgb("blue")
+# # print rgb
+# blobs = image.findBlobs(threshval=(0,0,255))
+# # blobs = image.findBlobsFromPalette( (p[0],p[1],p[2]) )
+# # blobs = image.findBlobsFromHueHistogram((0,0,50,50))
+# if blobs:
+#   for blob in blobs:
+#     print "got a blob"
+#     blob.draw(color=(0, 128, 0))
+# image.show()
+# image.show()
+# time.sleep(4)
+
+
+
+
+
+

driving_server.py

@@ -0,0 +1,45 @@
+from socket import *
+import run_rc_car as picar
+
+def createPiReceiver():
+  serversocket = socket(AF_INET, SOCK_STREAM)
+	port = 9000
+  new_picar = picar.PiCar()
+  serversocket.bind(('',port))
+  serversocket.listen(5)
+	print "New Server Created, listening on port " + str(port)
+  while(1):
+    (clientsocket, address) = serversocket.accept()
+		print "accepted connection from " + str(address)
+		while(1):
+      transfer = clientsocket.recv(20)
+			if not transfer: break
+			if (transfer == "quit"):
+				exit()
+			else:
+				transfer = transfer.split(';')
+			if transfer == ['']: break
+			print "received: " + str(transfer)
+			inputParser(transfer[0].strip(),float(transfer[1]),new_picar)
+			clientsocket.send("true")
+
+def inputParser(command,number,car):
+	print 'executing ' + command + ' for ' + str(number) +' seconds'
+	if   command == "forward":
+		car.go_forward(number)
+	elif command == "backward":
+		car.go_backward(number)
+	elif command == "right":
+		car.go_forward_right(number)
+	elif command == "left":
+		car.go_forward_left(number)
+	elif command == "backward right":
+		car.go_backward_right(number)
+	elif command == "backward left":
+		car.go_backward_left(number)
+	else:
+		car.stop()
+
+
+createPiReceiver()
+exit()

get_images_from_pi.py

@@ -0,0 +1,16 @@
+import os
+from PIL import Image
+
+def get_image(count):
+  scp_command = 'scp pi@192.168.2.5:Desktop/gregTest.jpg ' + 'images/' + str(count) + '.jpg'
+  os.system(scp_command)
+  return
+	# os.system('scp pi@192.168.2.5:Desktop/gregTest.jpg gregTest.jpg')
+  #use following to create key if needed:  https://www.raspberrypi.org/documentation/remote-access/ssh/passwordless.md
+
+def valid_image(path):
+	try:
+		Image.open(path)
+	except IOError:
+		return False
+	return True