Hey what’s up guys, we were testing vision alignment to the gear peg when we moved the command into autonomous from teleop. When we ran the code the center of the tape on the camera calculated from the grip pipeline was always 0, causing the PID loop to fail. The strange thing is that this works fine in teleop but not autonomous. The code for the vision command is below:
public class AdjustRotationToTarget extends PIDCommand {
public static double kp = 0.85;
public static double ki = 0.02;
public static double kd = 0.001;
private boolean buttonLastPressed = false;
public static final int IMG_WIDTH = 320;
public static final int IMG_HEIGHT = 240;
private double centerX = 0.0;
private final Object imgLock = new Object();
private UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(0);
private VisionThread visionThread;
public AdjustRotationToTarget() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
super(kp, ki, kd);
requires(Robot.swerveBase);
getPIDController().setContinuous(true);
setSetpoint(0.5);
}
// Called just before this Command runs the first time
protected void initialize() {
//rotationTarget.clearValues();
Robot.swerveBase.setZero();
setSetpoint(0.5);
camera.setExposureManual(3);
camera.setResolution(IMG_WIDTH, IMG_HEIGHT);
visionThread = new VisionThread(this.camera, new GripPipeline(), pipeline -> {
//System.out.println(pipeline.filterContoursOutput().size());
if (pipeline.filterContoursOutput().size() == 2) {
synchronized (imgLock) {
Rect r = Imgproc.boundingRect(pipeline.filterContoursOutput().get(0));
Rect r2 = Imgproc.boundingRect(pipeline.filterContoursOutput().get(1));
centerX = ((r.x + (r.width))/2 + (r2.x + (r2.width))/2);
System.out.println(centerX);
this.centerX = centerX;
}
}
});
visionThread.start();
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
//camera.setExposureManual(3);
double centerX;
synchronized(imgLock){
centerX = this.centerX;
}
//System.out.println("p: " + kp + "; i: " + ki + "; d: " + kd + "; input: " + centerX/IMG_WIDTH);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return false;
//return Math.abs(error.addValue(getPIDController().getError())) < 0.10;
}
// Called once after isFinished returns true
protected void end() {
Robot.swerveBase.setZero();
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
}
@Override
protected double returnPIDInput() {
// TODO Auto-generated method stub
return centerX/IMG_WIDTH;
}
@Override
protected void usePIDOutput(double output) {
// TODO Auto-generated method stub
Robot.swerveBase.swerveDrive(0, 0, -output, false);
}
}