PID Subsystem -- Align With Tote

[Ozuru]

Hello,

I've been working on creating a command to align myself with a tote in autonomous. I'm finding the tote find but I'm looking into making my code more efficient and was thinking a PID subsystem (or command?) might help.

At the moment, here's basically what I'm doing: I'm isolating the tote and getting the pixel offset from the center. In the execute of my command (not PID command -- just a regular command) I'm checking to see the value of that number. If it's less than 25 pixels off it drives forward, if not, it turns the appropriate direction to make the center of the image be aligned with the centroid of the tote.

In my isFinished, I'm returning this:

// returning if we are less than 20 inches and within 20 pixels, in either direction, of the centroid of the tote
return Math.abs(distanceToCenter) < 20 && Robot._driveTrain.getDistanceIn() <= 20;

Would a PID subsystem be better suited for this? How would I go about a speed-based approach? At the moment it is pausing and turning and I feel it would be way more efficient to simply vary the speed to the motors proportionally but am unsure where to star.

Thanks guys.

[cstelter]

We did something similar to drive to the low goal 2 years ago. I think the best avenue is a PID command although you may only need to apply P.

We had a gyro and had the PID follow a setpoint while driving forward.

returnPIDInput() {
    return gyro.getAngle();
}

usePIDOutput(double output) {
    driveTrain.tankDrive(m_speed+output, m_speed-output);
}

So you begin driving at some intended speed (say 0.5) and an angle you want to maintain (say 0.0).

Eventually you get a picture that says you're 50 pixels off center and perhaps that calculates to a new angle of +10 degrees, so in your execute() of your PIDCommand, you observe this and do setSetpoint(10) (change to 10 degrees) and your pid starts applying the resulting output incrementally above your base speed to rotate your robot while it continues to move forward.

The picture data constantly updates the angle.

We haven't done vision yet this year (many new students-- way behind)-- but the above will only drive you to the center of the tote. when you get there, you may be at an angle to the tote, depending on your starting point, etc. but if you know you are pretty much lined up to begin with the above should keep you on target.

Generally the gyro data comes a lot quicker than the camera data which is why we chose to drive along a gyro angle and update tangle relative to the gyro as often as we acquired a new picture.

[Ozuru]

Originally Posted by cstelter

We did something similar to drive to the low goal 2 years ago. I think the best avenue is a PID command although you may only need to apply P.

We had a gyro and had the PID follow a setpoint while driving forward.

returnPIDInput() {
    return gyro.getAngle();
}

usePIDOutput(double output) {
    driveTrain.tankDrive(m_speed+output, m_speed-output);
}

So you begin driving at some intended speed (say 0.5) and an angle you want to maintain (say 0.0).

Eventually you get a picture that says you're 50 pixels off center and perhaps that calculates to a new angle of +10 degrees, so in your execute() of your PIDCommand, you observe this and do setSetpoint(10) (change to 10 degrees) and your pid starts applying the resulting output incrementally above your base speed to rotate your robot while it continues to move forward.

The picture data constantly updates the angle.

We haven't done vision yet this year (many new students-- way behind)-- but the above will only drive you to the center of the tote. when you get there, you may be at an angle to the tote, depending on your starting point, etc. but if you know you are pretty much lined up to begin with the above should keep you on target.

Generally the gyro data comes a lot quicker than the camera data which is why we chose to drive along a gyro angle and update tangle relative to the gyro as often as we acquired a new picture.

Thank you for all of that. If you want help with vision tracking this year, let me know. The gyroscope angle is an interesting idea -- it seems this competition that it may just be easier to use a gyro for that.