# Get X coordinate of blob on camera
if(len(keypoints) > 0):
# X,Y points of blob
pts = np.array(cv.KeyPoint_convert(keypoints))
# Array of x points of blob
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:
# Get distance sensor data
front_distance = fSensor.get_distance() / 25.4
left_distance = lSensor.get_distance() / 25.4
# Get current state of robot
currentState()
# Determine motion based on state
if state == "starting":
print(state)
elif state == "face goal":
print(state)
angular_error = move.distanceToPIDParam(x, rt_targetInframe)
# PID functions
w = move.Ur(kp, angular_error, cmax, cmin)
my.setSpeedsVW(0, -w, leftTotal, rightTotal, pwmData)
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)
# Get X coordinate of blob on camera
if (len(keypoints) > 0):
# X,Y points of blob
pts = np.array(cv.KeyPoint_convert(keypoints))
# Array of x points of blob
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
et = move.distanceToPIDParam(x, rt)
#print("angular error",et)
# PID functions
w = move.Ur(kp, et, cmax, cmin)
frontVelocity = 0
#print("angular w", w)
# For each detected blob, draw a circle on the frame
frame_with_keypoints = cv.drawKeypoints(
frame,
keypoints,
None,
color=(0, 255, 0),
flags=cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
# Write text onto the frame
#cv.putText(frame_with_keypoints, "FPS: {:.1f}".format(fps), (5, 15), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0))
#cv.putText(frame_with_keypoints, "{} blobs".format(len(keypoints)), (5, 35), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0))
stateN = state(leftDistance, rightDistance)
if (stateN == 'LR'):
totalDistance = 4.375 + leftDistance + rightDistance
rt_r = -(totalDistance - 4.375) / 2
rt_l = rt_r
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)
