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