def getCourse(self):
#Obstacles aus karte lesen
obstacles = self.karte.getObstacles()
#Wenn Vorne kein Platz retour
steer, speed = self.navigation.MinInFront(obstacles)
if speed == -1:
return (steer, speed)
#Wenn Pumper Hinderniss erkannt retour
pumpL, pumpR = self.karte.getPumperStatus()
if pumpL == True:
return (0, -1)
if pumpR == True:
return (0, -1)
#Bei Hinderniss R oder L nicht drehen
#obstacles=self.navigation.Querab(obstacles)
#Parallele Wand erkennen
obstacles = self.navigation.WandParallel(obstacles)
#Suche Luecke in Dist
obstacles = self.navigation.LueckeInX(80, obstacles)
sollkurs = self.karte.getZielkurs()
istkurs = Kompass.getKompass()
#Lokale Koordinaten in Globale umwandeln
LueckeList = self.navigation.LokalZuGlobal(istkurs, obstacles)
#Suche beste Luecke um nach Zielkurs zu kommen
to_steer = self.navigation.BesteLueckeKompass(sollkurs, istkurs,
obstacles)
#print(to_steer)
#Ausgabe der Motor Comands steer und speed
steer, speed = self.navigation.SteuerkursInSteerSpeed(to_steer)
#print(steer,speed)
return (steer, speed)
def getCourse(self):
#Obstacles aus karte lesen
obstacles=self.karte.getObstacles()
#Wenn Vorne kein Platz retour
steer,speed=self.navigation.MinInFront(obstacles)
if speed == -1:
return(steer,speed)
#Wenn Pumper Hinderniss erkannt retour
pumpL,pumpR=self.karte.getPumperStatus()
if pumpL == True:
return(0,-1)
if pumpR == True:
return(0,-1)
#Bei Hinderniss R oder L nicht drehen
#obstacles=self.navigation.Querab(obstacles)
#Parallele Wand erkennen
obstacles=self.navigation.WandParallel(obstacles)
#Suche Luecke in Dist
obstacles=self.navigation.LueckeInX(80,obstacles)
sollkurs=self.karte.getZielkurs()
istkurs=Kompass.getKompass()
#Lokale Koordinaten in Globale umwandeln
LueckeList=self.navigation.LokalZuGlobal(istkurs,obstacles)
#Suche beste Luecke um nach Zielkurs zu kommen
to_steer=self.navigation.BesteLueckeKompass(sollkurs,istkurs,obstacles)
#print(to_steer)
#Ausgabe der Motor Comands steer und speed
steer,speed=self.navigation.SteuerkursInSteerSpeed(to_steer)
#print(steer,speed)
return(steer,speed)
x,y,pose=karte.getRoboPos()
#print("Pose:"+str(int(pose)))
grid.setStartInGrid(int(x/10),int(y/10))
walls=karte.getObstacles()
grid.obstaclesInGrid(walls)
#grid.addClearance()
#grid.saveGridObstacles()
#solved_path = grid.getSolvedPath(motor)
#Position updaten
weggeber.runAllTime()
deltaL,deltaR=weggeber.getPulseLR()
#print(deltaL,deltaR)
kompassCourse=Kompass.getKompass()
karte.updateRoboPos(deltaL,deltaR,kompassCourse)
karte.saveRoboPath()
encoder.clearEncoderLR()
encoder.clearEncoderDist()
weggeber.clearWeggeberLR()
#Send Data via NET
solved_path = []
roundet_walls=grid.getRoundetWalls()
#print(roundet_walls)
#json.sendVisual(roundet_walls, [[x,y]],solved_path)
#Ziel erreicht?
logic.setRoboPos(x,y,pose)
kurs_to_ziel,dist_to_ziel=plan.calcGlobalZielkurs(x,y,pose)
print("KursZuZiel: "+str(kurs_to_ziel))
while Robo == True:
motor.setCommand(0, 0)
sleep(1)
#Obstacles eintragen
obstacles = scanner.getNewDistValues()
#obstacles=[[0,50],[10,50],[20,50],[30,50],[40,50],[50,50]]
karte.updateObstacles(obstacles)
pumperL, pumperR = encoder.getPumper()
karte.updateHardObstacles(pumperL, pumperR)
#Position updaten
deltaL, deltaR = encoder.getPulseLR()
kompassCourse = Kompass.getKompass()
karte.updateRoboPos(deltaL, deltaR, kompassCourse)
karte.saveRoboPath()
#Grid mit Astar
x, y, pose = karte.getRoboPos()
grid.setStartInGrid(int(x / 10), int(y / 10))
walls = karte.getObstacles()
grid.obstaclesInGrid(walls)
#grid.addClearance()
grid.saveGridObstacles()
#motor.setCommand(0,0)
path = grid.getSolvedPath(motor)
grid.saveGridPath(path)
#! python
# -*- coding: utf-8 -*-
from Motion import *
from Manuell import *
from Kompass import *
from time import sleep
from SMBUSBatt import *
from Scanner3d import *
if __name__ == "__main__":
man = Manuell()
motion = Motion()
kompass = Kompass()
batterie = SMBUSBatt()
scanner = Scanner()
start = time.time()
scanner.init_3D_scan(
min_pitch=10,
max_pitch=15,
min_heading=-15.0,
max_heading=15.0,
)
ThreadEncoder = Thread(target=man.runManuell, args=())
ThreadEncoder.daemon = True
ThreadEncoder.start()
return ("Slope")
else:
return (False)
def overcurrent(self):
if self.batterie.get_current() > self.MAX_CURRENT:
return ("overcurrent")
else:
return (False)
def not_turning(self):
heading = self.kompass.get_heading()
print(heading)
if heading == self.heading_old:
status = "not_turning"
else:
status = False
self.heading_old = heading
return (status)
if __name__ == "__main__":
kompass = Kompass()
batterie = SMBUSBatt()
limits = Limits(kompass, batterie)
print(limits.danger())
print(limits.danger())
