Autopilot hinzugefuegt

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "sensoren/Adafruit_Python_DHT"]
+	path = sensoren/Adafruit_Python_DHT
+	url = https://github.com/adafruit/Adafruit_Python_DHT.git

autopilot.py

@@ -0,0 +1,92 @@
+"""
+Autopilot-Klasse schmeisst Exceptions und stoppt Erinaco bei Fehlern
+
+@author kf
+@since 	2017-04-15
+
+Status-Codes 
+0: Stopp
+1: Forward
+2: Backward
+3: Left
+4: Right
+"""
+
+#Bewegungs-Exeptions:
+class MoveException(Exception):	
+	pass
+class RightMoveException(MoveException):
+	pass
+class LeftMoveException(MoveException):
+	pass
+class ForwardMoveException(MoveException):
+	pass
+
+#Hauptklasse
+from core import Core as CORE
+from threading import Thread as THREAD
+from time import sleep 
+class Autopilot(CORE,THREAD):
+	def __init__(self):
+		CORE.__init__(self);
+		THREAD.__init__(self);
+		self.moveStatus=0; 
+		self.ultraschalminimum=10;
+		self.threadRun=True;
+		#self.start();
+	def run(self):
+		x=1
+		while self.threadRun:
+			self.statusTest();
+			print("Autopilot-Test #{0}".format(x));			
+			#sleep(0.2);
+			x+=1		
+	def statusTest(self):
+		switcher = {
+			1: lambda: self.testForward(),
+			3: lambda: self.testLeft(),
+			4: lambda: self.testRight(),
+		}
+		func = switcher.get(self.moveStatus, lambda:0)
+		return func();		
+	def testLeft(self):
+		ultraschallLinks=self.ultraschallLinks.getValue();
+		if(ultraschallLinks<=self.ultraschalminimum or self.infarotLinks.getValue()):
+			self.stop();					
+			raise LeftMoveException("Abstand Ultraschall-Links betraegt {0}cm und Infarot-Links hat Wert {1}".format(ultraschallLinks,self.infarotLinks.getValue()));
+	def testRight(self):
+		ultraschallRechts=self.ultraschallRechts.getValue();
+		if(ultraschallRechts<=self.ultraschalminimum or self.infarotRechts.getValue()):
+			self.stop();					
+			raise RightMoveException("Abstand Ultraschall-Rechts betraegt {0}cm und Infarot-Rechts hat Wert {1}".format(ultraschallRechts,self.infarotRechts.getValue()));
+	def testForward(self):
+		ultraschallMitte=self.ultraschallMitte.getValue();
+		if(ultraschallMitte<=self.ultraschalminimum 
+			or self.ultraschallRechts.getValue()<=self.ultraschalminimum 
+			or self.ultraschallLinks.getValue()<=self.ultraschalminimum 
+			or self.infarotMitteLinks.getValue() 
+			or self.infarotMitteRechts.getValue()):
+			self.stop();					
+			raise ForwardMoveException("Abstand Ultraschallmitte betraegt {0}cm. Infarot-Mitte-Links: {1}. Infarot-Mitte-Rechts:{2}".format(ultraschallMitte,self.infarotMitteLinks.getValue(),self.infarotMitteRechts.getValue()));	
+	def turnLeft(self):
+		self.statusTest();
+		self.moveStatus=3;
+		CORE.turnLeft(self);
+	def turnRight(self):
+		self.statusTest();
+		self.moveStatus=4;	
+		CORE.turnRight(self);
+	def forward(self):
+		self.statusTest();
+		self.moveStatus=1;		
+		CORE.forward(self);
+	def backward(self):
+		self.statusTest();
+		self.moveStatus=2;
+		CORE.backward(self);
+	def stop(self):
+		self.statusTest();
+		self.moveStatus=0;
+		CORE.stop(self);
+	def __del__(self):
+		self.threadRun=False;

manual_controll.py

@@ -1,7 +1,43 @@
 from time import sleep
 import getch
 from core import Core as CORE
+from autopilot import Autopilot as AUTOPILOT
+from autopilot import MoveException,RightMoveException,LeftMoveException,ForwardMoveException
+from random import randint
 core=CORE();
+#Enthaelt die Hilfe
+def help():
+	print("MOTION:")
+	print("Forward:  w");
+	print("Backward: s");
+	print("Left:     a");
+	print("Right:    d");
+	print("Stop:     Space\n")
+	print("GENERAL:")
+	print("Help:	 h\n");
+	print("MODE:")
+	print("Automatisch:     q")
+	print("Halbautomatisch: w")
+	print("Manuell:         e")
+def autopilot():
+	core=AUTOPILOT()
+	#core.start();
+	while True:	
+		try:	
+			core.forward();
+		except ForwardMoveException:
+			if randint(0,1):			
+				core.backward();
+			else:
+				if randint(0,1):
+					core.turnLeft();				
+				else:
+					core.turnRight();
+		except LeftMoveException:
+			core.turnLeft();
+		except RightMoveException:
+			core.turnRight();
+		sleep(0.5);
 def doIt(order):
 	switcher = {
         	'w': lambda: core.forward(),
@@ -10,14 +46,19 @@ def doIt(order):
 		'a': lambda: core.turnLeft(),
 		' ': lambda: core.stop(),
 		'i': lambda: core.printValues(),
+		'h': lambda: help(),
+		'q': lambda: autopilot(),
 	}
