#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)) steer, speed = plan.nextStep(path, x, y, pose) #wall_near=grid.obstacleNear() #steer,speed=plan.ZuNahe(steer,speed,wall_near)
#get Distances from IR-Sensors dist_front, dist_left , dist_right, obstacles = 100,100,100,[[10,10],[20,20]]#scanner.getFixData() print(dist_front,dist_left, dist_right) #Obstacles eintragen karte.updateObstacles(obstacles) #Obstacles von Pumper eintragen pumperL,pumperR=encoder.getPumper() deltaH = encoder.getDistCounts() pumperL, pumperR = logic.blocked(steer,speed,deltaH, pumperL, pumperR) karte.updateHardObstacles(pumperL,pumperR) #Grid 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()
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() avoid_steering, max_left, max_right = avoid.get_nearest_obst(