Obstacle Avoidance algorithm: The obstacle avoidance algorithm used was a combination of Bug 0 and Bug 1. Every millisecond the Robot would sweep the entire range of LADAR and check if a window of +/- 5 degrees towards the Destination has any obstacles in the way. If there are, then the robot would check if the obstacle is on the left or the right, depending on that it will start following the obstacle's periphery till again the destination is visible and obstacle free. As soon as this condition is met, the robot will break out of wall following and move towards the obstacle. Some corner cases were included as well so that the robot does not get stuck in the arena forever, or it does not follow the outside walls of the arena.
Ball Detechtion Algorithm: Golf balls were detected using the Robot's camera system. During coding, photos were taken of both the blue and orange balls and HSV threshold values were determined for each. The Colorvision.c program detected regions of pixels in blue and orange and relayed this information to the Robot. When a ball was detected, the Robot entered into ball collection. Mis-detections were prevented by establishing a threshold of 15 pixels minimum prior to entering this mode. The x centroid of the region of pixels was sent to the robot destination and the velocity of the robot was considerably slowed to .1 tiles/second, which slowed the velocity of other balls already contained within the Robot to prevent them from leaving the compartment. The Robot continued to center the ball in its trajectory, up until the ball was a distance of approximately 1 tile away. At this point, depending on the color of the robot, the corresponding color door would open using a servo and the robot would turn slightly in the direction of that compartment. The robot would continue moving forward for .2 seconds to ensure the ball was contained inside the compartment, at which point the color door would close and the Robot would continue forward using Optitrack and dead reckoning. The closing of the compartment door also triggered data to be sent to out VB application to plot the x position, y position and color of the collected ball.Source Code