-	func = switcher.get(order, "Der gewuenschte Befehl steht nicht zur Verfuegung")
+	func = switcher.get(order, lambda: print("Der gewuenschte Befehl steht nicht zur Verfuegung"))
 	return func();
-print("Herzlich Willkommen im manuellen Controll-Interface!") 
+print("Herzlich Willkommen im manuellen Controll-Interface!\n") 
 try:	
 	while True:
 		input=getch.getch();
 		print("Erinaco>>{0}".format(input))
 		doIt(input);
+except MoveException:
+	print("Move Exception...")
 except KeyboardInterrupt:
 	print("Verlasse Erinaco...")
+	core.__del__();

sensoren/Adafruit_Python_DHT

@@ -0,0 +1 @@
+Subproject commit da8cddf7fb629c1ef4f046ca44f42523c9cf2d11

sensoren/dht11.py

@@ -0,0 +1,11 @@
+import Adafruit_DHT
+class Dht11(object):
+	def __init__(self,pin):
+		self.pin = pin;
+	def setValues(self):
+		self.humidity, self.temperature = Adafruit_DHT.read_retry(Adafruit_DHT.DHT11, self.pin)
+	def getLuftfeuchtigkeit(self):
+		return self.humidity;
+	def getTemperatur(self):
+		return self.temperature;		
+	

sensoren/ultraschall.py

@@ -0,0 +1,20 @@
+#!/usr/bin/python3
+import time
+import RPi.GPIO as GPIO
+class Ultraschall(object):
+	def __init__(self,trigPin,echoPin):
+		self.trigPin=trigPin
+		self.echoPin=echoPin
+		GPIO.setup(echoPin, GPIO.IN)
+		GPIO.setup(trigPin, GPIO.OUT)
+	def getValue(self):
+    		GPIO.output(self.trigPin, True)
+    		time.sleep(0.00001) # 10 Mikrosekunden
+    		GPIO.output(self.trigPin, False)
+    		while GPIO.input(self.echoPin) == 0:
+        		pass
+    		start = time.time()   
+    		while GPIO.input(self.echoPin) == 1:
+        		pass
+    		ende = time.time()
+    		return ((ende - start) * 34300) / 2

vero.py

@@ -4,25 +4,28 @@
 	@since 2017-04-15
 """
 from sensoren.boolsensor import Boolsensor as BOOLSENSOR 
+from sensoren.ultraschall import Ultraschall as ULTRASCHALL
+#from sensoren.dht11 import Dht11 as DHT11
 class Vero(object):
 	def __init__(self):
 		self.pir=BOOLSENSOR(19);
-		#self.dht;
+		#self.dht=DHT11(26);
 		#self.camera;
-		#self.ultraschallLinks;
-		#self.ultraschallMitte;
-		#self.ultraschallRechts;
+		self.ultraschallLinks=ULTRASCHALL(5,17);
+		self.ultraschallMitte=ULTRASCHALL(6,27);
+		self.ultraschallRechts=ULTRASCHALL(13,22);
 		self.infarotLinks=BOOLSENSOR(12);
 		self.infarotRechts=BOOLSENSOR(21);
 		self.infarotMitteLinks=BOOLSENSOR(16);
 		self.infarotMitteRechts=BOOLSENSOR(20);
 	def printValues(self):
+		#self.dht.setValues();
 		print("PIR:			{0}".format(self.pir.getValue()));
 		#print("Temperatur:		{0}".format(self.dht.getTemperatur()));
 		#print("Luftfeuchtigkeit:	{0}".format(self.dht.getLuftfeuchtigkeit()));
-                #print("PIR:              {0}".format(self.ultraschallLinks.getValue()));
-                #	self.ultraschallMitte.getValue();
-                #	self.ultraschallRechts.getValue();
+		print("Ultraschall-Links:	{0}cm".format(self.ultraschallLinks.getValue()));
+		print("Ultraschall-Mitte:	{0}cm".format(self.ultraschallMitte.getValue()));
+		print("Ultraschall-Rechts:	{0}cm".format(self.ultraschallRechts.getValue()));
 		print("Infarot Links:		{0}".format(self.infarotLinks.getValue()));
 		print("Infarot Mitte-Links:	{0}".format(self.infarotMitteLinks.getValue()));
 		print("Infarot Mitte-Rechts:	{0}".format(self.infarotMitteRechts.getValue()));