def linearMovement(ips, distance, leftTotal, rightTotal, pwmData):
# Check if inches per second are possible
if not checkInputLinearIPS(ips, leftTotal, rightTotal):
print("Inches per second excedes phyical parameters")
# Get counts from encoders at start
ticksL, ticksR = en.getCounts()
# Set forward speeds and begin linear movement
en.setSpeedsIPS(ips, ips, leftTotal, rightTotal, pwmData)
while True:
# Calculate distance of wheels traveled based on encoder counts (ticks)
distanceL, distanceR = km.DistanceWheels(ticksL, ticksR)
# Break loop when complete distance is traveled
if (distanceR >= abs(distance) or distanceL >= abs(distance)):
en.setSpeedsPWM(1.5, 1.5)
#print("Distance Left Right: ", distanceL, ' ',distanceR)
#print('Time taken to complete task: ', time.monotonic() - timeStart)
break
return
def orientationMovement(w, turnAngle, leftTotal, rightTotal, pwmData):
# Check if inches per second are possible
if not checkInputLinearIPS(w, leftTotal, rightTotal):
print("Inches per second excedes phyical parameters")
# Calculate arch length (distance) to travel given turnAngle and motion is opientation (R = DMID)
distance = turnAngle * DMID
#print('Distance each wheel travels in inches: ', distance)
# Sets speeds to angular velocity w, stops based on distance of wheels traveled calculated
ticksL, ticksR = en.getCounts()
en.setSpeedsVW(0, w, leftTotal, rightTotal, pwmData)
while True:
# Calculate distance of wheels traveled based on encoder counts (ticks)
distanceL, distanceR = km.DistanceWheels(ticksL, ticksR)
# Break loop when complete distance is traveled
if (distanceR >= abs(distance) or distanceL >= abs(distance)):
en.setSpeedsPWM(1.5, 1.5)
#print("Distance Left Right: ", distanceL, ' ',distanceR)
#print('Time taken to complete task: ', time.monotonic() - timeStart)
break
return
# Connect the right sensor and start measurement GPIO.output(RSHDN, GPIO.HIGH) time.sleep(0.01) rSensor.start_ranging(VL53L0X.VL53L0X_GOOD_ACCURACY_MODE) # Connect the front sensor and start measurement GPIO.output(FSHDN, GPIO.HIGH) time.sleep(0.01) fSensor.start_ranging(VL53L0X.VL53L0X_GOOD_ACCURACY_MODE) # Initialize encoders my.initEncoders() # Set servos to netural my.setSpeedsPWM(1.5, 1.5) pwmData, leftTotal, rightTotal = my.readCalibrateFile() # Tuple of colors with hsv min and max HSV_MIN = 0 HSV_MAX = 1 # Goal colors HSV color1 = [(0,91,61),(9,132,176)] # orange color2 = [(42,106,55),(68,164,157)] # Green color3 = [(152,114,77),(173,185,194)] # pink # PID constants cmax = 1 cmin = -1 kp = .02
#calibration.py #This progeam is stepping through each PWM from 1.3 - 1.7 at intervals of 0.005 #Saves results in a file import time import encoder_lib as enc # Calibrate encoders enc.initEncoders() timeX = time.monotonic() enc.setSpeedsPWM(1.5, 1.5) enc.calibrateSpeeds(1, 'CalibrateResults.txt') print(time.monotonic() - timeX) enc.end()
def stopForTurn(t=.5):
en.setSpeedsPWM(1.5, 1.5)
timeStartWaste = time.monotonic()
while (time.monotonic() - timeStartWaste < t):
continue
return