Fixing OdometryTags: control
Task: find and fix old odometry code
This year, more than the 2 previous years, odometry plays a critical role in our solution while vision is on the fritz. This is due to the fact that we plan to do continuous targeting, which requires a way to know the distance and angle to any given target. Our original plan was to use VuForia, but after preliminary testing gave mixed results, we decided to review our other options. Odometry seemed fitting since it's a lot more stable. Plus, if we find that it drifts, we can always add live offsets through a sensor-fusion based model.
So today we went through the catacombs of the code and dusted off some 2-3 year fine aged code. Uncommenting the code and adding some of the variables to dashboard, we started up the robot to be met with the variables in dashboard going absolutely wild. They seemed to be rapidly going from -infin to infin. After a bit of head scratching, a couple of Hmms, at least one sip of coffee, and no fewer than 3 “A-Ha!” moments (as opposed to “ABBA!” moments), we fixed the run-away. And, get this, the code just worked. They do say 2018 was a good code year, after all.
With that in the bag, we started working on the supportive code. First the getters and setters, then a getDistanceTo() method. These were all the methods that will be useful to us for continuous targeting. However, we took it a step further, and made a driveToFieldPosition, and a supplementary method getBearingTo. While not validated as of writing this, when working it will help us both in auton and in demo.
Test reliability of odometry over long durations of time, and validate the driveToFieldPos method.