def compass_thread(self):
compass = Compass()
compass_read_id = 0
compass_update_id = 0
valid = False
previous_values = []
COMPASS_NUMBER_OF_READINGS = 10
while True:
try:
# get the compass heading
heading = compass.headingNormalized()
# TODO: Move validity checks to it's own module.
# maintain a short array of previous values
#previous_values.append(heading)
#if len(previous_values) >= 10:
# previous_values.pop(0)
# ensure the compass heading has not changed too much between readings
#standard_deviation = np.std(np.diff(np.asarray(previous_values)))
#if standard_deviation > 10:
# raise Exception('invalid data')
#else:
if compass_read_id < COMPASS_NUMBER_OF_READINGS: # store the first 10 results
ins.insert_compass_reading(self.poly, compass_read_id, heading)
else: # start updating results
compass_update_id = compass_read_id % COMPASS_NUMBER_OF_READINGS
up.overwrite_compass_reading(self.poly, compass_update_id, heading)
compass_read_id += 1
#compass_monitor(heading, control)
# TODO: Again, something weird is going on here!
# If not valid then valid = True?!
#if not valid:
# valid = True
up.update_compass_sensor_validity(self.poly, True)
except Exception as e:
print e
#if valid:
#valid = False
up.update_compass_sensor_validity(self.poly, False)
time.sleep(0.5)
import sys
from tiberius.control.sensors import Ultrasonic
from tiberius.control.sensors import Compass
import time
if __name__ == "__main__":
while(True):
u = Ultrasonic()
c = Compass()
vals = u.senseUltrasonic()
heading = c.headingNormalized()
print str(vals)
print str(heading)
time.sleep(1)
