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 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 backward(direction=None): _CONTROLLER.pid.set_target( compass.getHeading() if direction is None else direction) _CONTROLLER.backward() _CONTROLLER.start() current_direction = DIRECTION_BACKWARD
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()