def supermagiskt():
global done
while not done:
m = ultrasonic.get_middle()
u = n + vector.from_polar(m, compass.getHeading())
l = ultrasonic.get_left()
v = n + vector.from_polar(l, compass.getHeading() + math.radians(30))
r = ultrasonic.get_right()
w = n + vector.from_polar(r, compass.getHeading() - math.radians(30))
data = []
if m <= 300 and m >= 15:
data.append([u.x, u.y])
if l <= 300 and l >= 15:
data.append([v.x, v.y])
if r <= 300 and l >= 15:
data.append([w.x, w.y])
if len(data) == 0:
maplogger.log(position=[[n.x, n.y]], heading=compass.getHeading())
else:
maplogger.log(position=[[n.x, n.y]],
heading=compass.getHeading(),
walls=data)
time.sleep(0.2)
def supermagiskt():
global done
while not done:
m = ultrasonic.get_middle()
u = n + vector.from_polar(m, compass.getHeading())
l = ultrasonic.get_left()
v = n + vector.from_polar(l, compass.getHeading() + math.radians(30))
r = ultrasonic.get_right()
w = n + vector.from_polar(r, compass.getHeading() - math.radians(30))
data = []
if m <= 300 and m >= 15:
data.append([u.x, u.y])
if l <= 300 and l >= 15:
data.append([v.x, v.y])
if r <= 300 and l >= 15:
data.append([w.x, w.y])
if len(data) == 0:
maplogger.log(position=[[n.x, n.y]], heading=compass.getHeading())
else:
maplogger.log(position=[[n.x, n.y]], heading=compass.getHeading(), walls=data)
time.sleep(0.2)
def turn_to(heading, error=math.radians(4)):
pid = PID(3, 0.1, 5, -50, 50)
pid.set_target(heading)
pid.difference = compass.angleDifference
h = compass.getHeading()
while True:
ret = pid.update(h)
if ret < 0:
motors.right(50-ret)
elif ret > 0:
motors.left(50+ret)
if abs(compass.angleDifference(h, heading)) <= error:
motors.stop()
time.sleep(0.5)
if abs(compass.angleDifference(compass.getHeading(), heading)) <= error:
break
h = compass.getHeading()
time.sleep(0.05)
def turn_to(heading, error=math.radians(4)):
pid = PID(3, 0.1, 5, -50, 50)
pid.set_target(heading)
pid.difference = compass.angleDifference
h = compass.getHeading()
while True:
ret = pid.update(h)
if ret < 0:
motors.right(50 - ret)
elif ret > 0:
motors.left(50 + ret)
if abs(compass.angleDifference(h, heading)) <= error:
motors.stop()
time.sleep(0.5)
if abs(compass.angleDifference(compass.getHeading(),
heading)) <= error:
break
h = compass.getHeading()
time.sleep(0.05)
def run(self):
dt = time.time() - self.t
self.t = time.time()
direction = compass.getHeading()
ret = self.pid.update(direction, dt)
if ret < 0:
self.r_speed = 100 + ret
self.l_speed = 100
if ret > 0:
self.r_speed = 100
self.l_speed = 100 - ret
if self.drive_forward:
LEFT.forward(self.l_speed)
RIGHT.forward(self.r_speed)
elif self.drive_backward:
LEFT.backward(self.l_speed)
RIGHT.backward(self.r_speed)
def run(self):
dt = time.time() - self.t
self.t = time.time()
direction = compass.getHeading()
ret = self.pid.update(direction, dt)
if ret < 0:
self.r_speed = 100 + ret
self.l_speed = 100
if ret > 0:
self.r_speed = 100
self.l_speed = 100 - ret
if self.drive_forward:
LEFT.forward(self.l_speed)
RIGHT.forward(self.r_speed)
elif self.drive_backward:
LEFT.backward(self.l_speed)
RIGHT.backward(self.r_speed)
def count():
global n
n += vector.from_polar(distance.HALF_CIRCUMFERENCE, compass.getHeading())
maplogger.log(position=[[n.x, n.y]], heading=compass.getHeading(), walls=data)
time.sleep(0.2)
try:
maplogger.initialize("/var/www/map.txt")
robot.on_battery_low(avsluta)
compass.calibrate(5)
input("Enter")
logger = threading.Thread(target=supermagiskt)
logger.start()
distance.start_measuring(count)
motors.forward()
t = 0
last = compass.getHeading()
while True:
left = ultrasonic.get_left()
middle = ultrasonic.get_middle()
right = ultrasonic.get_right()
if left <= 50 and right <= 50:
distance.stop_measuring()
motors.stop()
robot.turn_to(compass.getHeading() + math.pi, math.radians(10))
last = compass.getHeading()
distance.start_measuring(count)
motors.forward()
elif left <= 50:
distance.stop_measuring()
motors.stop()
def start(self):
self.last = compass.getHeading()
self.t = time.time()
super().start()
def backward(direction=None):
_CONTROLLER.pid.set_target(compass.getHeading() if direction is None else direction)
_CONTROLLER.backward()
_CONTROLLER.start()
current_direction = DIRECTION_BACKWARD
def start(self):
self.last = compass.getHeading()
self.t = time.time()
super().start()
def backward(direction=None):
_CONTROLLER.pid.set_target(
compass.getHeading() if direction is None else direction)
_CONTROLLER.backward()
_CONTROLLER.start()
current_direction = DIRECTION_BACKWARD
def count():
global n
n += vector.from_polar(distance.HALF_CIRCUMFERENCE, compass.getHeading())
time.sleep(0.2)
try:
maplogger.initialize("/var/www/map.txt")
robot.on_battery_low(avsluta)
compass.calibrate(5)
input("Enter")
logger = threading.Thread(target=supermagiskt)
logger.start()
distance.start_measuring(count)
motors.forward()
t = 0
last = compass.getHeading()
while True:
left = ultrasonic.get_left()
middle = ultrasonic.get_middle()
right = ultrasonic.get_right()
if left <= 50 and right <= 50:
distance.stop_measuring()
motors.stop()
robot.turn_to(compass.getHeading() + math.pi, math.radians(10))
last = compass.getHeading()
distance.start_measuring(count)
motors.forward()
elif left <= 50:
distance.stop_measuring()
motors.stop()
