-
Notifications
You must be signed in to change notification settings - Fork 0
/
functions.py
146 lines (121 loc) · 4.52 KB
/
functions.py
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
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
from setup import *
import math
def DRIVE_STOP():
robot.stop()
robot.stop()
left_drive_motor.brake()
right_drive_motor.brake()
wait(100)
left_drive_motor.stop()
right_drive_motor.stop()
# -----------------------------------------------------------------------------------
def GYRO_HOLD(time):
# get starting angle and starting time (milis)
straight_angle = gyro_sensor.angle()
starting_time = stopwatch.time()
# get ending time
ending_time = starting_time + time
sensitivity = 4
# loop until ending time is reached
while stopwatch.time() <= ending_time:
# drive forward, correcting angle. multiply gyro_sensor.angle for sensitivity
current_angle = gyro_sensor.angle()
error = current_angle - straight_angle
drive_angle = -error * sensitivity # multiply error by sensitivity
robot.drive(0, -drive_angle)
robot.stop()
# -----------------------------------------------------------------------------------
# time is in miliseconds
def GYRO_STRAIGHT(speed, time):
starting_time = stopwatch.time()
robot.reset()
print("straight")
# get starting angle
straight_angle = gyro_sensor.angle()
print(straight_angle)
ending_time = starting_time + time
sensitivity = 5 # adjust this value to change the sensitivity
# loop until distance is achieved
while -robot.distance() <= ending_time:
# drive forward, correcting angle
current_angle = gyro_sensor.angle()
error = current_angle - straight_angle
drive_angle = -error * sensitivity # multiply error by sensitivity
robot.drive(-speed, -drive_angle)
print(drive_angle)
robot.stop()
# -----------------------------------------------------------------------------------
# time is in miliseconds
def GYRO_STRAIGHT_DISTANCE(speed, distance):
robot.reset()
wait(50)
print(robot.distance())
print("straight")
# get starting angle
straight_angle = gyro_sensor.angle()
print(straight_angle)
starting_distance = robot.distance()
desired_distance = starting_distance + distance
sensitivity = 8 # adjust this value to change the sensitivity
# loop until distance is achieved
while robot.distance() >= -desired_distance:
# drive forward, correcting angle
current_angle = gyro_sensor.angle()
error = current_angle - straight_angle
drive_angle = -error * sensitivity # multiply error by sensitivity
robot.drive(-speed, -drive_angle)
print(robot.distance())
robot.stop()
# -----------------------------------------------------------------------------------
def GYRO_LEFT(angle):
# get desired turn angle derived from base gyro angle
starting_angle = gyro_sensor.angle()
desired_angle = starting_angle + angle
# coarse turn
while gyro_sensor.angle() <= desired_angle:
robot.drive(0, -120)
# fine, slower turn for correction
while gyro_sensor.angle() >= desired_angle:
robot.drive(0, 20)
# -----------------------------------------------------------------------------------
def GYRO_RIGHT(angle):
# get desired turn angle derived from base gyro angle
starting_angle = gyro_sensor.angle()
desired_angle = starting_angle - angle
# coarse turn
while gyro_sensor.angle() >= desired_angle:
robot.drive(0, 120)
print(gyro_sensor.angle())
# fine, slower turn for correction
while gyro_sensor.angle() <= desired_angle:
robot.drive(0, -20)
print(gyro_sensor.angle())
# stop
robot.stop()
# -----------------------------------------------------------------------------------
def PLAY_NOTES(event):
if event == "confirm":
ev3.speaker.beep(frequency=1000, duration=50)
wait(20)
ev3.speaker.beep(frequency=800, duration=50)
wait(20)
ev3.speaker.beep(frequency=1000, duration=50)
wait(20)
ev3.speaker.beep(frequency=1200, duration=50)
elif event == "cancel":
ev3.speaker.beep(frequency=400, duration=50)
wait(20)
ev3.speaker.beep(frequency=600, duration=50)
wait(20)
ev3.speaker.beep(frequency=400, duration=50)
wait(20)
ev3.speaker.beep(frequency=200, duration=50)
elif event == "warning":
ev3.speaker.beep(frequency=400, duration=50)
wait(20)
ev3.speaker.beep(frequency=400, duration=50)
wait(20)
ev3.speaker.beep(frequency=400, duration=50)
wait(20)
else:
wait(20)