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 ))