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
# Stop motion of the robot to get distances
my.setSpeedsIPS(0,0,leftTotal,rightTotal,pwmData)
# Checks which goal and finds distance to goal
if not seenGoal1:
i = 0
while i < 30:
distance1 = fSensor.get_distance()/25.4
if distance1 > 74:
distanceC1 += 65
else:
distanceC1 += distance1
i += 1
r1 = distanceC1 / 30
seenGoal1 = True
my.setSpeedsVW(0,w,leftTotal,rightTotal, pwmData)
elif not seenGoal2 and seenGoal1 and not seenGoal3:
i = 0
while i < 30:
distance2 = fSensor.get_distance()/25.4
if distance2 > 74:
distanceC2 += 65
else:
distanceC2 += distance2
i += 1
r2 = distanceC2 / 30
seenGoal2 = True
my.setSpeedsVW(0,w,leftTotal,rightTotal, pwmData)
elif not seenGoal3 and seenGoal1 and seenGoal2:
# Display the frame
cv.imshow(WINDOW1, mask)
cv.imshow(WINDOW2, frame_with_keypoints)
# Check for user input
c = cv.waitKey(1)
if c == 27 or c == ord('q') or c == ord('Q'): # Esc or Q
camera.stop()
break
if et < 15 and et > -15:
# PID functions for front distance
# print("distance", fSensor.get_distance()/25.4)
error_front = move.distanceToPIDParam(fSensor.get_distance() / 25.4,
goal)
# print("error front--", error_front)
frontVelocity = move.Ur(kp_f, error_front, cmax_f, cmin_f)
my.setSpeedsIPS(-frontVelocity, -frontVelocity, leftTotal, rightTotal,
pwmData)
blobSizes.append(keypoints[0].size)
distances.append(fSensor.get_distance() / 25.4)
# print("move to goal")
else:
# Set angular speed
w = move.Ur(kp, et, cmax, cmin)
# Set angular speed
my.setSpeedsVW(-frontVelocity, -w, leftTotal, rightTotal, pwmData)
camera.stop()
move.Uturn(leftTotal, rightTotal, pwmData)
elif (stateN == 'R'):
move.LeftTurn(leftTotal, rightTotal, pwmData)
elif (stateN == 'L' or stateN == 'F'):
move.RightTurn(leftTotal, rightTotal, pwmData)
while stateN == 'LR':
frontDistance = fSensor.get_distance() / 25.4
leftDistance = lSensor.get_distance() / 25.4
rightDistance = rSensor.get_distance() / 25.4
if (rightDistance >= leftDistance):
while (frontDistance < 10):
my.setSpeedsVW(0, .8, leftTotal, rightTotal, pwmData)
frontDistance = fSensor.get_distance() / 25.4
my.setSpeedsIPS(0, 0, leftTotal, rightTotal, pwmData)
break
elif (rightDistance < leftDistance):
while (frontDistance < 10):
my.setSpeedsVW(0, -.8, leftTotal, rightTotal, pwmData)
frontDistance = fSensor.get_distance() / 25.4
my.setSpeedsIPS(0, 0, leftTotal, rightTotal, pwmData)
break
# Stop measurement for all sensors
lSensor.stop_ranging()
fSensor.stop_ranging()
rSensor.stop_ranging()