# Connect sensor to sensor port 1
imu = Sensor(INPUT_2)
# allow for some time to load the new drivers
time.sleep(0.5)
print("#######################################################################")
print("############### starting compass calibration ######################")
print("#######################################################################")
print("")
print(" Sensor PORT 2 ")
print("")
imu.mode = 'COMPASS'
time.sleep(.5)
imu.command = 'BEGIN-COMP-CAL' # send the command to begin compass calibration
print ("Hit x to end calibration and exit...")
while (doExit == False):
x = ord(sys.stdin.read(1))
if x == 120: # x key pushed
print("x key pushed - command accepted")
imu.command = 'END-COMP-CAL' # send the end cal command
time.sleep(5)
print("calibration cycle complete. Exiting.")
break
time.sleep(.2)
plt.title('Odometry data')
plt.legend()
plt.show()
sample_mode = -1
print("Sampling complete. Returning to keyboard cmd mode.")
if htCmpCal_mode > 0:
try:
# Make sure motors are initially stopped
mL.on(0, brake=True)
mR.on(0, brake=True)
# start compass cal
cmp.command = 'BEGIN-CAL'
time.sleep(0.5)
# start 360 cw robot rotation
mL.on_for_degrees(speed=3, degrees= 1700)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
mL.on(0, brake=False)
mR.on(0, brake=False)
cmp.command = 'END-CAL'
htCmpCal_mode = -1
# Make sure motors are both stopped
mL.on(0, brake=True)
mR.on(0, brake=True)
# Connect sensor to sensor port ?
mi = Sensor(INPUT_2)
# mi = Sensor('spi0.1:S2:i2c1') # unclear if this change helped in any way or not
# mi = I2cSensor('spi0.1:S2:i2c1') # unclear if this change helped in any way or not
# mi = I2cSensor(INPUT_2) # unclear if this change helped in any way or not
print("wait 5")
# allow for some time to stabilze
time.sleep(3)
# mi.poll_ms = 0 # didn't work
# set the mi sensor mode
mi.command = 'RESET'
print("wait 6")
# allow for some time to stabilze
time.sleep(3)
# set the mi sensor mode
mi.mode = 'ANGLE'
# mi.mode = 'ALL'
print("wait 7")
print("")
print("")
# allow for some time to stabilze
