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()