def dpad_button_pressed(value, number, _joystickD_padCurrentButton):
if (value == 0) and (number == _joystickD_padCurrentButton):
drive.stop()
if ps3_ControllerMode == 1:
debug.print_to_all_devices("Stop", "SP")
# Up button
else:
if number == 4:
if value: # value is 1 for pressed 0 for released.
dpad_up_button_pressed()
# Right button
elif number == 5:
if value:
dpad_right_button_pressed()
# Down button
elif number == 6:
if value:
dpad_down_button_pressed()
# Left button
elif number == 7:
if value:
dpad_left_button_pressed()
def run(self):
self.running = True
debug.print_to_all_devices("Challenge 'Straight line' Started.")
# self.arduino_sensor_data.start() # starts the new process and runs in the background
self.arduino_sensor_data.start()
time.sleep(0.2)
while self.running:
drive.forward(self.dalek_settings.max_speed)
time.sleep(.1)
# # detects we have finished the challenge.
if self.arduino_sensor_data.frontPing <= 18:
drive.stop()
debug.print_to_all_devices(
"Center Distance:{}cm Run Finished".format(
self.arduino_sensor_data.frontPing))
self.stop_running()
if self.arduino_sensor_data.leftPing <= 5:
debug.print_to_all_devices("turnForwardRight", "TrR")
drive.turnForwardRight(self.dalek_settings.outer_turn_speed,
self.dalek_settings.inner_turn_speed)
time.sleep(.05)
drive.forward(self.dalek_settings.max_speed)
if self.arduino_sensor_data.rightPing <= 5:
debug.print_to_all_devices("turnForwardLeft", "TrL")
drive.turnForwardLeft(self.dalek_settings.inner_turn_speed,
self.dalek_settings.outer_turn_speed)
time.sleep(.05)
drive.forward(self.dalek_settings.max_speed)
def turn_left_90_deg_right_laser():
lz_left = DalekSensors.laser_front_left
lz_center = DalekSensors.front_distance
lz_right = DalekSensors.laser_front_right
print("{} {} {}".format(lz_left, lz_center, lz_right))
while DalekSensors.front_distance < 80:
drive.spinLeft(40)
while (lz_right < 50):
lz_left = DalekSensors.laser_front_left
lz_center = DalekSensors.front_distance
lz_right = DalekSensors.laser_front_right
print("{} {} {}".format(lz_left, lz_center, lz_right))
drive.spinLeft(40)
drive.stop()
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]
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~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
def dpad_button_pressed(value, number, _joystickD_padCurrentButton):
if (value == 0) and (number == _joystickD_padCurrentButton):
drive.stop()
# Up button
else:
if number == 4:
if value: # value is 1 for pressed 0 for released.
dpad_up_button_pressed()
# else:
# dpad_up_button_released()
# Right button
elif number == 5:
if value:
dpad_right_button_pressed()
# else:
# dpad_right_button_released()
# Down button
elif number == 6:
if value:
dpad_down_button_pressed()
# else:
# dpad_down_button_released()
# Left button
elif number == 7:
if value:
dpad_left_button_pressed()
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 getMag():
drive.stop()
time.sleep(DalekTurnSettings['sleepTime'])
currentMag = -1
# ensure we get a valid reading must be between 0 and 360
while not (0 <= currentMag <= 360):
currentMag = spi.get_mag()
return currentMag
def getMag():
drive.stop()
time.sleep(DalekTurnSettings['sleepTime'])
currentMag = -1
# ensure we get a valid reading must be between 0 and 360
while not (0 <= currentMag <= 360):
currentMag = spi.get_mag()
# print("---getStartingMag:{}".format(currentMag))
return currentMag
def drive_forwards_to_distance(distance=distance_to_wall):
# counter =0
print("drive_forwards_to_distance()")
while DalekSensors.front_distance != distance:
# get the speed to drive at
speed = CalculateSpeedToDrive(DalekSensors.front_distance, distance)
if DalekSensors.front_distance <= distance:
drive.backward(speed)
print("drive_backwards()")
else:
drive.paddleForward(speed, speed)
drive.stop()
def stop_running(self):
'''
When this is called it ends this thread
This is also called if the PS3 button is pressed during a challenge,
so add any cleanup code here.
'''
drive.stop()
if self.arduino_sensor_data.is_alive():
self.arduino_sensor_data.stop_running()
self.arduino_sensor_data.join() # wait for process to finish
self.running = False
debug.print_to_all_devices("Done...")
def drive_backwards_to_distance(distance=distance_to_wall):
print("driveBackwards()")
while DalekSensors.rear_distance != distance:
dalekSpeed = CalculateSpeedToDrive(
DalekSensors.rear_distance, distance)
if DalekSensors.rear_distance <= distance:
drive.forward(dalekSpeed)
else:
drive.backward(dalekSpeed)
drive.stop()
def destroy(): # Shutdown GPIO and Cleanup modules
# Allow access to sound volume
global dalek_sounds
debug.print_to_all_devices("\n... Shutting Down...\n", "Ext")
dalek_sounds.play_sound("Grow_stronger")
drive.stop() # Make sure Bot is not moving when program exits
drive.cleanup() # Shutdown all motor control
# time.sleep(2)
# cv2.destroyAllWindows() # Shutdown any open windows
# time.sleep(1.5)
debug.destroy() # Clear Scroll pHat
GPIO.cleanup() # Release GPIO resource
def run(self):
self.running = True
debug.print_to_all_devices("Challenge 'the_duck_shoot' Started.")
while self.running:
drive.forward(self.dalek_settings.max_speed)
time.sleep(1)
####################################################
# #
# Code for this challenge goes in this while loop #
# #
####################################################
debug.print_to_all_devices("BANG!!!") # this line can be removed
drive.stop()
self.stop_running() # this line can be removed
def tank_drive(_leftPaddle, _rightPaddle):
debug.print_to_all_devices("left: {} Right: {}".format(
_leftPaddle, _rightPaddle))
if (_leftPaddle == 0) and (_rightPaddle == 0):
drive.stop()
debug.clear()
elif (_leftPaddle < 0) and (_rightPaddle < 0):
drive.paddleForward(-_leftPaddle, -_rightPaddle)
# debug.print_to_all_devices("forwards","Fw")
elif (_leftPaddle > 0) and (_rightPaddle > 0):
drive.paddleBackward(_leftPaddle, _rightPaddle)
# debug.print_to_all_devices("Backwards", "Bw")
elif (_leftPaddle <= 0) and (_rightPaddle >= 0):
drive.turnForwardRight(-_leftPaddle, _rightPaddle)
# debug.print_to_all_devices("Spin Right", "SR")
elif (_leftPaddle >= 0) and (_rightPaddle <= 0):
drive.turnForwardLeft(_leftPaddle, -_rightPaddle)
def straighten_up():
print("{} {} {}".format(DalekSensors.laser_front_left,
DalekSensors.front_distance, DalekSensors.laser_front_right))
lz_left = DalekSensors.laser_front_left
lz_right = DalekSensors.laser_front_right
while lz_left != lz_right:
if lz_left > lz_right:
drive.spinRight(30)
elif lz_left < lz_right:
drive.spinLeft(30)
time.sleep(.1)
print("{} {} ".format(lz_left, lz_right))
lz_left = DalekSensors.laser_front_left
lz_right = DalekSensors.laser_front_right
print("{} {} ".format(lz_left, lz_right))
drive.stop()
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 ))
def print_compass():
print("L F R Rear {} {} {} {}".format(
compass_positions['left'],
compass_positions['forward'],
compass_positions['right'],
compass_positions['backwards']))
# def turn_left():
# turnspeed = 50
# while DalekSensors.compass > (145+5):
# drive.spinLeft(turnspeed)
# time.sleep(.2)
# print(DalekSensors.compass)
# drive.stop()
# print("Stop")
# time.sleep(1)
# print(DalekSensors.compass)
# DalekSensors.stop_running()
if __name__ == "__main__":
main()
drive.stop()
head_controller.leds_change_color(head_controller.leds_color['off'])
drive.cleanup()
DalekSensors.stop_running()
def DalekTurn(degreesToTurn):
def getMag():
drive.stop()
time.sleep(DalekTurnSettings['sleepTime'])
currentMag = -1
# ensure we get a valid reading must be between 0 and 360
while not (0 <= currentMag <= 360):
currentMag = spi.get_mag()
# print("---getStartingMag:{}".format(currentMag))
return currentMag
def calculateEndHeading(degreesToTurn):
# we don't need more than one rotation.
if degreesToTurn > 360:
degreesToTurn = degreesToTurn % 360
if degreesToTurn < -360:
degreesToTurn = -degreesToTurn
degreesToTurn = degreesToTurn % 360
degreesToTurn = -degreesToTurn
currentHeading = getMag()
finalHeading = (currentHeading + degreesToTurn) % 360
print(
"\n~~~~~~~~~~~~~~~~~\ndegreesToTurn:{} currentMag:{} FinalHeading:{}"
.format(degreesToTurn, currentHeading, finalHeading))
return currentHeading, finalHeading
# I am a robot I want the quickest route.
def calculateTurnDirection(endHeading):
turnClockwise = True
turnSpeed = 60
currentHeading = getMag()
diffBetweenStartAndEnd = 0
botMoveTime = 1
if endHeading < currentHeading:
print("less")
diffBetweenStartAndEnd = -(currentHeading - endHeading)
print(" diffBetweenStartAndEnd:{} \n ".format(
diffBetweenStartAndEnd))
if (-180 <= diffBetweenStartAndEnd <= 180):
print("this way")
if (0 <= diffBetweenStartAndEnd <= 180):
print(">>>Turn Clockwise:{} deg".format(
diffBetweenStartAndEnd))
turnClockwise = True
else:
print(">>>Turn anti Clockwise:{} deg".format(
-diffBetweenStartAndEnd))
turnClockwise = False
else:
print("switch direction")
var2 = 180 - ((currentHeading - endHeading) - 180)
print(" diffBetweenStartAndEnd:{} \n ".format(var2))
else:
print("more")
diffBetweenStartAndEnd = endHeading - currentHeading
print("@@@@@@ diffBetweenStartAndEnd:{} \n ".format(
diffBetweenStartAndEnd))
if diffBetweenStartAndEnd > 180:
ttt = diffBetweenStartAndEnd - 180
print("<<<Turn anti Clock wise:{} deg".format(ttt))
turnClockwise = False
else:
print("<<<Turn Clock wise:{} deg".format(
diffBetweenStartAndEnd))
turnClockwise = True
if diffBetweenStartAndEnd < 0:
diffBetweenStartAndEnd = -diffBetweenStartAndEnd
if diffBetweenStartAndEnd >= 60:
turnSpeed = DalekTurnSettings['TurnSpeedFast']
botMoveTime = DalekTurnSettings['BotMoveTimeFast']
elif 21 <= diffBetweenStartAndEnd <= 59:
turnSpeed = DalekTurnSettings['TurnSpeedNormal']
botMoveTime = DalekTurnSettings['BotMoveTimeNormal']
elif 10 <= diffBetweenStartAndEnd <= 20:
turnSpeed = DalekTurnSettings['TurnSpeedSlow']
botMoveTime = DalekTurnSettings['BotMoveTimeSlow']
else: # diffBetweenStartAndEnd <= 20
turnSpeed = DalekTurnSettings['TurnSpeedFinal']
botMoveTime = DalekTurnSettings['BotMoveTimeFinal']
return turnClockwise, turnSpeed, currentHeading, botMoveTime
##################################################################
# initialise values
# botTurnClockwise= True
# botSpeed = 60
# botCurrentHeading= -1
# botMoveTime = 1
# get end heading
botCurrentHeading, finalHeading = calculateEndHeading(degreesToTurn)
# set current values
botTurnClockwise, botSpeed, botCurrentHeading, botMoveTime = calculateTurnDirection(
finalHeading)
# now do your stuff until you get to where you need to be.
while botCurrentHeading != finalHeading:
print("\n####################\ncurrentMag:{} ShouldBe:{} speed:{}".
format(botCurrentHeading, finalHeading, botSpeed))
if botTurnClockwise:
drive.spinRight(botSpeed)
else:
drive.spinLeft(botSpeed)
time.sleep(botMoveTime)
# drive.stop()
# time.sleep(.3)
botTurnClockwise, botSpeed, botCurrentHeading, botMoveTime = calculateTurnDirection(
finalHeading)
drive.stop()
time.sleep(.3)
botCurrentHeading = getMag()
print(
"\n--------------------\n END \n currentMag:{} ShouldBe:{} speed:{}"
.format(botCurrentHeading, finalHeading, botSpeed))
