def manualTurn(direction, angle):
global automaticNavigation,estimatedRoverDirection,lastTurnAutomatic
automaticNavigation = False
lastTurnAutomatic = False
if(direction == 0):
timeToTurn = (abs(angle) / 90) * turnCoefficientLeft
estimatedRoverDirection -= angle
estimatedRoverDirection = (estimatedRoverDirection + 360) % 360
arduino.motorCtl("B","F")
time.sleep(timeToTurn / 1000)
arduino.motorCtl("S","S")
else:
timeToTurn = (abs(angle) / 90) * turnCoefficientLeft
estimatedRoverDirection += angle
estimatedRoverDirection = (estimatedRoverDirection + 360) % 360
arduino.motorCtl("F","B")
time.sleep(timeToTurn / 1000)
arduino.motorCtl("S","S")
def update():
global turnCoefficientLeft, turnCoefficientRight
global refLat, refLon, targetLat, targetLon
global automaticNavigation, lastTurnAutomatic, directionLastTurn, estimatedRoverDirection, angleLastTurn
global directionValidityStatus
if(automaticNavigation):
currentLat = datahandler.latestLat
currentLon = datahandler.latestLon
if(approx_Equal(currentLat,targetLat,0.00005) and approx_Equal(currentLon,targetLon,0.00005)):
automaticNavigation = False
arduino.motorCtl("S","S")
return
distance, bearing = distBearing(currentLon,currentLat,targetLon,targetLat)
distanceTravelled, currentBearing = distBearing(refLon,refLat,currentLon,currentLat)
if(distanceTravelled > 7):
arduino.motorCtl("S","S")
if(directionLastTurn == 0):
delta = currentBearing - estimatedRoverDirection
else:
delta = estimatedRoverDirection - currentBearing
estimatedRoverDirection = bearing
if((directionValidityStatus >= 1) and lastTurnAutomatic ):
lastTurnCoefficient = (abs(delta) / 90) / timeLastTurn
if(directionLastTurn == 0):
turnCoefficientLeft = (turnCoefficientLeft * 10 + lastTurnCoefficient) / 11
else:
turnCoefficientRight = (turnCoefficientRight * 10 + lastTurnCoefficient) / 11
with open('/home/root/turncal', 'w') as f:
f.write(str(turnCoefficientLeft) + "," + str(turnCoefficientRight) + "\n")
directionValidityStatus = 1
amountToTurn = currentBearing - bearing
if(abs(amountToTurn) > 10):
directionValidityStatus = 2
lastTurnAutomatic = True
if(amountToTurn > 0):
timeToTurn = (amountToTurn / 90) * turnCoefficientLeft
angleLastTurn = amountToTurn
directionLastTurn = 0
estimatedRoverDirection -= amountToTurn
estimatedRoverDirection = (estimatedRoverDirection + 360) % 360
arduino.motorCtl("B","F")
time.sleep(timeToTurn / 1000)
arduino.motorCtl("S","S")
else:
timeToTurn = (abs(amountToTurn) / 90) * turnCoefficientRight
angleLastTurn = amountToTurn
directionLastTurn = 1
estimatedRoverDirection += amountToTurn
estimatedRoverDirection = (estimatedRoverDirection + 360) % 360
arduino.motorCtl("F","B")
time.sleep(timeToTurn / 1000)
arduino.motorCtl("S","S")
refLon = currentLon
refLat = currentLat
arduino.motorCtl("F","F")
def manualBackward():
global automaticNavigation
automaticNavigation = False
arduino.motorCtl("B","B")
def manualForward():
global automaticNavigation
automaticNavigation = False
arduino.motorCtl("F","F")
def manualStop():
global automaticNavigation
automaticNavigation = False
arduino.motorCtl("S","S")
