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