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 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):
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
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 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)
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)
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()
def returnTurning(flag):
if flag == 1:
motorSelfTest.LeftTurn(1.5)
elif flag == 2:
motorSelfTest.RightTurn(1.5)
motorSelfTest.Stop(0.25)
while True:
# ==== READS IN LOCATIONS =====
message = f.readline()
points = g.readline()
if message == '' or points == '':
break
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')
def motorTesting():
motorSelfTest.Forward(100, 75)
motorSelfTest.Stop()
motorSelfTest.Backward(100, 75)
motorSelfTest.Stop()
