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
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
|
import time
import board
from analogio import AnalogIn
from adafruit_motorkit import MotorKit
kit = MotorKit()
analog_in = AnalogIn(board.A1)
commandList = (180, 90, 0, 0, 270, 0,
180, 90, 180, 180, 270,
180, 90, 270, 0, 90, 90, 90,
0, 180, 270, 180, 90, 270, 0,
270, 270, 180, 90)
DIRECTIONS = {0: 'Up',
90: 'Right',
180: 'Down',
270: 'Left'}
robotDirection = 90
def get_voltage(pin):
return (pin.value * 3.3) / 65536
def turn(direction, amount):
turnDuration = 0.6
turnSpeed = 0.7
if amount == 2:
turnDuration = turnDuration * 2
if direction == 'counterClockwise':
turnSpeed = -(turnSpeed)
kit.motor3.throttle = -turnSpeed
kit.motor4.throttle = -turnSpeed
time.sleep(turnDuration)
kit.motor3.throttle = 0.0
kit.motor4.throttle = -0.0
time.sleep(1.0)
def move():
kit.motor3.throttle = -0.55
kit.motor4.throttle = 0.85
time.sleep(0.9)
kit.motor3.throttle = 0.0
kit.motor4.throttle = 0.0
time.sleep(1.0)
def centerAtStart():
kit.motor3.throttle = 0.6
kit.motor4.throttle = -0.6
time.sleep(0.45)
kit.motor3.throttle = 0.0
kit.motor4.throttle = 0.0
time.sleep(1.0)
def runInstructions():
global robotDirection
v = 0
while (v < 1.3):
v = get_voltage(analog_in)
print(v)
time.sleep(0.05)
print("Exited while loop.")
for i in range(len(commandList)):
turnIncrement = 0
if commandList[i] == robotDirection:
move()
# print("Move Foward (Facing %s)" % (DIRECTIONS[robotDirection]))
else:
while robotDirection != commandList[i]:
robotDirection += 90
turnIncrement += 1
if robotDirection == 360:
robotDirection = 0
if turnIncrement == 0:
# print("Move.")
move()
elif turnIncrement == 1:
# print("Turn 90° clockwise to face %s." % (DIRECTIONS[robotDirection]))
turn('clockwise', 1) # 1 represents 90 degrees
# print("Move.")
move()
elif turnIncrement == 2:
# print("Turn 180° clockwise to face %s." % (DIRECTIONS[robotDirection]))
turn('clockwise', 2)
# print("Move.")
move()
elif turnIncrement == 3:
# print("Turn 90° counterclockwise to face %s." % (DIRECTIONS[robotDirection]))
turn('counterClockwise', 1)
# print("Move.")
move()
# centerAtStart()
# turn('clockwise', 1)
# print("Hello World")
# runInstructions()
# move()
|
