def motorTesting():
motorSelfTest.Forward(100,75)
motorSelfTest.Stop()
motorSelfTest.Backward(100,75)
motorSelfTest.Stop()
motorSelfTest.RightTurn()
motorSelfTest.Stop()
motorSelfTest.LeftTurn()
motorSelfTest.Stop()
def motorTesting():
motorSelfTest.Forward(100, 75)
motorSelfTest.Stop()
motorSelfTest.Backward(100, 75)
motorSelfTest.Stop()
print("flag: " + str(flag))
Offset = CalibrateIMU(readValue)
#==== Angles =====
anglesArray = TurnAngles()
#==== Turning ====
print(anglesArray[i])
#Turning(flag, Offset, anglesArray[i], readValue)
#==== Forward Movement ====
lat, lon = GPSdata()
distance = distanceFormula(lat, ORIGIN_LONGITUDE, desLat, ORIGIN_LONGITUDE)
#Motor Activates for 10 Seconds
if (desLat < lat):
motorSelfTest.Backward(random.randint(25, 100), 100)
if (lat > desLat):
motorSelfTest.Forward(random.randint(25, 100), 100)
while (distance >= 0.05):
lat, lon = GPSdata()
boundary = checkBounds(lat, lon, LEFT_LAT, RIGHT_LAT, UP_LONG,
LOW_LONG)
distance = distanceFormula(lat, ORIGIN_LONGITUDE, desLat,
ORIGIN_LONGITUDE)
print distance
motorSelfTest.Stop()
#===== Turret Code =====
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')