def turnClockwise(currentHeading, endHeading, timerTime, pass360Point, _speed):
magOffset = 3000
print("---- turnClockwise\n speed{}".format(_speed))
minval = currentHeading
while currentHeading <= endHeading:
time.sleep(timerTime) # pause for sensor to settel
checkforPass360 = getMag(True, currentHeading)
DalekV2DriveV2.spinRight(_speed)
print(" checkforPass360: {}".format(checkforPass360 - magOffset))
# take a little off it to account for reading error.
# 3 < (350 -100) or 3 < 250 is true
# 350 < 250 is false, not passed zero point
# you wont move more than 100 dec in 0.3 seconds
if checkforPass360 < (currentHeading - 100):
checkforPass360 = checkforPass360 + 360 # you have passed 360
pass360Point = pass360Point + 360 # add to the return value.
if currentHeading < checkforPass360:
currentHeading = checkforPass360 # this is now your new value
print(" currentHeading: {}".format(currentHeading - magOffset))
DalekV2DriveV2.stop()
print("---------------------exit turnClockwise\n")
return pass360Point
def magTurn(currentHeading, endHeading, speed, clockwise=True):
print("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@")
print("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@")
debugDirection = "turnAntiClockwise"
if clockwise:
debugDirection = "turnClockwise"
stopingTime = 0.3
if speed > 45:
stopingTime = 0.1
elif speed >= 26:
stopingTime = 0.3
print("-START magTurn_{}({},{},{})".format(debugDirection, currentHeading,
endHeading, speed))
while endHeading > currentHeading:
print("---turnClockwise() currentHeading: {}".format(currentHeading -
3000))
for i in range(10, speed):
if clockwise:
DalekV2DriveV2.spinRight(speed)
time.sleep(stopingTime)
else:
DalekV2DriveV2.spinLeft(speed)
time.sleep(stopingTime)
currentHeading = getMag(
stopingTime,
True,
currentHeading,
)
print("-done.")
return currentHeading
def getStartingMag():
# used for all timings in this function.
timerTime = 0.0
magOffset = 3000
# print("\n--getStartingMag()")
DalekV2DriveV2.stop()
time.sleep(timerTime)
currentMag = -1
while not (0 <= currentMag <=
360): # ensure we get a valid reading must be between 0 and 360
currentMag = DalekSpi.getMag()
currentMag += magOffset # add the offset value
print("---getStartingMag:{}".format(currentMag - magOffset))
return currentMag
def turnAntiClockwise(currentHeading, endHeading, timerTime, pass360Point,
speed):
magOffset = 3000
print("--------------------- turnAntiClockwise\n speed{}".format(speed))
while currentHeading >= endHeading:
time.sleep(timerTime)
checkforPass360 = getMag()
DalekV2DriveV2.spinLeft(speed)
print(" checkforPass360: {}".format(checkforPass360 - magOffset))
# take a little off it to account for reading error.
if checkforPass360 > (currentHeading + 100):
checkforPass360 = checkforPass360 - 360
pass360Point = -360
currentHeading = checkforPass360
print(" currentHeading: {}".format(currentHeading - magOffset))
DalekV2DriveV2.stop()
print("---------------------exit turnAntiClockwise\n")
return pass360Point
def turnAntiClockwise(currentHeading, endHeading, speed):
stopingTime = 0.3
if speed > 45:
stopingTime = 0.1
elif speed >= 26:
stopingTime = 0.4
print("-START turnAntiClockwise({},{},{})".format(currentHeading,
endHeading, speed))
while endHeading < currentHeading:
print(
"---turnAntiClockwise() currentHeading: {}".format(currentHeading -
3000))
for i in range(10, speed):
DalekV2DriveV2.spinLeft(speed)
time.sleep(0.015)
currentHeading = getMag(stopingTime, False, currentHeading)
print("-done.")
return currentHeading
def getMag(stopingTime, clockwise=True, currentHeading=None):
# used for all timings in this function.
timerTime = stopingTime
# The value we add to all readings to
# get over the 360 to 0 rollover and all the
# calculations that would be needed.
# use a high number so it stays above Zero
magOffset = 3000
currentMag = -1 # set a value that we can not get.
# print("\n1---getMag({},{})".format(clockwise, currentHeading ))
if currentHeading == None:
return getStartingMag()
else:
previousMagReading = currentHeading - magOffset # subtract off the offset to get previous reading.
# print("2---getMag({},{}\n)".format(clockwise, previousMagReading ))
if previousMagReading > 360:
floorDivisionOfPreviousMagReading = previousMagReading // 360 # should be 1,2 or 3
previousMagReading = previousMagReading % 360 # between 0 and 360
magOffset += (floorDivisionOfPreviousMagReading * 360
) # add the back for using later
# print("\n----getMag() previousMagReading > 360 previousMagReading:{} magOffset:{}) ".format( previousMagReading, magOffset))
# now we can get a new value.
DalekV2DriveV2.stop()
time.sleep(timerTime) # settle the bot for better reading
##################################################################################
magErrorOfset = 60
if clockwise: # Clockwise
currentMag = DalekSpi.getMag()
if 0 <= currentMag <= 360: # is between 0 and 360
# is between previous reading and an upper limit this prevents errors if
# the mag is affected buy other factors.
if previousMagReading <= currentMag <= (previousMagReading +
40):
currentMag += magOffset
# print("-------Clockwise {}".format(currentMag))
# you have rolled over the 360 mark
# val: 355 <= (5 + 360){365} <= ( 355 + 30{385}) = True
elif previousMagReading <= (currentMag + 360) <= (
previousMagReading + magErrorOfset):
currentMag += 360 + magOffset
# print("-------Clockwise Rollover{}".format(currentMag))
else:
if currentMag > (previousMagReading + magErrorOfset):
print("------error mag reading too high:{}".format(
currentMag))
DalekV2DriveV2.stop() # make sure we have stopped
time.sleep(.5) # now wait again to settle
tempcurrentMag = DalekSpi.getMag(
) # get another reading this one we will go with.
print("4 mag now {}".
format(tempcurrentMag))
else:
print("-----error in mag reading > 360 value:{}".format(
currentMag))
currentMag = currentHeading
# elif clockwise == False:
# print("---asdafdsasdf---anti Clockwise:{}".format(currentMag))
else: # anti Clockwise
currentMag = DalekSpi.getMag()
# print("1------anti Clockwise:{}".format(currentMag))
if 0 <= currentMag <= 360: # is between 0 and 360
# 120 -40{80} <= 100 <= 120
if (previousMagReading -
magErrorOfset) <= currentMag <= previousMagReading:
currentMag += magOffset
# print("2-------antiClockwise {}".format(currentMag))
elif previousMagReading - magErrorOfset <= (
currentMag - 360) <= previousMagReading:
currentMag -= 360 - magOffset
print(
"3-------antiClockwise Rollover{}".format(currentMag))
else:
if (previousMagReading - magErrorOfset) <= currentMag:
print("4------error mag reading too low:{}".format(
currentMag))
DalekV2DriveV2.stop() # make sure we have stopped
time.sleep(.5) # now wait again to settle
tempcurrentMag = DalekSpi.getMag(
) # get another reading this one we will go with.
print("4 mag now {}".
format(tempcurrentMag))
else:
print("-----error in mag reading > 360 value:{}".format(
currentMag))
currentMag = currentHeading
return currentMag
def DalekTurn(degreesToTurn):
magOffset = 3000
# used for all timings in this function.
# timerTime = 0.1
fastModeSpeed = 50
normalModeSpeed = 35
slowModeSpeed = 25
runTime = 1
print("\n#################")
print("DalekTurn({})".format(degreesToTurn))
startHeading = getStartingMag()
currentHeading = startHeading
endHeading = startHeading + degreesToTurn
print("#################\nStartHeading:{} CurrentHeading:{} EndHeading:{}".
format((startHeading - magOffset), (currentHeading - magOffset),
(endHeading - magOffset)))
# used to hold any pass of the 360/0 point
pass360Point = 0
# turn counter clockwise
if degreesToTurn < 0:
print("turn counter clockwise")
counter = 0
while endHeading < currentHeading:
if (currentHeading - endHeading >= 40):
print("#### FAST MODE ##### ")
## subtract off the endHeading so is dose not over shoot.
currentHeading = turnAntiClockwise(currentHeading,
endHeading + 30,
fastModeSpeed)
if (currentHeading - endHeading >= 20):
print("#### NORMAL MODE ##### ")
## subtract off the endHeading so is dose not over shoot.
currentHeading = turnAntiClockwise(currentHeading,
endHeading + 20,
normalModeSpeed)
print("#### SLOW MODE ##### ")
currentHeading = turnAntiClockwise(currentHeading, endHeading,
slowModeSpeed)
DalekV2DriveV2.stop()
startval = currentHeading - magOffset
endval = (endHeading % 360) - magOffset
print(" End Heading:{} should be:{}".format(startval,
endHeading - magOffset))
# turn clockwise
elif degreesToTurn > 0:
while endHeading > currentHeading:
if (endHeading - currentHeading >= 40):
print("#### FAST MODE ##### ")
## subtract off the endHeading so is dose not over shoot.
currentHeading = turnClockwise(currentHeading, endHeading - 30,
fastModeSpeed)
# currentHeading = getMag(True,currentHeading)
if (endHeading - currentHeading >= 20):
print("#### NORMAL MODE ##### ")
## subtract off the endHeading so is dose not over shoot.
currentHeading = turnClockwise(currentHeading, endHeading - 20,
normalModeSpeed)
# currentHeading = getMag(True,currentHeading)
print("#### SLOW MODE ##### ")
currentHeading = turnClockwise(currentHeading, endHeading,
slowModeSpeed)
DalekV2DriveV2.stop()
startval = currentHeading - magOffset
endval = (endHeading % 360) - magOffset
print(" End Heading:{} should be:{}".format(startval,
endHeading - magOffset))
# you entered 0 so exit
else:
DalekV2DriveV2.stop()
DalekV2DriveV2.stop()
#!/usr/bin/env python
import time
import DalekV2DriveV2
import DalekSpi
import RPi.GPIO as GPIO # Import GPIO divers
GPIO.setwarnings(False)
DalekV2DriveV2.init()
DalekSpi.init()
# this gets the spi reading and gets rid of bad readings and
# readings that are out of range due to the motors and acceleration
def getStartingMag():
# used for all timings in this function.
timerTime = 0.0
magOffset = 3000
# print("\n--getStartingMag()")
DalekV2DriveV2.stop()
time.sleep(timerTime)
currentMag = -1
while not (0 <= currentMag <=
360): # ensure we get a valid reading must be between 0 and 360
currentMag = DalekSpi.getMag()
currentMag += magOffset # add the offset value
def DalekTurn(degreesToTurn):
magOffset = 3000
# used for all timings in this function.
timerTime = 0.4
fastModeSpeed = 60
normalModeSpeed = 25
print("\n---------")
print("DalekTurn({})".format(degreesToTurn))
startHeading = getStartingMag()
currentHeading = startHeading
endHeading = startHeading + degreesToTurn
print(
"\n################ \nStartHeading:{} CurrentHeading:{} EndHeading:{}".
format((startHeading - magOffset), (currentHeading - magOffset),
(endHeading - magOffset)))
# used to hold any pass of the 360/0 point
pass360Point = 0
# turn counter clockwise
if degreesToTurn < 0:
print("turn counter clockwise")
counter = 0
while endHeading <= currentHeading:
if (currentHeading - endHeading >= 60):
print("#### FAST MODE ##### ")
## subtract off the endHeading so is dose not over shoot.
pass360Point = turnAntiClockwise(currentHeading,
endHeading + 30, timerTime,
pass360Point, fastModeSpeed)
time.sleep(timerTime)
currentHeading = getMag(True, currentHeading)
pass360Point = turnAntiClockwise(currentHeading, endHeading,
timerTime, pass360Point,
normalModeSpeed)
time.sleep(timerTime)
currentHeading = getMag(True, currentHeading)
print(" currentHeading Heading:{} should be:{} pass360Point:{}".
format((currentHeading - magOffset),
((endHeading - magOffset) - pass360Point),
pass360Point))
if counter == 5:
break
# turn clockwise
elif degreesToTurn > 0:
counter = 0
while endHeading >= currentHeading:
if (endHeading - currentHeading >= 60):
print("#### FAST MODE ##### ")
## subtract off the endHeading so is dose not over shoot.
pass360Point = turnClockwise(currentHeading, endHeading - 30,
timerTime, pass360Point,
fastModeSpeed)
time.sleep(timerTime)
currentHeading = getMag(True, currentHeading)
pass360Point = turnClockwise(currentHeading, endHeading, timerTime,
pass360Point, normalModeSpeed)
time.sleep(timerTime)
currentHeading = getMag(True, currentHeading)
print(" currentHeading Heading:{} should be:{} pass360Point:{}".
format((currentHeading - magOffset),
((endHeading - magOffset) - pass360Point),
pass360Point))
if counter == 5:
break
# you entered 0 so exit
else:
pass
DalekV2DriveV2.stop()
time.sleep(timerTime)
# mag = getMag() - magOffset
print("-- End Heading:{} should be:{}".format(
(getMag() - magOffset), ((endHeading - magOffset) - pass360Point)))
def DalekTurn1(degreesToTurn):
print("\n---------")
print("DalekTurn({})".format(degreesToTurn))
print("speed{}".format(speed))
startHeading = DalekSpi.getMag()
currentHeading = startHeading
endHeading = 0
rotationsFull = 0
rotationDegrees = 0
direction = 0
# normalise the data a
if degreesToTurn < 0:
direction = 1 # 0 clockwise , 1 anticlockwise
degreesToTurn = -degreesToTurn
# Calculate if more than one rotation
if degreesToTurn > 360:
rotationsFull = int(degreesToTurn / 360)
rotationDegrees = int(degreesToTurn % 360)
else:
rotationDegrees = degreesToTurn
print(" Rotations:{} then Deg:{} " .format(
rotationsFull, rotationDegrees))
# Clockwise rotation
if direction == 0:
# check if it goes past 360
if (startHeading + rotationDegrees) > 360:
endHeading = startHeading + rotationDegrees - 360
rotationsFull += 1
print(" Rolls over 360")
print("A: currrent:{} :{} Rotation then Deg:{} endheading:{}" .format(
currentHeading, rotationsFull, rotationDegrees, endHeading))
else:
endHeading = startHeading + rotationDegrees
print("B: currrent:{} :{}Full Rotations, then Deg:{} --- endheading:{}" .format(
currentHeading, rotationsFull, rotationDegrees, endHeading))
print(" clockwise rotation")
lastHeading = currentHeading
# while (currentHeading -5 ) <=
# endHeading <= (currentHeading +5): # between 5 either side
DalekV2DriveV2.spinRight(speed)
# if there is a rotation passed 360 then look for it
while rotationsFull > 0:
DalekV2DriveV2.spinRight(speed)
# mag = DalekSpi.getMag()
print("going passed 306")
#time.sleep(.1)
# not passed 360 yet
while currentHeading > endHeading:
DalekV2DriveV2.spinRight(speed)
currentHeading = denoiseGetMag(currentHeading)
print(" currentHeading:{}".format(currentHeading))
time.sleep(.1)
# you are now passed 360
if (currentHeading <= endHeading):
rotationsFull = 0
print(" Passed 360")
while (endHeading >= currentHeading) : # between 5 either side
DalekV2DriveV2.spinRight(speed)
print(" speed:{}".format(speed))
time.sleep(.1)
DalekV2DriveV2.stop()
currentHeading = denoiseGetMag(currentHeading)
print(" currrent:{} :{} Rotation then Deg:{} endheading:{}" .format(
currentHeading, rotationsFull, rotationDegrees, endHeading))
lastHeading = currentHeading
# if currentHeading <= (lastHeading): # gone past the 360 point
# rotationsFull -= 1
print("done currentHeading:{}".format(currentHeading))
DalekV2DriveV2.stop()
# anticlockwise rotation
else:
print(" anticlockwise rotation")
import time
import DalekV2DriveV2
import DalekMag
import RPi.GPIO as GPIO # Import GPIO divers
GPIO.setwarnings(False)
DalekV2DriveV2.init()
speed = 50
heading = DalekMag.getMag()
def DalekTurn(degreesToTurn):
startHeading = DalekMag.getMag()
currentHeading = startHeading
endDegrees = 0
endHeading = 0
rotations = 0
rotationsDegrees = 0
direction = 0 # spinright
print("\n~~~~~~~~~~")
print("Deg to Turn:{}" .format(degreesToTurn))
# normalise the data a
if degreesToTurn < 0:
direction = 1
degreesToTurn = -degreesToTurn
# more than one rotation
if degreesToTurn > 360: