Vision Tracking in FRC — What I’ve Learned this Year

Vision tracking in FRC games every year is something that every team has a challenge with. For me, this was actually my first year as being a head programmer for my team, which was tough. Our mentor is always one to push, whether it be having full control over the code, using PID’s, and even tuning our flywheel to be within a couple RPM of our target, he’s pushed us to do it. This year, our mentor had a new task: tackling vision tracking. It seemed really daunting to myself, being the only programmer on our team, to completely have control over our robot, and being able to do vision tracking for the high goal and the low goal. But I finished the code quite early into the season, even before bag and tag this year, with all of the code Open-Sourced on my GitHub here. Our code worked really reliably throughout the season, even winning us Innovation in Control at the Oklahoma City Regional. In this article I’m going to be talking about the do’s and the dont’s of vision tracking, and how to succeed with it

Where do I start?

That’s a really good question. Honestly at the start of the season, I had no idea what I was doing. If you would’ve asked me about vision tracking, I would tell you to find another guy, I just had no idea on where to start. I first looked at the 254 Vision Video from 2015, which really helped on creating the library. If you haven’t watched this video, stop reading this article, and watch the video, it’s honestly one of the best videos to watch.

After watching this video, I’d look at some other teams code from past years on vision tracking. Some good repos to check are LiftTracker and Fauge7’s TowerTracker. I think both are good repos to check out because they use different methods on finding the goal, and getting the distance from the contours.

Alright I’m Competent, Where do I Start?

The first place I would start out when making this vision tracking program is the math side. Laying out your game plan on how you are going to find you angle will make your life a whole lot easier once you come to actually programming it. You need two things to find the angle: the distance away from the target and the distance we are off in the X direction. After you find those two terms, you just simply do the inverse tangent, and you are done. After you do that, think of the methodology you are going to use to find the contours of the retro reflective tape. Whether it be a GRIP Pipeline or your own custom OpenCV pipeline, the choice is yours.

GRIP and OpenCV, How Do I Find the Goal?

This is all in personal preference. For us, we chose GRIP as a basic library to find the contours. GRIP is pretty much a GUI that allows you to build your own OpenCV pipeline, without dealing with code. I will add on more later on why we chose this decision. Whichever one you use, you will be using close to the same pipeline idea. The first thing you want to do is remove any noise from your picture that you can. You can do this by adding a box blur to the image. Next you will plug in the video into an HSV Threshold. This takes time to tune, but you will get good results and good feedback if you tune this correctly. Next you will want to filter out unwanted contours. You can do this in different ways like plugging in a minimum area, width, height, or ratio. You can find an example GRIP file that does all of this on my GitHub.

How Do I Find How Far Away I Am?

In my mind, there are three different ways you can do this. One way to do it is by using a sensor to tell you how far away you are from the goal, eg: ultrasonic. Another way you can achieve this is by fitting an equation to the pixel location of the contour, and plugging in the pixels to the equation. You form the equation by pre-measuring out points with your robot and the camera. Record your distance, then record where the pixel is on the screen. Once you do this, use a program like Logger Pro to form your equation based off the data. Inside of the code you would then want to plug in the pixel to your newly formed equation, and you have a distance. The third and final way to do this is to form an equation to find the distance based off the FOV of the camera. I haven’t found a solid equation that I will put here, but take a look at Miss Daisy’s code that they used to find the distance.

Okay, I Know How Far Away I Am, Now How Far Off Am I?

So I’m talking about the other term (the y term) in our triangle. You know the X term (the distance), you just need to find how off you are from the center of the goal. Now that you have your GRIP / OpenCV pipeline, you also have the centerX value of your contour. What we find the best to find how off you are is by using a proportion. We set this up by using the equation:

All you do in this equation is multiply the distance you are from the center in pixels, by your known width of the target (8.5 in for the Lift this year), divided by the length of the contour, in pixels. After you get this distance, you can now do the inverse tangent and get your angle. Be sure you aren’t in radians (Thanks Mr. Logan)!

Awesome, I’m Getting my Angle, Now What?

After that, you now have to make your robot do something with the angle. One way that we manipulate the angle is by attaching a PID controller to the robot. So in our auto, we shoot 10 balls, then vision track the lift, and place our gear. The drive to lift in our code is on a PID with our angle, and this has worked really well for us.

How Do I Run My Vision Program?

Congrats! You have working vision, but now you might be wondering how you will do this in a match. You pretty much have two ways to do this. One way to do it is run this on the driver station laptop. We have been doing this for the past 2 years, and it has worked well for us. Though, after lots of FMS pains, we are deciding against it now. It has worked for us most of every time, but there may be issues with the FMS where we might only get 5 FPS coming back to the driver station, and cause jerkiness. We are now switching over to having a co-processor. A co-processor is essentially just a second computer on the robot that will run your vision. For a more cost efficient co-processor we recommend the Raspberry Pi. Though, if you do have some extra FIRST credits lying around, check out the Jetson TK1 / TX1. This is a development board created by NVIDIA for vision use. Though we have found that they are a bit hard to setup. These are usually available for FIRST choice, so be on the lookout!

Things To Remember / Tips

You shouldn’t have to tune much of anything at competition. For us, we barely had to do any calibration on our cameras. All of it was done at school with different lighting conditions. Though, we try to tune for at least 30 minutes at each competition. For myself, I just cloned our camera settings locally on a laptop, so we don’t bring a robot with me while tuning. I just walk out to the field with a laptop only. Speaking of tuning, GRIP makes it really quick and easy to do this, and why I like using it. Tuning for me literally is sliding a HSL tuner, and that’s it. No messing with lines of code. Another thing that I like to do is make the image as dark as possible. Use the brightest LED’s you can so you can drop the exposure a lot. This makes tuning a whole lot easier. Also, programmers, show this next line to your build team. Build around sensors, not just sloppily putting them in. There is no reason to have an awkward camera placement just because your build-team “forgot”. Always nag them to have a good central spot for the camera. I’ve seen teams have bad problems with having to deal with camera offset from different robot subsystems, there is no point to having it offset, it just causes inefficiency.