Active Vision Processing using C++


My team has been struggling for quite some time now with programming our robot to be able to align itself using vision processing and a camera for quite some time now. We use C++ with wpilib, and are so far unsuccessful at achieving vision control for our robot. We have been mainly trying the GRIP pipeline which seems to work in the program itself, but we appear to have trouble using it on the robot and controlling the robot with it. Is there anything we are missing? How did you go about implementing the GRIP generated code into your robot program? Thank you!

I’m not sure if you use command based code, but here’s what my team uses for our gear alignment in autonomous.
Our use of encoders to get our robot close enough for vision to take over is used in this command as the command stops as the speed of the encoders is too slow, signaling we have hit the tower. This command needs just a few RPM on the encoders before it’s started, I normally do a Drive for Time at only about quarter speed for 3/4 of a second.
This command is made with a lot of arguments specifically because I wanted it to be malleable, as center peg can be faster with less turn speed than a left peg, which might have a higher error range and needs more adjustment speed to fix it.

#include "DriveGearV.h"
#include <stdlib.h>
#include <Robot.h>
DriveGearV::DriveGearV(int target, double forwardspeed, double adjustment,
		double stopspeed) {

	m_finish = false;
	m_target = target;
	m_centerpos = 0;

	m_forwardspeed = forwardspeed;
	m_adjustment = adjustment;
	m_stopspeed = stopspeed;
void DriveGearV::Initialize() {
	m_finish = false;
void DriveGearV::Execute() {
	if ((Robot::DriveTrainsubsystem->VArrSize() < 2)) {
Robot::DriveTrainsubsystem->Arcade(.25, 0);
	if (Robot::DriveTrainsubsystem->EncoderSpeed("LEFT") < m_stopspeed) {
		m_finish = true;

	m_contour1 = (Robot::DriveTrainsubsystem->VReport("centerX", 0));
	m_contour2 = (Robot::DriveTrainsubsystem->VReport("centerX", 1));

	if (m_contour1 < m_contour2) {
		m_centerpos = ((abs(m_contour2 - m_contour1) / 2) + m_contour1);
	if (m_contour2 < m_contour1) {
		m_centerpos = ((abs(m_contour2 - m_contour1) / 2) + m_contour2);

//The above determines which contour is on the left and which is on the right, then sets the center position to in between them

	if ((m_centerpos > m_target - 1) && (m_centerpos < m_target + 1)) {
		Robot::DriveTrainsubsystem->Arcade(-m_forwardspeed, 0);
	} else if (m_centerpos > m_target - 1) {
		Robot::DriveTrainsubsystem->Arcade(-m_forwardspeed, m_adjustment);
	} else if (m_centerpos < m_target + 1) {
		Robot::DriveTrainsubsystem->Arcade(-m_forwardspeed, -m_adjustment);
bool DriveGearV::IsFinished() {
	return m_finish || IsTimedOut();
void DriveGearV::End() {
	Robot::DriveTrainsubsystem->Arcade(0, 0);
void DriveGearV::Interrupted() {

Hope this helps!

If you haven’t seen it yet - - would specifically speak to the “Controlling the robot” part of your question.

Thanks for the responses! I think I will try some of the methods that both of you have suggested. Also, when you programed your vision code, how exactly did you include the GRIP Generated code into your program/roborio, and if you used something else, what exactly was that and how did you use it? Thanks!

Instead of running the vision processing on the Rio, I have the contour reports from GRIP wrote to an NTTable on my driver station and having the RoboRio read it. This process slows processing down as images have to go from the camera on the robot to the laptop and the data has to go back, which was why we experimented with a NVIDIA Jetson TX1 onboard to do the processing. We didn’t experience terrible lagtime running processing on the laptop though.

1319 used GRIP to generate a Java pipeline that we linked into a Java program built using Gradle. We deployed that program to a Raspberry Pi 3, to which two Microsoft USB cameras were attached. We processed the USB cameras into a MJPEG feed using mjpg_streamer, and our Java vision program read one of those feeds and updated Network Tables.

The Vision subsystem in our C++ robot program read these network tables values and commands could use this information to decide how to steer the robot.

Our SmartDashboard displayed both mpeg streams for driver assist.

Do you, by chance, know exactly how your team linked the Java pipeline into the Gradle program? Sorry if I sound like I am asking a lot of questions, but our team does not have a programming mentor, so we are just learning by ourselves. Would it be easier for our team to do this immediately, or should we try the method that maccopacco mentioned first by running it on the driver station or the robot itself? Thanks.

Driving to a target will depend very strongly on your drive train. Because getting to the peg depends more on the trajectory your robot can take than the actual vision processing. What do I mean?

281 did our vision processing using opencv on a RaspPi this year. It posted the estimated distance and left/right offset to NetworkTables for use by our drive code. So far so good.

Now we had Mechanum drive so we could control yaw, forward/backward, and left/right independently of each other. Yaw was controlled by the gyro and the direction depended on the peg we were aiming for. The driver/autonomous controlled the forward speed. For left/right, we plugged the left/right distance into a PID controller whose output was the left/right joystick value for the RobotDrive. By the end of the season, we could pretty reliably load a gear on the side pegs, and our driver was even using the auto left/right in Teleop mode. Conceptually pretty simple because of the independence of left/right motion.

If you have a tank drive, you’ll need more drive logic than that since you’ll need to make S-turns in order to get aligned properly (turn “too far” to one side, drive straight for a distance, and turn back to the target). If you don’t, you won’t approach the peg “straight-on” at the end of your drive trajectory. To make matters worse, the initial turn can cause you to lose sight of the target, so you might not be able to use the vision to trigger your turn back.

Anyway, think about what motion your robot is capable of and plan/program accordingly.