def log_robot_status():
ping = robosensors.get_sensors()
touch_sensor = robosensors.get_bumper()
log(">> Speed=%d" % robopower.get_power())
log(">> Compass=%3.2f" % robosensors.get_compass())
log(">> Touch=%s, Ping1=%d, Ping2=%d, Ping3=%d" % \
(touch_sensor, ping[0], ping[1], ping[2]))
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)
