(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():
- Capture an image
- Mask for green LED(s) of the dock
- Find number and position in the image of green LED(s)
- 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)” - Calculate dock angle relative to heading angle based on horizontal LED distance from center
- Estimate dock distance based on vertical LED position in image
- Point distance sensor toward dock, take distance reading
- Fuse estimate and reading for distance to dock
- Point distance sensor fwd and 10" away (for U turn clearance plus 1" )
- If distance to dock GE 30" turn to face dock, otherwise turn away from dock
- While distance sensor reading > 9" (U turn clearance), drive to point 30" from dock
- If drove away from dock, turn to face dock
- Perform wall_scan() returns distance to wall, angle to wall normal
- Calculate turn angle to intersect wall-normal from dock-center at 90 degrees
- Calculate distance from current position to dock-ctr-wall-normal
- Turn to intersect wall-normal from dock at 90 degrees
- While distance sensor reading > 9", drive to dock-wall-normal
- Turn to face dock
Followed by approach_dock(), and then dock()
Can You Help Me Find My Dock?
