Beispiel #1
0
    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)
Beispiel #2
0
    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)
Beispiel #3
0
    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))
Beispiel #4
0
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)
Beispiel #5
0
#! 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()
Beispiel #6
0
            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())