|
1
|
|
|
|
|
2
|
|
|
from itertools import cycle |
|
3
|
|
|
from curio import sleep |
|
4
|
|
|
from bricknil import attach, start |
|
5
|
|
|
from bricknil.hub import DuploTrainHub |
|
6
|
|
|
from bricknil.sensor import DuploTrainMotor, DuploSpeedSensor, LED, DuploVisionSensor, DuploSpeaker |
|
7
|
|
|
from bricknil.const import Color |
|
8
|
|
|
import logging |
|
9
|
|
|
|
|
10
|
|
|
@attach(DuploSpeaker, name='speaker') |
|
11
|
|
|
@attach(DuploVisionSensor, name='vision_sensor', capabilities=['sense_color', 'sense_ctag', 'sense_reflectivity']) |
|
12
|
|
|
@attach(LED, name='led') |
|
13
|
|
|
@attach(DuploSpeedSensor, name='speed_sensor', capabilities=['sense_speed', 'sense_count']) |
|
14
|
|
|
@attach(DuploTrainMotor, name='motor') |
|
15
|
|
|
class Robot(DuploTrainHub): |
|
16
|
|
|
|
|
17
|
|
|
async def speed_sensor_change(self): |
|
18
|
|
|
speed = self.speed_sensor.value[DuploSpeedSensor.capability.sense_speed] |
|
19
|
|
|
count = self.speed_sensor.value[DuploSpeedSensor.capability.sense_count] |
|
20
|
|
|
logging.info(f'Speed sensor changed speed: {speed} count: {count}') |
|
21
|
|
|
|
|
22
|
|
|
async def vision_sensor_change(self): |
|
23
|
|
|
cap = DuploVisionSensor.capability |
|
24
|
|
|
color = self.vision_sensor.value[cap.sense_color] |
|
25
|
|
|
ctag = self.vision_sensor.value[cap.sense_ctag] |
|
26
|
|
|
reflt = self.vision_sensor.value[cap.sense_reflectivity] |
|
27
|
|
|
logging.info(f'Vision sensor changed color: {color} ctag: {ctag} reflt: {reflt}') |
|
28
|
|
|
|
|
29
|
|
|
async def motor_change(self): |
|
30
|
|
|
pass |
|
31
|
|
|
async def tilt_sensor_change(self): |
|
32
|
|
|
pass |
|
33
|
|
|
|
|
34
|
|
|
async def run(self): |
|
35
|
|
|
self.message_info("Running") |
|
36
|
|
|
|
|
37
|
|
|
colors = cycle([Color.red, Color.purple, Color.yellow, Color.blue, Color.white]) |
|
38
|
|
|
snd = DuploSpeaker.sounds |
|
39
|
|
|
sounds = cycle([snd.brake, snd.station, snd.water, snd.horn, snd.steam]) |
|
40
|
|
|
for i in range(5): |
|
41
|
|
|
await self.led.set_color(next(colors)) |
|
42
|
|
|
await self.speaker.play_sound(next(sounds)) |
|
43
|
|
|
tgt_speed = 30 + i*15 |
|
44
|
|
|
await self.motor.ramp_speed(tgt_speed, 2000) |
|
45
|
|
|
self.message_info(f"Set speed to {i}") |
|
46
|
|
|
await sleep(3) |
|
47
|
|
|
|
|
48
|
|
|
self.message_info("Done") |
|
49
|
|
|
|
|
50
|
|
|
async def system(): |
|
51
|
|
|
hub = Robot('robot') |
|
52
|
|
|
|
|
53
|
|
|
if __name__ == '__main__': |
|
54
|
|
|
logging.basicConfig(level=logging.INFO) |
|
55
|
|
|
start(system) |
|
56
|
|
|
|
virantha /
bricknil
