mirror of
https://github.com/kevinveenbirkenbach/erinaco.git
synced 2024-11-23 14:31:04 +01:00
93 lines
2.7 KiB
Python
93 lines
2.7 KiB
Python
"""
|
|
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;
|