-
Notifications
You must be signed in to change notification settings - Fork 0
/
robomaster.py
executable file
·223 lines (194 loc) · 7.88 KB
/
robomaster.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
#!/usr/bin/python
# Main control program
import sys,threading,traceback,time
import robocamera,robosensors,robopower,robocontrol,robonavigation,roboconfig
import robocomms
from robocomms import log
from roboutils import *
#-------------------------------------------------------------------
# Robot initialization
#
# Parse the control file and then call the intialize() function
# of each of the robot subsystems. If the subsystems run into problems
# they will throw a RobotError which will be caught here to terminate
# processing.
#
# If everything starts well, return the control data structure to
# the main loop
#
def initialize():
try:
control_data = robocontrol.process_control_file(sys.argv[1])
robocomms.initialize()
robopower.initialize()
robocamera.initialize()
robosensors.initialize()
robonavigation.initialize()
except RobotError as e:
log("RobotException: " + e.value)
log("Exception occurred in initialization - ending")
robopower.halt()
exit(0)
return control_data
#-------------------------------------------------------------------------
# Main processing loop
#
# Loop through each of the control data instructions, calling the
# appropriate function to handle the call
#
def mainloop(control_data):
for command in control_data:
log("Processing command: " + repr(command))
if command['command'] == 'CONE':
navigate_to(command['latitude'], command['longitude'], mode='cone')
elif command['command'] == 'GOTO':
navigate_to(command['latitude'], command['longitude'], mode='goto')
elif command['command'] == 'DRIV':
log("DRIV command not yet implemented")
elif command['command'] == 'STOP':
robopower.stop()
return
elif command['command'] == 'POWR':
robopower.power(command['power'])
elif command['command'] == 'STER':
robopower.steer(command['steering'])
elif command['command'] == 'WAIT':
time.sleep(command['sleeptime'])
else:
# Should never happen: robocontrol should catch, but hey...
log("Error in mainloop: invalid command " + command['command'])
robopower.halt()
raise RobotError
#-------------------------------------------------------------------------
# navigate_to()
#
# Navigate to a given waypoint, optionally finding the cone that is
# there.
#
# Call this function with mode set to 'cone' (the default) or 'goto'.
# If mode == 'goto', the function will terminate when the waypoint is
# reached (within the GPS tolerance. If mode == 'cone' then camera-
# based cone detection will be used to find the cone before the task
# is completed.
#
def navigate_to(latitude, longitude, mode='cone'):
assert (mode == 'cone') or (mode == 'goto')
log("Navigate to lat: " + str(latitude) + ", lon: " + str(longitude))
log("Navigate to mode=" + mode)
task_complete = False
while task_complete == False: #TODO - probably need to time out
# Get camera, compass heading, and GPS data
(camera_X, camera_Y, camera_distance) = robocamera.get_data()
log("Navigate to: camera_X = %03d, camera_Y = %03d, distance = %1.2f" \
% (camera_X, camera_Y, camera_distance))
current_heading = robosensors.get_compass()
log("Navigate to: current heading = %d" % current_heading)
(gps_distance, heading_to_target) = \
robonavigation.vector_to_target(latitude, longitude)
log("Navigate to: target distance = %2.2f, vector = %d" % \
(gps_distance, heading_to_target))
# Test to see if we are at the GPS waypoint (within the radius of
# error). We we are and we're in goto mode, we're done. Otherwise
# switch to looking for the cone with the camera.
if gps_distance < roboconfig.gps_radius:
log("GPS waypoint found")
if mode == 'goto':
log("Goto mode - task complete")
return
else:
# If we can see the cone at this point, go and locate
# it. If we can't, keep going until we can see it.
#
# Note that locate_cone() may fail if the camera loses
# sight of the cone. If that happens, we drop back to
# locating the GPS waypoint again
if camera_distance < roboconfig.max_camera_distance:
if locate_cone() == True:
return
# Outside of GPS range - set the steer angle and drive/turn
# towards the cone
steer_angle = \
robonavigation.get_steer_angle(current_heading, heading_to_target)
log("Navigate to: steer_angle = " + str(steer_angle))
robopower.speed(5)
robopower.steer(steer_angle/180.0)
# Slow things down!
time.sleep(3)
#------------------------------------------------------------------------
# locate_cone()
#
# Called when we are within GPS range of the waypoint and have already
# seen some valid camera data.
#
# First we stop the robot and turn to center the cone. Then we drive
# towards the cone until the touch sensors show we've made contact.
#
# If we lose sight of the cone for whatever reason, we'll try to re-
# sight it. If that fails, we return and go back to GPS waypoint
# finding in the calling routine.
def locate_cone():
# First stop the robot
robopower.speed(0)
# Now drive slowly towards the cone
while True:
(camera_X, camera_Y, camera_distance) = robocamera.get_data()
# If we lose sight of the cone try and find it again
if camera_distance > roboconfig.max_camera_distance:
if re_sight_cone() == False:
return False # failed to re-sight: go back to GPS
else:
continue
# Steer toward the cone
robopower.steer(int(camera_X/32.0-10.0))
robopower.speed(1)
# Check the touch sensor
if robosensors.touch_down():
return True
#--------------------------------------------------------------------------
# re_sight_cone()
#
# Called when the camera loses sight of the cone. Try a number of times
# to find it again.
def re_sight_cone():
log("Re-sighting cone...")
# Try a few times
for i in range(roboconfig.re_sight_attempts):
(camera_X, camera_Y, camera_distance) = robocamera.get_data()
if camera_distance < roboconfig.max_camera_distance:
log("...sucess!")
return True
else:
time.sleep(re_sight_sleep_time)
# Couldn't find it - return False
log("...failed")
return False
#-----------------------------------------------------------------------
# Main
#
# Call initialize to start the robot. Initialize will return the
# control file data structure if all is well. Pass this to the
# mainloop() funtion to carry out each control function.
#
# Note: initialize will terminate with a RobotError exception if
# problems occur: no need to check returned value.
#
def main():
log("RoboMagellan 2012 started")
control_data = initialize()
time.sleep(1) # Allow time for processing threads to start
try:
mainloop(control_data)
except:
log("*** ERROR *** A fatal error has occured *** ERROR ***")
log(str(traceback.format_exc()))
else:
log("RoboMagellan 2012 ended - Champagne Time!")
# Stop the robot
robopower.halt()
# Terminate running threads
for thread in threading.enumerate():
if thread.name != 'MainThread':
log("Terminating thread: " + str(thread))
thread.stop()
if __name__ == '__main__':
main()