class calibWallRunner(Thread): def __init__(self): Thread.__init__(self) self.askStop = False self.thymio = ThymioController() def run(self): self.thymio.set_motors(100, 100) while self.thymio.get_prox_v()[0] < 500 and self.thymio.get_prox_v()[1] < 500: sleep(0.1) self.thymio.set_motors(0, 0) global WALL_VALUE WALL_VALUE = self.thymio.get_prox_h()[2] global d d["wall"] = WALL_VALUE global running running = False
class progRunner(Thread): def __init__(self, prog): Thread.__init__(self) self.prog = prog self.askStop = False self.options = { "MoveForward": self.move_forward, "MoveBackward": self.move_backward, "Turn": self.turn, "FollowLine": self.follow_line, "SetTopColor": self.set_top_color, "SetBottomColor": self.set_bottom_color, } self.color = { "off": [0, 0, 0], "red": [32, 0, 0], "blue": [0, 0, 32], "green": [0, 32, 0], "pink": [32, 10, 10], "orange": [32, 20, 0], "white": [32, 32, 32], } self.thymio = ThymioController() def run(self): for instr in self.prog: action = instr["Action"] param = instr["Param"] self.options[action](param) if self.askStop: break global running self.thymio.set_motors(0, 0) self.thymio.set_led_bottom_left(self.color["off"]) self.thymio.set_led_bottom_right(self.color["off"]) self.thymio.set_led_top(self.color["off"]) running = False return def move_forward(self, param): self.thymio.set_motors(300, 300) if param == "UntilWall": self.until_wall_front() elif param == "UntilBlackFloor": self.until_black_floor() elif param == "UntilWhiteFloor": self.until_white_floor() elif param == "10cm": self.until_time(10) elif param == "20cm": self.until_time(20) elif param == "50cm": self.until_time(50) return def move_backward(self, param): self.thymio.set_motors(-300, -300) if param == "UntilWall": self.until_wall_back() elif param == "UntilBlackFloor": self.until_black_floor() elif param == "UntilWhiteFloor": self.until_white_floor() else: self.until_time(int(param[:2])) return def turn(self, param): angle = 0 if param[0] == "R": angle = int(param[5:]) self.thymio.set_motors(300, -300) else: angle = int(param[4:]) self.thymio.set_motors(-300, 300) global TIME_ANGLE_CONV target_time = angle * TIME_ANGLE_CONV time = 0.0 while self.askStop is False and time < target_time: sleep(0.1) time += 0.1 return def follow_line(self, param): if param == "UntilWall": self.follow_line_until_wall() else: self.follow_line_until_time(int(param[:2])) def set_top_color(self, param): self.thymio.set_led_top(self.color[param]) return def set_bottom_color(self, param): self.thymio.set_led_bottom_left(self.color[param]) self.thymio.set_led_bottom_right(self.color[param]) return def until_wall_front(self): while self.thymio.get_prox_h()[2] < WALL_VALUE and self.askStop is False: sleep(0.1) return def until_wall_back(self): while self.thymio.get_prox_h()[5] < 1000 and self.thymio.get_prox_h()[6] < 1000 and self.askStop == False: sleep(0.1) return def until_black_floor(self): while self.thymio.get_prox_v()[0] > 500 and self.thymio.get_prox_v()[1] > 500 and self.askStop is False: sleep(0.1) return def until_white_floor(self): while self.thymio.get_prox_v()[0] < 500 and self.thymio.get_prox_v()[1] < 500 and self.askStop is False: sleep(0.1) return def until_time(self, cm): global TIME_CM_CONV target_time = cm * TIME_CM_CONV time = 0.0 while self.askStop is False and time < target_time: sleep(0.1) time += 0.1 def follow_line_until_wall(self): left = True while self.thymio.get_prox_h()[2] < WALL_VALUE and self.askStop is False: ground = self.thymio.get_prox_v() if ground[0] < 500 and ground[1] < 500: self.thymio.set_motors(300, 300) elif ground[0] < 500 and ground[1] > 500: self.thymio.set_motors(50, 300) left = True elif ground[0] > 500 and ground[1] < 500: self.thymio.set_motors(300, 50) left = False else: if left: self.thymio.set_motors(-200, 200) else: self.thymio.set_motors(200, -200) self.thymio.set_motors(0, 0) def follow_line_until_time(self, cm): left = True target_time = cm * TIME_CM_CONV time = 0.0 while self.askStop is False and time < target_time: ground = self.thymio.get_prox_v() if ground[0] < 500 and ground[1] < 500: self.thymio.set_motors(300, 300) time += 0.1 elif ground[0] < 500 and ground[1] > 500: self.thymio.set_motors(50, 300) left = True time += 0.06 elif ground[0] > 500 and ground[1] < 500: self.thymio.set_motors(300, 50) left = False time += 0.06 else: if left: self.thymio.set_motors(-200, 200) else: self.thymio.set_motors(200, -200) self.thymio.set_motors(0, 0)