77 lines
2.0 KiB
Python
77 lines
2.0 KiB
Python
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():
|
|
try:
|
|
autopilot=AUTOPILOT()
|
|
while True:
|
|
try:
|
|
try:
|
|
autopilot.setSensorValues();
|
|
autopilot.printValues();
|
|
print("Richtung: {0}".format(autopilot.moveStatus));
|
|
if autopilot.moveStatus!=1:
|
|
autopilot.forward();
|
|
else:
|
|
autopilot.statusTest();
|
|
except ForwardMoveException:
|
|
if randint(0,1):
|
|
autopilot.backward();
|
|
else:
|
|
if randint(0,1):
|
|
autopilot.turnLeft();
|
|
else:
|
|
autopilot.turnRight();
|
|
except LeftMoveException:
|
|
core.turnLeft();
|
|
except RightMoveException:
|
|
core.turnRight();
|
|
except MoveException:
|
|
autopilot.stop();
|
|
sleep(0.5);
|
|
except KeyboardInterrupt:
|
|
print("Verlasse Autopilot...")
|
|
def doIt(order):
|
|
switcher = {
|
|
'w': lambda: core.forward(),
|
|
's': lambda: core.backward(),
|
|
'd': lambda: core.turnRight(),
|
|
'a': lambda: core.turnLeft(),
|
|
' ': lambda: core.stop(),
|
|
'i': lambda: core.printValues(),
|
|
'h': lambda: help(),
|
|
'q': lambda: autopilot(),
|
|
'x': lambda: core.turnDegree(int(input("Grad:"))),
|
|
'r': lambda: core.runCm(int(input("cm:"))),
|
|
}
|
|
func = switcher.get(order, lambda: print("Der gewuenschte Befehl steht nicht zur Verfuegung"))
|
|
return func();
|
|
print("Herzlich Willkommen im manuellen Controll-Interface!\n")
|
|
try:
|
|
while True:
|
|
#core.setSensorValues()
|
|
modus=getch.getch();
|
|
print("Erinaco>>{0}".format(modus))
|
|
doIt(modus);
|
|
except KeyboardInterrupt:
|
|
print("Verlasse Erinaco...")
|
|
core.__del__();
|