ThreadScanAllTime.start() ThreadEncoder = Thread(target=encoder.runAllTime, args=()) ThreadEncoder.daemon = True ThreadEncoder.start() sleep(1) 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()
##ThreadEncoder=Thread(target=weggeber.runAllTime,args=()) ##ThreadEncoder.daemon=True ##ThreadEncoder.start() sleep(1) while Robo==True: #os.system("clear") #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)
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( 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()