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]