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))
logic.setZielkurs(kurs_to_ziel)
plan.zielErreicht(dist_to_ziel,motor)
#Plan next Steps
steer,speed=logic.wsa(dist_front,dist_left,dist_right,pumperL,pumperR)
steer,speed=logic.checkPumperStatus(pumperL,pumperR,steer,speed)
print(steer,speed)
speed_L,speed_R = encoder.getSpeedLR()
motor.booster(speed_L,speed_R)
#sleep(0.3)
#motor.setCommand(0,0)
#Manuell Control
#steer,speed=manuell.getManuellCommand()
motor.setCommand(steer,speed)
print(karte.getRoboPos())
