diff --git a/examples/automata/__init__.py b/examples/automata/__init__.py index dfdf8a5..0d51a0f 100644 --- a/examples/automata/__init__.py +++ b/examples/automata/__init__.py @@ -2,17 +2,18 @@ import logging import time from collections import Counter -from pylgbst.hub import MoveHub, COLOR_NONE, COLOR_BLACK, COLORS, COLOR_CYAN, COLOR_BLUE, COLOR_RED +from pylgbst.hub import MoveHub, COLOR_NONE, COLOR_BLACK, COLORS, COLOR_CYAN, COLOR_BLUE, COLOR_RED, COLOR_YELLOW, \ + COLOR_WHITE from pylgbst.peripherals import EncodedMotor class Automata(object): - BASE_SPEED = 0.75 + BASE_SPEED = 1.0 def __init__(self): super(Automata, self).__init__() self.__hub = MoveHub() - # self.__hub.vision_sensor.subscribe(self.__on_sensor) + self.__hub.vision_sensor.subscribe(self.__on_sensor) self._sensor = [] def __on_sensor(self, color, distance=-1): @@ -31,51 +32,48 @@ class Automata(object): def get_color(self): res = self._sensor self._sensor = [] - logging.info("Sensor data: %s", res) + logging.debug("Sensor data: %s", res) cnts = Counter([x[0] for x in res]) clr = cnts.most_common(1)[0][0] if cnts else COLOR_NONE if clr == COLOR_CYAN: clr = COLOR_BLUE + self.__hub.led.set_color(clr) return clr def left(self): - self.__hub.motor_AB.angled(270, self.BASE_SPEED, -self.BASE_SPEED, end_state=EncodedMotor.END_STATE_HOLD) + self.__hub.motor_AB.angled(270, self.BASE_SPEED, -self.BASE_SPEED, end_state=EncodedMotor.END_STATE_FLOAT) time.sleep(0.1) self.__hub.motor_AB.stop() def right(self): - self.__hub.motor_AB.angled(235, -self.BASE_SPEED, self.BASE_SPEED, end_state=EncodedMotor.END_STATE_HOLD) + self.__hub.motor_AB.angled(270, -self.BASE_SPEED, self.BASE_SPEED, end_state=EncodedMotor.END_STATE_FLOAT) time.sleep(0.1) self.__hub.motor_AB.stop() def forward(self): - self.__hub.motor_AB.angled(500, self.BASE_SPEED) + self.__hub.motor_AB.angled(600, self.BASE_SPEED) def backward(self): - self.__hub.motor_AB.angled(500, -self.BASE_SPEED) + self.__hub.motor_AB.angled(600, -self.BASE_SPEED) if __name__ == '__main__': logging.basicConfig(level=logging.INFO) bot = Automata() - bot.left() - bot.left() - bot.left() - bot.left() - - exit(0) - - color = COLOR_NONE + color = None cmds = [] - while color != COLOR_RED: + while color != COLOR_NONE: bot.feed_tape() color = bot.get_color() - logging.warning(COLORS[color]) - cmds.append(COLORS[color]) - exp = ['BLUE', 'BLUE', 'BLUE', 'WHITE', 'BLUE', 'BLUE', 'WHITE', 'BLUE', 'WHITE', 'YELLOW', 'BLUE', 'BLUE', 'BLUE', - 'BLUE', 'YELLOW', 'WHITE', 'RED'] - logging.info("Exp: %s", exp) - logging.info("Act: %s", cmds) - assert exp == cmds + logging.warning(COLORS[color]) + + if color == COLOR_BLUE: + bot.forward() + elif color == COLOR_RED: + bot.backward() + elif color == COLOR_YELLOW: + bot.left() + elif color == COLOR_WHITE: + bot.right()