def calibrate_compass():
head_controller.leds_change_color(head_controller.leds_color['yellow'])
# print("point your bot in the forward position.")
head_controller.head_move_to_center(0)
time.sleep(.4)
compass_positions['forward'] = DalekSensors.compass
if compass_positions['left'] == -1:
head_controller.head_move_left_90deg(0)
time.sleep(1)
compass_positions['left'] = DalekSensors.compass
if compass_positions['right'] == -1:
head_controller.head_move_right_90deg(0)
time.sleep(1)
compass_positions['right'] = DalekSensors.compass
# forward, left and right done
head_controller.head_move_to_center(0)
time.sleep(.3)
head_controller.leds_change_color(head_controller.leds_color['green'])
print("left mag:{} center mag:{} right mag:{} backwards:{}" .format(
compass_positions['left'],
compass_positions['forward'],
compass_positions['right'],
compass_positions['backwards']))
def main():
# # changeDalekTurnSettings(1)
# head_controller.leds_change_color(head_controller.leds_color['red'])
head_controller.head_move_to_center()
fw_mag = spi.get_mag()
print("center mag:{}" .format(fw_mag))
time.sleep(1)
# # print("3")
# head_controller.head_move_left_90deg()
# time.sleep(2)
# left_mag = spi.get_mag()
# print("left mag:{}" .format(left_mag))
# time.sleep(.5)
# # head_controller.head_move_right_90deg()
# # time.sleep(2)
# # right_mag = spi.get_mag()
# # print("left mag:{}" .format(right_mag))
# head_controller.head_move_to_center()
# time.sleep(1)
# # head_controller.leds_change_color(head_controller.leds_color['white'])
# # DalekTurn(176)
# # DalekTurn(354)
# turnspeed = 60
# print("mag:{}" .format(spi.get_mag()))
# drive.spinLeft(turnspeed)
# print(turnspeed)
# time.sleep(1)
# drive.spinLeft(turnspeed-20)
# print(turnspeed-20)
# time.sleep(1)
# print("mag:{}" .format(spi.get_mag()))
Compass = spi.SensorData()
# Compass = spi.CompassData()
Compass.start()
turnspeed = 50
while Compass.compass > (145+5):
# while Compass.data > (145+5):
drive.spinLeft(turnspeed)
# print(turnspeed)
time.sleep(.2)
# time.sleep(5) dri
# drive.spinLeft(turnspeed-10)
# # print(turnspeed)
# time.sleep(1)
# drive.spinLeft(turnspeed-20)
print(Compass.compass)
# time.sleep(1)
# time.sleep(5) dri
drive.stop()
print("Stop")
time.sleep(1)
print(Compass.compass)
Compass.stop_running()
def get_missing_compass_positions():
head_controller.head_move_to_center(0)
time.sleep(.4)
if compass_positions['forward'] == -1:
compass_positions['forward'] = DalekSensors.compass
if compass_positions['right'] == -1:
head_controller.head_move_right_90deg(0)
time.sleep(.8)
compass_positions['right'] = DalekSensors.compass
if compass_positions['left'] == -1:
head_controller.head_move_left_90deg(0)
time.sleep(.8)
compass_positions['left'] = DalekSensors.compass
time.sleep(.2)
head_controller.head_move_to_center(0)
def turn_left_90_deg():
if compass_positions['left'] ==-1:
get_missing_compass_positions()
c_f = compass_positions['forward']
c_l = compass_positions['left']
head_controller.head_move_to_center(0)
print("{} {}" .format(c_f, c_l))
# deg_to_turn = v2_calculate_degrees_still_to_turn(c_f, c_l, False)
# print("deg_to_turn:{}".format(deg_to_turn))
def move(_turnspeed):
turnspeed = _turnspeed
deg_to_turn = v2_calculate_degrees_still_to_turn(c_f, c_l, False)
while -1 < deg_to_turn > 1:
if deg_to_turn in range(121,300):
print("L_1")
time.sleep(.4)
drive.spinLeft(turnspeed+10)
elif deg_to_turn in range(40,120):
print("L_2")
drive.spinLeft(turnspeed )
elif deg_to_turn in range(0,39):
print("L_3")
drive.spinLeft(turnspeed -10)
elif deg_to_turn in range(-39,-1):
print("L_4")
drive.spinRight(turnspeed -10)
elif deg_to_turn in range(-40,-100):
print("L_5")
drive.spinRight(turnspeed )
else:
drive.stop()
deg_to_turn = v2_calculate_degrees_still_to_turn(c_f, c_l, False)
drive.stop()
print("\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
move(50)
time.sleep(.4)
move(40)
time.sleep(.4)
move(40)
new_position =[compass_positions['left'],compass_positions['forward'],compass_positions['right'],compass_positions['backwards']]
compass_positions['left'] = new_position[3]
compass_positions['forward'] = new_position[0]
compass_positions['right'] = new_position[1]
compass_positions['backwards'] = new_position[2]
# ALL done
# just debug
time.sleep(1)
_compass =DalekSensors.compass
print("left mag:{} center mag:{} right mag:{} backwards:{} ---- final {} diff:{}" .format(
compass_positions['left'],
compass_positions['forward'],
compass_positions['right'],
compass_positions['backwards'],
_compass, _compass - compass_positions['left']
))
def turn_right_90_deg():
turnspeed = 50
# this will be hit if you have not set the backwards before
# the start method should have set the left right and center values
# but once set it will jump over it.
if compass_positions['right'] == -1:
get_missing_compass_positions()
c_f = compass_positions['forward']
c_r = compass_positions['right']
print("left mag:{} center mag:{} right mag:{} backwards:{}" .format(
compass_positions['left'],
compass_positions['forward'],
compass_positions['right'],
compass_positions['backwards']))
head_controller.head_move_to_center(0)
print("{} {}" .format(c_f, c_r))
# get your data
deg_to_turn = v2_calculate_degrees_still_to_turn(c_f, c_r, True)
print("deg_to_turn:{}".format(deg_to_turn))
# use the between syntax
while -1 < deg_to_turn > 1:
deg_to_turn = v2_calculate_degrees_still_to_turn(c_f, c_r, True)
if deg_to_turn in range(121,300):
print("x1")
drive.spinRight(turnspeed+10)
elif deg_to_turn in range(40,120):
print("x2")
drive.spinRight(turnspeed )
elif deg_to_turn in range(0,39):
print("x2")
drive.spinRight(turnspeed -10)
elif deg_to_turn in range(-39,-1):
print("x3")
drive.spinLeft(turnspeed -10)
elif deg_to_turn in range(-40,-100):
print("x4")
drive.spinLeft(turnspeed )
else:
drive.stop()
print("'''''''''''''''''''''''''x1'''''''''''''''''''''''''")
turnspeed = 40
drive.stop()
time.sleep(.5)
deg_to_turn = v2_calculate_degrees_still_to_turn(c_f, c_r, True)
while -100 < deg_to_turn > 1:
deg_to_turn = v2_calculate_degrees_still_to_turn(c_f, c_r, True)
if deg_to_turn in range(40,300):
print("x11")
drive.spinRight(turnspeed)
elif deg_to_turn in range(0,39):
print("x12")
drive.spinRight(turnspeed -10)
elif deg_to_turn in range(-39,-1):
print("x13")
drive.spinLeft(turnspeed -10)
elif deg_to_turn in range(-40,-100):
print("x14")
drive.spinLeft(turnspeed )
else:
drive.stop()
print("'''''''''''''''''''''''''x2'''''''''''''''''''''''''")
turnspeed = 40
drive.stop()
time.sleep(.5)
deg_to_turn = v2_calculate_degrees_still_to_turn(c_f, c_r, True)
while -100 < deg_to_turn > 1:
deg_to_turn = v2_calculate_degrees_still_to_turn(c_f, c_r, True)
if deg_to_turn in range(40,300):
print("x11")
drive.spinRight(turnspeed)
elif deg_to_turn in range(0,39):
print("x12")
drive.spinRight(turnspeed -10)
elif deg_to_turn in range(-39,-1):
print("x13")
drive.spinLeft(turnspeed -10)
elif deg_to_turn in range(-40,-100):
print("x14")
drive.spinLeft(turnspeed )
else:
drive.stop()
drive.stop()
# swap compass positions
new_position =[compass_positions['left'],compass_positions['forward'],compass_positions['right'],compass_positions['backwards']]
compass_positions['left'] = new_position[1]
compass_positions['forward'] = new_position[2]
compass_positions['right'] = new_position[3]
compass_positions['backwards'] = new_position[0]
print("END {} {} {}" .format(c_f, c_r, DalekSensors.compass))
print("left mag:{} center mag:{} right mag:{} backwards:{}" .format(
compass_positions['forward'],
compass_positions['left'],
compass_positions['right'],
compass_positions['backwards']))
time.sleep(1)
final_val = DalekSensors.compass
print("final {} {}" .format( final_val, compass_positions['right'] -final_val ))
