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
pts = pts[:,0]
# x point closest to middle
x = pts[(np.abs(pts-rt)).argmin()]
# No blobs detected spin around until one is found
else:
x = 0
# Error from goal using PID functions
et = move.distanceToPIDParam(x, rt)
w = move.Ur(kp,et, cmax, cmin)
# Goal close to middle of frame
if et < 10 and et > -10:
# 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)
# 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()
elif state == "motion to goal":
# Motion to go
print(state)
# PID functions
w = move.Ur(kp, angular_error, cmax, cmin)
frontVelocity = 0
#print("angular w", w)
if angular_error < 17 and angular_error > -17:
print("facing goal")
# PID functions for front distance
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)
else:
# Set angular speed
w = move.Ur(kp, angular_error, cmax, cmin)
my.setSpeedsVW(-frontVelocity, -w, leftTotal, rightTotal, pwmData)
elif state == "wall following":
print(state)
if (fSensor.get_distance() / 25.4 < 5):
state = "turn"
continue
error_front_wf = move.distanceToPIDParam(fSensor.get_distance() / 25.4,
4)
frontVelocity_wf = move.Ur(1, error_front_wf, 3, -3)
else:
rt_r = -5
rt_l = -5
yt, et, yt_r, et_r, yt_l, et_l = move.distancesToPIDParam(
fSensor.get_distance(), rt, rSensor.get_distance(), rt_r,
lSensor.get_distance(), rt_l)
# PID functions
ur = move.Ur(kp, rt, yt, cmax, cmin)
ur_r = move.Ur(kp, rt_r, yt_r, cmax, cmin)
ur_l = move.Ur(kp, rt_l, yt_l, cmax, cmin)
if (et < 1 and et > -1):
print("ONLY FRONT to front")
my.setSpeedsIPS(ur, ur, leftTotal, rightTotal, pwmData)
elif (stateN == 'LR'):
print("In Hallway")
my.setSpeedsIPS(ur, -ur_r + ur, leftTotal, rightTotal, pwmData)
print("set speeds", ur, -ur_r + ur)
elif (stateN == 'L'):
print("only left wall")
my.setSpeedsIPS(-ur_l + ur, ur + ur_l, leftTotal, rightTotal, pwmData)
print("set speeds", -ur_l + ur, ur + ur_l)
elif (stateN == 'R'):
print("only right wall")
my.setSpeedsIPS(ur + ur_r, -ur_r + ur, leftTotal, rightTotal, pwmData)
print("set speeds", ur + ur_r, -ur_r + ur)