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