mirror of
https://github.com/undera/pylgbst.git
synced 2020-11-18 19:37:26 -08:00
32eecac1a6
* It's HUB ID * Rename file * Working with official doc * Some progress * AttachedIO msg * device action impl * some better device alert impl * restructuring * Some port commands handled * Some command feedback waiting * Some more request-reply things * Some more request-reply things * Reworked msg classes * Getting back to UTs * Getting back to UTs * Facing sync lock problems * Facing sync lock problems * Testing it * Covering more with tests * handle actions * Hub class is almost covered * done coverage for Hub * done coverage for MoveHub * Button is tested * remove debug server from examples * Current and voltage tested * color sensor basic test * cover tilt sensor * motor sensor tested * constant motor * motor is tested * hold_speed impl for motor * motor commands recorded * some cleanup * some cleanup * some cleanup * debug * debug * FIX a bug * acc/dec profiles figured out * UT motor ops * UT motor ops * Get rid of weird piggyback * fix UT * Fix encoding? * fix test mb * More robust * Checked demo works * cosmetics * cosmetics * Maybe better test * fetching and decoding some device caps * describing devs * describing devs works * Applying modes we've learned * Simple and extensible dev attach * Reworking peripherals based on modes * Applying modes we've learned * implemented getting sensor data * fixed port subscribe * Added led out cmds on vision sensor * Worked on color-distance sensor * Introduce some locking for consistency * Improved it all * Travis flags * improve * improve * improve docs
83 lines
2.4 KiB
Python
83 lines
2.4 KiB
Python
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.peripherals import EncodedMotor
|
|
|
|
|
|
class Automata(object):
|
|
BASE_SPEED = 0.5
|
|
|
|
def __init__(self):
|
|
super(Automata, self).__init__()
|
|
self.__hub = MoveHub()
|
|
self.__hub.vision_sensor.subscribe(self.__on_sensor)
|
|
self._sensor = []
|
|
|
|
def __on_sensor(self, color, distance=-1):
|
|
logging.debug("Sensor data: %s/%s", COLORS[color], distance)
|
|
if distance <= 4:
|
|
if color not in (COLOR_NONE, COLOR_BLACK):
|
|
self._sensor.append((color, int(distance)))
|
|
logging.debug("Sensor data: %s", COLORS[color])
|
|
|
|
def feed_tape(self):
|
|
self.__hub.motor_external.angled(60, 0.5)
|
|
time.sleep(0.1)
|
|
self.__hub.motor_external.angled(60, 0.5)
|
|
time.sleep(0.1)
|
|
|
|
def get_color(self):
|
|
res = self._sensor
|
|
self._sensor = []
|
|
logging.info("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
|
|
return clr
|
|
|
|
def left(self):
|
|
self.__hub.motor_A.angled(270, self.BASE_SPEED, -self.BASE_SPEED, end_state=EncodedMotor.END_STATE_HOLD)
|
|
time.sleep(0.1)
|
|
self.__hub.motor_A.stop()
|
|
|
|
def right(self):
|
|
self.__hub.motor_B.angled(-320, self.BASE_SPEED, -self.BASE_SPEED, end_state=EncodedMotor.END_STATE_HOLD)
|
|
time.sleep(0.1)
|
|
self.__hub.motor_B.stop()
|
|
|
|
def forward(self):
|
|
self.__hub.motor_AB.angled(450, self.BASE_SPEED)
|
|
|
|
def backward(self):
|
|
self.__hub.motor_AB.angled(-450, self.BASE_SPEED)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
logging.basicConfig(level=logging.INFO)
|
|
bot = Automata()
|
|
|
|
bot.forward()
|
|
bot.right()
|
|
bot.forward()
|
|
bot.left()
|
|
bot.forward()
|
|
|
|
exit(0)
|
|
|
|
color = COLOR_NONE
|
|
cmds = []
|
|
while color != COLOR_RED:
|
|
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
|