#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(
