def motorTesting():
motorSelfTest.Forward(100,75)
motorSelfTest.Stop()
motorSelfTest.Backward(100,75)
motorSelfTest.Stop()
motorSelfTest.RightTurn()
motorSelfTest.Stop()
motorSelfTest.LeftTurn()
motorSelfTest.Stop()
currentLine = float(message)
desLat, desLong = stripString(points)
if currentLine > 0.0:
flag = 1
elif currentLine < 0.0:
flag = 2
# ===== ANGLES ======
anglesArray = TurnAngles()
# ===== Turning ======
motorSelfTest.Stop(4.0)
Turning(flag)
# ===== Forward Movement ======
lat, lon = GPSdata()
distance = distanceFormula(lat, lon, desLat, desLong)
start_time = time.time()
motorSelfTest.Forward(2.0, 100, 100)
motorSelfTest.Stop(0.1)
if flag == 1:
motorSelfTest.LeftTurn(1.5)
elif flag == 2:
motorselfTest.RightTurn(1.5)
motorSelfTest.Stop(2.0)
# ====== TURRET CODE =======
TurretRotation(lat, lon)
os.system(' python /home/pi/Desktop/build/Platform/platformCCW.py')
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')
vibration = open("/home/pi/Desktop/build/Platform/vibration.txt", "r")
data = vibration.readline()
flag = 2
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, lon, desLat, desLong)
#Motor Activates for 10 Seconds
motorSelfTest.Forward(100,100)
while (distance >= 0.05):
lat, lon = GPSdata()
boundary = checkBounds(lat, lon, LEFT_LAT, RIGHT_LAT, UP_LONG, LOW_LONG)
if boundary == 1:
print("Out of Bounds")
# sys.exit()
if lat > desLat and flag==1:
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)
def motorTesting():
motorSelfTest.Forward(100, 75)
motorSelfTest.Stop()
motorSelfTest.Backward(100, 75)
motorSelfTest.Stop()