def Turning(flag, Offset, a, readValue):
calcAngle = a
if calcAngle < 0.0:
calcAngle = calcAngle + 360.0
elif calcAngle > 360.0:
calcAngle = calcAngle - 360.0
print("Turning now:")
print("Target Angle" + str(calcAngle))
if flag == 1:
angle = IMU(Offset,readValue)
motorSelfTest.RightTurn()
while angle < calcAngle:
print("Current angle " + str(angle))
#Loops until angle is achieved
angle = IMU(Offset,readValue)
motorSelfTest.Stop()
elif flag == 2:
angle = IMU(Offset,readValue)
motorSelfTest.LeftTurn()
while angle > calcAngle:
print("Current angle " + str(angle))
#Loops until angle is achieved
angle = IMU(Offset,readValue)
motorSelfTest.Stop()
motorSelfTest.Stop()
def returnTurn(flag, Offset):
if flag == 1:
while (IMU(Offset) > 0) or (IMU(Offset) < 360):
motorSelfTest.LeftTurn(1.5)
elif flag == 2:
while (IMU(Offset) > 0) or (IMU(Offset) < 360):
motorSelfTest.RightTurn(1.5)
motorSelfTest.Stop(0.25)
def returnTurn(flag, Offset):
if flag == 1:
while (IMU(Offset) > 10) or (IMU(Offset) < 350):
motorSelfTest.LeftTurn()
elif flag == 2:
while (IMU(Offset) > 10) or (IMU(Offset) < 350):
motorSelfTest.RightTurn()
motorSelfTest.Stop()
def motorTesting():
motorSelfTest.Forward(100,75)
motorSelfTest.Stop()
motorSelfTest.Backward(100,75)
motorSelfTest.Stop()
motorSelfTest.RightTurn()
motorSelfTest.Stop()
motorSelfTest.LeftTurn()
motorSelfTest.Stop()
def returnTurn(flag, Offset,readValue):
angle = IMU(Offset,readValue)
if flag == 1:
while (angle > 10) or (angle < 350):
motorSelfTest.LeftTurn()
elif flag == 2:
while (angle > 10) or (angle < 350):
motorSelfTest.RightTurn()
motorSelfTest.Stop()
def Turning(flag, Offset, a):
CurrentAngle = IMU(Offset)
calcAngle = CurrentAngle + a
if flag == 1:
while IMU(Offset) < calcAngle:
print(IMU(Offset))
motorSelfTest.RightTurn(1.5)
elif flag == 2:
while IMU(Offset) > calcAngle:
print(IMU(Offset))
motorSelfTest.LeftTurn(1.5)
motorSelfTest.Stop(0.25)
def returnTurn(flag, Offset, readValue):
if flag == 1:
angle = IMU(Offset, readValue)
while (angle > 10):
motorSelfTest.LeftTurn()
print(angle)
angle = IMU(Offset, readValue)
motorSelfTest.Stop()
elif flag == 2:
angle = IMU(Offset, readValue)
while (angle < 350):
motorSelfTest.RightTurn()
print(angle)
angle = IMU(Offset, readValue)
motorSelfTest.Stop()
motorSelfTest.Stop()
return
motorSelfTest.Stop()
Turning(flag, Offset, 90, readValue)
lat, lon = GPSdata()
distance = distanceFormula(lat,lon,desLat,desLong)
motorSelfTest.Forward(random.randint(25,100),100)
elif lat < destLat and flag==2:
motorSelfTest.Stop()
Turning(flag, Offset, 270, readValue)
lat, lon = GPSdata()
distance = distanceFormula(lat,lon,desLat,desLong)
motorSelfTest.Forward(random.randint(25,100),100)
distance = distanceFormula(lat, lon, desLat, desLong)
print distance
motorSelfTest.Stop()
if flag == 1:
motorSelfTest.LeftTurn()
elif flag == 2:
motorSelfTest.RightTurn()
#===== Turn to North =====
returnTurning(flag, Offset, readValue)
#===== Turret Code =====
# Fix turret calculation
TurretRotation(lat, lon)
os.system('python /home/pi/Desktop/build/Platform/platformCCW.py')
# Arm Up works fine, no need to touch it
os.system('python /home/pi/Desktop/build/Platform/armUp.py')
start_time = time.time()
while time.time() - start_time <= 4:
os.system('python /home/pi/Desktop/build/Platform/vibration.py')
def returnTurning(flag):
if flag == 1:
motorSelfTest.LeftTurn(1.5)
elif flag == 2:
motorSelfTest.RightTurn(1.5)
motorSelfTest.Stop(0.25)
