Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
import asyncio
import random
import user.lib.controller as controller
from fischertechnik.controller.Motor import Motor
from fischertechnik.logging import log as print
speed = None
directions = None
async def backward(speed):
global directions
controller.RX_M1_motor.set_speed(int(speed), Motor.CW)
controller.RX_M1_motor.start()
controller.RX_M2_motor.set_speed(int(speed), Motor.CW)
controller.RX_M2_motor.start()
async def forward(speed):
global directions
controller.RX_M1_motor.set_speed(int(speed), Motor.CCW)
controller.RX_M1_motor.start()
controller.RX_M2_motor.set_speed(int(speed), Motor.CCW)
controller.RX_M2_motor.start()
async def turn_left():
global speed, directions
controller.RX_M1_motor.set_speed(int(250), Motor.CW)
controller.RX_M1_motor.start()
controller.RX_M2_motor.set_speed(int(250), Motor.CCW)
controller.RX_M2_motor.start()
async def turn_right():
global speed, directions
controller.RX_M1_motor.set_speed(int(250), Motor.CCW)
controller.RX_M1_motor.start()
controller.RX_M2_motor.set_speed(int(250), Motor.CW)
controller.RX_M2_motor.start()
async def stop():
global speed, directions
controller.RX_M1_motor.stop()
controller.RX_M2_motor.stop()
async def follow_line():
global speed, directions
while ((controller.RX_I7_trail_follower.get_state() == 0) or (controller.RX_I8_trail_follower.get_state() == 0)) and (controller.RX_I2C_1_gesture_sensor.get_proximity()) < 240:
if controller.RX_I8_trail_follower.get_state() == 1:
await turn_left()
await asyncio.sleep(0.05)
if controller.RX_I7_trail_follower.get_state() == 1:
await turn_right()
await asyncio.sleep(0.05)
if (controller.RX_I7_trail_follower.get_state() == 0) and (controller.RX_I8_trail_follower.get_state() == 0):
await forward(250)
await asyncio.sleep(0.01)
if (controller.RX_I2C_1_gesture_sensor.get_proximity()) >= 240:
await backward(250)
await asyncio.sleep(1)
if 1 == random.randint(1, 2):
directions = 'Right'
await turn_right()
else:
directions = 'Left'
await turn_left()
await asyncio.sleep(0.5)
async def find_line(directions):
global speed
while (controller.RX_I7_trail_follower.get_state() == 1) and (controller.RX_I8_trail_follower.get_state() == 1):
if directions == 'Right':
await turn_right()
else:
await turn_left()
await asyncio.sleep(0.01)
async def run(instance):
global speed, directions
await controller.init(instance)
controller.RX_I2C_1_gesture_sensor.enable_proximity()
directions = 'Left'
while True:
await find_line(directions)
await follow_line()
await asyncio.sleep(0.01)