#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)) print("DistZuZiel: "+str(dist_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) #print("Path:"+str(path))
speed = 1 halt = 1 scans = [] steering_output = 0 scanner.scanner_reset() while True: pumper.status_led("on") if battery.get_relative_charge() < 30: print(3 * "BATTERY-EMPTY ") #Position weggeber.update_weggeberpulse() deltaL, deltaR = weggeber.get_pulseLR() karte.updateRoboPos(deltaL, deltaR) x, y, pose = karte.getRoboPos() print(x, y, pose) ## soll_x, soll_y, soll_pose = pathplaning.move_to_pose(x, y, radians(pose), x_goal, y_goal, radians(theta_goal)) ## print(soll_x, soll_y, soll_pose) ## steering_angle = pathplaning.get_steering_angle(radians(soll_pose), radians(pose)) ## motion.setMotion(steering_angle, 0.2) ## print(steering_angle) scanner.do_scan(step=25) scan_data = scanner.get_scan_data() karte.updateObstacles(scan_data) obstacles = karte.getObstacles()
ThreadScanAllTime=Thread(target=scanner.runAllTime, args=(1,)) ThreadScanAllTime.daemon=True ThreadScanAllTime.start() ThreadEncoder=Thread(target=encoder.runAllTime,args=()) ThreadEncoder.daemon=True ThreadEncoder.start() while Robo==True: obstacles=scanner.getNewDistValues() karte.updateObstacles(obstacles) deltaDist=encoder.getDistCounts() steerDiff=encoder.getSteerDiff() kompassCourse=Kompass.getKompass() karte.updateRoboPos(deltaDist,steerDiff,kompassCourse) pumperL,pumperR=encoder.getPumper() karte.updateHardObstacles(pumperL,pumperR) steer,speed=plan.getCourse() print(steer,speed) motor.setCommand(steer,speed) sleep(1.5)
plan = Plan(karte, navigation) encoder = Encoder() motor = Motor() ThreadScanAllTime = Thread(target=scanner.runAllTime, args=(1, )) ThreadScanAllTime.daemon = True ThreadScanAllTime.start() ThreadEncoder = Thread(target=encoder.runAllTime, args=()) ThreadEncoder.daemon = True ThreadEncoder.start() while Robo == True: obstacles = scanner.getNewDistValues() karte.updateObstacles(obstacles) deltaDist = encoder.getDistCounts() steerDiff = encoder.getSteerDiff() kompassCourse = Kompass.getKompass() karte.updateRoboPos(deltaDist, steerDiff, kompassCourse) pumperL, pumperR = encoder.getPumper() karte.updateHardObstacles(pumperL, pumperR) steer, speed = plan.getCourse() print(steer, speed) motor.setCommand(steer, speed) sleep(1.5)