Problem
When we get near a cone, we need to be able to decouple the motion code from GPS navigation and rely on OpenCV. However, we need to keep reading GPS data to make sure we have not left the zone where a cone may exist.
Solution
Add a state to the motion code that relies on OpenCV to attack the cone. This should include reading in data over I2C that will tell the robot how much to move in each direction, creation of a PID controller to minimize error in targeting the cone, and appropriate control to avoid straight up missing the cone.
Additionally, this code needs to make sure that it does not leave the radius around the cone. If it does, it should recouple to the localization module and try to find the cone again. If the cone is hit successfully (we will know this due to limit switches) then the appropriate GPS coordinates should be popped off the queue, and the robot should continue along its way
Problem When we get near a cone, we need to be able to decouple the motion code from GPS navigation and rely on OpenCV. However, we need to keep reading GPS data to make sure we have not left the zone where a cone may exist.
Solution Add a state to the motion code that relies on OpenCV to attack the cone. This should include reading in data over I2C that will tell the robot how much to move in each direction, creation of a PID controller to minimize error in targeting the cone, and appropriate control to avoid straight up missing the cone.
Additionally, this code needs to make sure that it does not leave the radius around the cone. If it does, it should recouple to the localization module and try to find the cone again. If the cone is hit successfully (we will know this due to limit switches) then the appropriate GPS coordinates should be popped off the queue, and the robot should continue along its way