How GoPiGo3 Can Find Recharge Dock (adapted iRobot dock)

(I haven’t programmed it yet - this is the design phase.)

The iRobot Dock has twin directional LEDs that are both visible +/-12 degrees off center, and one visible from 12deg out to 30degrees off center, (when my bot is greater than about 23" from the dock).

I moved the green iRobot docking status LEDs to be the directional LEDs to make the OpenCV masking easier. (I gutted the dock, added a 5v supply, and a current limit resistor for the LEDS in series.)

The mask can be either:

HSV:
    29 < H < 99
    S > 190
    V > 150

RGB:  G > 200

Use OpenCV on successive images captured by the PyCam to find the recharging dock.

Algorithm for find_dock():

  1. Capture an image
  2. Mask for green LED(s) of the dock
  3. Find number and position in the image of green LED(s)
  4. If no LEDs and number of captures < “360 degrees of captures” :
    turn capture width and continue from step 1
    else if no LEDs in 360 degrees: declare “dock not visible (at this location)”
  5. Calculate dock angle relative to heading angle based on horizontal LED distance from center
  6. Estimate dock distance based on vertical LED position in image
  7. Point distance sensor toward dock, take distance reading
  8. Fuse estimate and reading for distance to dock
  9. Point distance sensor fwd and 10" away (for U turn clearance plus 1" )
  10. If distance to dock GE 30" turn to face dock, otherwise turn away from dock
  11. While distance sensor reading > 9" (U turn clearance), drive to point 30" from dock
  12. If drove away from dock, turn to face dock
  13. Perform wall_scan() returns distance to wall, angle to wall normal
  14. Calculate turn angle to intersect wall-normal from dock-center at 90 degrees
  15. Calculate distance from current position to dock-ctr-wall-normal
  16. Turn to intersect wall-normal from dock at 90 degrees
  17. While distance sensor reading > 9", drive to dock-wall-normal
  18. Turn to face dock

Followed by approach_dock(), and then dock()

Can You Help Me Find My Dock?