/
teacher.py
228 lines (195 loc) · 8.29 KB
/
teacher.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
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
# PYTHON3 ONLY
# Based on:
# https://www.dexterindustries.com/GoPiGo/
# https://github.com/DexterInd/GoPiGo3
# Copyright (c) 2017 Dexter Industries
# Released under the MIT license (http://choosealicense.com/licenses/mit/).
# For more information see https://github.com/DexterInd/GoPiGo3/blob/master/LICENSE.md
# Distance sensor and IMU both plugged into I2C
import gopigo3, sys, time
from di_sensors.easy_distance_sensor import EasyDistanceSensor
from di_sensors import inertial_measurement_unit
class PiggyParent(gopigo3.GoPiGo3):
'''
UTILITIES
'''
def __init__(self, addr=8, detect=True):
gopigo3.GoPiGo3.__init__(self)
self.MIDPOINT = 1500
self.scan_data = {}
# mutex sensors on IC2
self.distance_sensor = EasyDistanceSensor(port="RPI_1", use_mutex=True)
self.imu = inertial_measurement_unit.InertialMeasurementUnit(bus="RPI_1") # mutex crashes this
# buffer for reading the gyro
self.gyro_buffer = 0
self.stop()
def calibrate(self):
"""allows user to experiment on finding centered midpoint and even motor speeds"""
print("Calibrating...")
self.servo(self.MIDPOINT)
response = str.lower(input("Am I looking straight ahead? (y/n): "))
if response == 'n':
while True:
response = str.lower(input("Turn right, left, or am I done? (r/l/d): "))
if response == "r":
self.MIDPOINT -= 25
print("Midpoint: " + str(self.MIDPOINT))
self.servo(self.MIDPOINT)
elif response == "l":
self.MIDPOINT += 25
print("Midpoint: " + str(self.MIDPOINT))
self.servo(self.MIDPOINT)
else:
print("Midpoint temporarily saved to: " + str(self.MIDPOINT) + "\nYou'll need to update your magic number.")
break
else:
print('Cool, %d is the correct self.MIDPOINT' % self.MIDPOINT)
response = str.lower(input("Do you want to check if I'm driving straight? (y/n)"))
if 'y' in response:
while True:
self.set_motor_limits(self.MOTOR_LEFT, self.LEFT_DEFAULT)
self.set_motor_limits(self.MOTOR_RIGHT, self.RIGHT_DEFAULT)
print("LEFT: {} // RIGHT: {} ".format(self.MOTOR_LEFT, self.MOTOR_RIGHT))
self.fwd()
time.sleep(1)
self.stop()
response = str.lower(input("Reduce left, reduce right or drive? (l/r/d): "))
if response == 'l':
self.LEFT_SPEED -= 5
elif response == 'r':
self.RIGHT_SPEED -= 5
elif response == 'd':
self.fwd()
time.sleep(1)
self.stop()
else:
break
def quit(self):
"""Terminates robot movement and settings then closes app"""
print("\nIt's been a pleasure.\n")
self.reset_all()
sys.exit(1)
'''
MOVEMENT
'''
def deg_fwd(self, deg):
"""Zeroes current encorder values then moves forward based on degrees given"""
self.offset_motor_encoder(self.MOTOR_LEFT, self.get_motor_encoder(self.MOTOR_LEFT))
self.offset_motor_encoder(self.MOTOR_RIGHT, self.get_motor_encoder(self.MOTOR_RIGHT))
self.set_motor_position(self.MOTOR_LEFT + self.MOTOR_RIGHT, deg)
def turn_by_deg(self, deg):
"""Rotates robot relative to it's current heading. If told -20, it
will rotate left by 20 degrees."""
# get our current angle
current = self.get_heading()
# calculate delta
goal = current + deg
# LOOP AROUND THE 360 marker
goal %= 360
# call turn to deg on the delta
self.turn_to_deg(goal)
def turn_to_deg(self, deg):
"""Turns to a degree relative to the gyroscope's readings. If told 20, it
will rotate until the gyroscope reads 20."""
# error check
goal = abs(deg) % 360
current = self.get_heading()
turn = self.right # connect it to the method without the () to activate
if (current - goal > 0 and current - goal < 180) or \
(current - goal < 0 and (360 - goal) + current < 180):
turn = self.left
# while loop - keep turning until my gyro says I'm there
while abs(deg - self.get_heading()) > 4:
turn(primary=70, counter=-70)
#time.sleep(.05) # avoid spamming the gyro
# once out of the loop, hit the brakes
self.stop()
# report out to the user
print("\n{} is close enough to {}.\n".format(self.get_heading(), deg))
def fwd(self, left=50, right=50):
"""Blindly charges your robot forward at default power which needs to be configured in child class"""
if self.LEFT_DEFAULT and left == 50:
left = self.LEFT_DEFAULT
if self.RIGHT_DEFAULT and right == 50:
right = self.RIGHT_DEFAULT
self.set_motor_power(self.MOTOR_LEFT, left)
self.set_motor_power(self.MOTOR_RIGHT, right)
def right(self, primary=90, counter=0):
"""Rotates right by powering the left motor to default"""
self.set_motor_power(self.MOTOR_LEFT, primary)
self.set_motor_power(self.MOTOR_RIGHT, counter)
def left(self, primary=90, counter=0):
"""Rotates left by powering the left motor to 90 by default and counter motion 0"""
self.set_motor_power(self.MOTOR_LEFT, counter)
self.set_motor_power(self.MOTOR_RIGHT, primary)
def back(self, left=-50, right=-50):
if self.LEFT_DEFAULT and left == -50:
left = -self.LEFT_DEFAULT
if self.RIGHT_DEFAULT and right == -50:
right = -self.RIGHT_DEFAULT
self.set_motor_power(self.MOTOR_LEFT, left)
self.set_motor_power(self.MOTOR_RIGHT, right)
def servo(self, angle):
"""Moves the servo to the given angle"""
print("Servo moving to: {} ".format(angle))
self.set_servo(self.SERVO_1, angle)
time.sleep(.05)
def stop(self):
"""Cut power to the motors"""
print("\n--STOPPING--\n")
self.set_motor_power(self.MOTOR_LEFT + self.MOTOR_RIGHT, 0)
'''
SENSORS
'''
def read_distance(self):
"""Returns the GoPiGo3's distance sensor in MM over IC2"""
d = self.distance_sensor.read_mm()
print("Distance Sensor Reading: {} mm ".format(d))
return d
def get_heading(self):
"""Returns the heading from the IMU sensor, or if there's a sensor exception, it returns
the last saved reading"""
try:
self.gyro_buffer = self.imu.read_euler()[0]
print("Gyroscope sensor is at: {} degrees ".format(self.gyro_buffer))
except Exception as e:
print("----- PREVENTED GYRO SENSOR CRASH -----")
print(e)
return self.gyro_buffer
'''
SHOWOFF SCRIPTS
'''
def shy(self):
"""Responds to a close reading on the distance sensor by backing up"""
while True:
for ang in range(self.MIDPOINT-400, self.MIDPOINT+401, 100):
self.servo(ang)
time.sleep(.1)
if self.read_distance() < 250:
self.back()
time.sleep(.3)
self.stop()
for x in range(3):
self.servo(self.MIDPOINT + 300)
time.sleep(.15)
self.servo(self.MIDPOINT - 300)
time.sleep(.15)
self.servo(self.MIDPOINT)
self.fwd()
time.sleep(.2)
self.stop()
def follow(self):
"""Responds to a close reading on the distance sensor by attempting to maintain heading"""
while True:
for ang in range(self.MIDPOINT-400, self.MIDPOINT+401, 100):
self.servo(ang)
time.sleep(.1)
if self.read_distance() < 250:
self.look_excited()
def look_excited(self):
for x in range(2):
self.turn_by_deg(30)
time.sleep(.25)
self.turn_by_deg(-30)
time.sleep(.25)
self.stop()