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) count += 1
#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() karte.updateRoboPos(deltaL,deltaR,kompassCourse) karte.saveRoboPath() encoder.clearEncoderLR()
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( x, y, pose, obstacles) steering_output, speed = planer.set_modus(x, y, pose, steer, speed, avoid_steering, max_left, max_right, pumper.em_stop()) print(steering_output) avoided_obstacles = avoid.avoided_obst() transmit.send_data(obstacles, avoided_obstacles, [[x, y]]) #obstacles[0] = [x, y,pose] #scans.append(obstacles) #pickle_file = open("scanfile.p", "wb") #pickle.dump(scans, pickle_file) #pickle_file.close()