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 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 dpad_left_button_pressed():
if ps3_ControllerMode == 1:
debug.print_to_all_devices('Spin Left', "SL")
drive.spinLeft(dalek_settings.speed)
elif ps3_ControllerMode == 2:
debug.print_to_all_devices('Spin Left', "SL")
drive.spinLeft(dalek_settings.speed)
else:
head_controller.head_move_left()
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 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 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 ))
# GPIO.setup(pinMotorBRSpeed, GPIO.OUT)
# GPIO.setup(pinMotorBRForwards, GPIO.OUT)
# GPIO.setup(pinMotorBRBackwards, GPIO.OUT)
# pwmMotorBRSpeed = GPIO.PWM(pinMotorBRSpeed, Frequency)
# pwmMotorBRSpeed.start(Stop)
#######################
### main
#######################
while True:
# toggle the Back right motor
# GPIO.output(pin_motor_relay, GPIO.HIGH)
# time.sleep(1)
# print("on")
# GPIO.output(pin_motor_relay, GPIO.LOW)
# time.sleep(1)
# print("off")
# pwmMotorBRSpeed.ChangeDutyCycle(Speed)
# GPIO.output(pinMotorBRForwards, GPIO.HIGH)
# GPIO.output(pinMotorBRBackwards, GPIO.LOW)
drive.forward(30)
time.sleep(2)
drive.backward(30)
time.sleep(2)
drive.spinLeft(50)
time.sleep(2)
drive.spinRight(50)
time.sleep(2)
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))