def getLocation(self):
gpsData = db_q.get_latest(GPSTable)
"""
while self.gps.latitude is None:
print 'Not valid gps fix, retying...'
self.gps.update()
# TESTING REMOVE
for i in range(0, 10):
self.gps.update()
if __debug__:
self.gps.latitude = 55.912658
self.gps.longitude = -3.321353
# -----------------------------
count = 0
if count is not 10:
if self.gps.latitude is "" or self.gps.longitude is "":
self.gps.update()
else:
"No gps values gained"
return None
"""
return [gpsData.latitude, gpsData.longitude]
from tiberius.database_wrapper.polyhedra_database import PolyhedraDatabase
from tiberius.database.query import get_latest
from tiberius.database.tables import GPSTable
pd = PolyhedraDatabase("test_gps_fetcher")
print get_latest(pd, GPSTable)
def turnTo(self, desired_bearing):
"""Turn the robot until it is facing desired_bearing.
Args:
desired_bearing (float): The bearing that the robot should be facing
upon completion of the function.
"""
# Ensure we have sufficient priviledges to access compass.
if not TiberiusConfigParser.isCompassEnabled():
raise SensorNotEnabledError("Compass is disabled, dependant \
function cannot be executed.")
count = 0
while(True):
count += 1
print 'Iteration: ' + str(count)
if count < 50:
time.sleep(0.1)
# actual_bearing = self.compass.headingNormalized()
actual_bearing = db_q.get_latest(CompassTable)
error = actual_bearing - desired_bearing
# self.logger.debug('Heading: ' + str(actual_bearing))
# self.logger.debug('Desired: ' + str(desired_bearing))
if(error < 5 and error > -5):
# self.logger.debug('At heading: ' + str(actual_bearing))
self.motors.stop()
break
if(error > 180):
#print 'error > 180'
error -= 360
if(error < -180):
#print 'error < -180'
error += 360
if(error > 0):
# print 'error < 0 turning left'
self.motors.setSpeedPercent(100)
self.motors.turnLeft()
# Reduce speed on approach to desired bearing
# Positive error is a left turn
if(error < 60):
self.motors.setSpeedPercent(70)
self.motors.turnLeft()
if(error < 30):
self.motors.setSpeedPercent(40)
self.motors.turnLeft()
if(error < 5):
self.motors.setSpeedPercent(20)
self.motors.turnLeft()
if(error < 0):
# print 'error > 0 turning right'
self.motors.setSpeedPercent(100)
self.motors.turnRight()
# Negative error is a right turn
if(error > -60):
self.motors.setSpeedPercent(70)
self.motors.turnRight()
if(error > -30):
self.motors.setSpeedPercent(40)
self.motors.turnRight()
if(error > -5):
self.motors.setSpeedPercent(20)
self.motors.turnRight()
# print str(error)
else:
print '50 Iterations Complete'
break
def getCurHeading(self):
compass_dict = db_q.get_latest(CompassTable)
heading = compass_dict.heading - 90
return bearing_math.normalize_bearing(heading)
def compass_monitor(poly):
import tiberius.database.query as db_q
from tiberius.database.tables import CompassTable
dicti = db_q.get_latest(poly, CompassTable)
if dicti is not None:
bearing = dicti[0].heading
print "Bearing: " + str(bearing)
if 7.5 > bearing > -7.5:
return [1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -7.5 > bearing > -22.5:
return [9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -22.5 > bearing > -37.5:
return [9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -37.5 > bearing > -52.5:
return [9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -52.5 > bearing > -67.5:
return [9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -67.5 > bearing > -82.5:
return [9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -82.5 > bearing > -97.5:
return [9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -97.5 > bearing > -112.5:
return [9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -112.5 > bearing > -127.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -127.5 > bearing > -142.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -142.5 > bearing > -157.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif -157.5 > bearing > -172.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif 22.5 > bearing > 7.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif 37.5 > bearing > 22.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif 52.5 > bearing > 37.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9]
elif 67.5 > bearing > 52.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9, 9]
elif 82.5 > bearing > 67.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9, 9]
elif 97.5 > bearing > 82.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9, 9]
elif 112.5 > bearing > 97.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9, 9]
elif 127.5 > bearing > 112.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9, 9]
elif 142.5 > bearing > 127.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9, 9]
elif 157.5 > bearing > 142.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9, 9]
elif 172.5 > bearing > 157.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1, 9]
elif 172.5 < bearing < -172.5:
return [9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 1]
else:
print "No Compass Data"
return [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]