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())