I made a post about a week ago asking for review of my vision code. I got some good suggestions about using GRIPS generated code since that’s much faster, and I think I have sorted out my logic to account for distance. The only thing I’ve noticed is that this only works for one spot on the field, I believe (if it were to even work at all). Watching videos of team highlights with vision tracking, they’re shooting from any position that works. I’m baffled by the math behind that, it does not make sense to me. Here is my code, if anyone could explain how shooting from any place on the field works that would be awesome.
Assume I have a auto generated GRIP class named VisionClass.
package org.usfirst.frc.team6077.robot;
import org.opencv.core.Rect;
import org.opencv.imgproc.Imgproc;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.RobotDrive.MotorType;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.VictorSP;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.vision.VisionThread;
public class Robot extends SampleRobot {
VictorSP leftFront, leftBack, rightFront, rightBack, shooterPaddle1, shooterPaddle2;
Spark shooter, shooter2;
GyroBase gyro = new ADXRS450_Gyro();
RobotDrive drive;
Joystick logitech = new Joystick(0);
SendableChooser<String> chooser = new SendableChooser<>();
static double kP = 0.03;
static final String highGoal = "High Goal";
static final String gear = "Gear";
double getAngle = gyro.getAngle();
// Vision Processing
private VisionThread visionThread;
private final Object imgLock = new Object();
double centerX = 0.0;
double width = 0.0;
public String shotReady = "Shot Ready";
int gripTolerance = 3;
int distanceBack = 90;
int distanceFront = 110;
int camWidth = 320;
int camHeight = 240;
int imageCenter = (camWidth / 2);
boolean isShotReady = false;
@Override
public void robotInit() {
UsbCamera camera = CameraServer.getInstance().startAutomaticCapture("cam0", "cam0");
camera.setResolution(camWidth, camHeight);
visionThread = new VisionThread(camera, new VisionClass(), pipeline -> {
if (!pipeline.filterContoursOutput().isEmpty()) {
Rect r = Imgproc.boundingRect(pipeline.filterContoursOutput().get(0));
synchronized (imgLock) {
centerX = r.x + (r.width / 2);
width = r.width;
}
}
});
visionThread.start();
chooser.addDefault(highGoal, highGoal);
chooser.addDefault(gear, gear);
shooter = new Spark(0);
shooter2 = new Spark(5);
leftFront = new VictorSP(1);
leftBack = new VictorSP(2);
rightFront = new VictorSP(3);
rightBack = new VictorSP(4);
drive = new RobotDrive(leftFront, rightFront, leftBack, rightBack);
drive.setInvertedMotor(MotorType.kFrontLeft, true);
drive.setInvertedMotor(MotorType.kRearLeft, true);
drive.setExpiration(0.1);
gyro.calibrate();
SmartDashboard.putData("Auto modes", chooser);
this.updateDashboard();
}
@Override
public void autonomous() {
drive.setSafetyEnabled(false);
String autoSelected = chooser.getSelected();
System.out.println("Auto selected: " + autoSelected);
gyro.reset();
while (isAutonomous() && isEnabled()) {
if (autoSelected == gear) {
double angle = gyro.getAngle();
double negangle = gyro.getAngle() * kP;
SmartDashboard.putNumber("Gyro Angle", angle);
SmartDashboard.putNumber("Neg ANgle", negangle);
drive.mecanumDrive_Cartesian(-0.2, 0, negangle, 0);
}
}
}
@Override
public void operatorControl() {
drive.setSafetyEnabled(true);
gyro.reset();
drive.setMaxOutput(0.75);
int reset = 0;
while (isOperatorControl() && isEnabled()) {
double gyroReduced = gyro.getAngle() * kP;
SmartDashboard.putNumber("gyro.getAngle() * kP", gyroReduced);
SmartDashboard.putNumber("Shooter Speed", shooter.getSpeed());
if (logitech.getRawButton(6)) {
/*
* for (reset=0; reset<1; reset++ ) { gyro.reset(); }
*/
if (reset < 1) {
gyro.reset();
reset++;
System.out.println("Success! Gyro was reset.");
} else {
System.out.println("Failed to reset gyro due to int reset >= 1");
}
if (Math.abs(logitech.getRawAxis(1)) > 0.15) {
drive.mecanumDrive_Cartesian(logitech.getRawAxis(1), 0, gyroReduced, 0);
} else if (Math.abs(logitech.getRawAxis(0)) > 0.15) {
drive.mecanumDrive_Cartesian(0, logitech.getRawAxis(0), gyroReduced, 0);
}
} else {
reset = 0;
if (Math.abs(logitech.getRawAxis(1)) > 0.15 || Math.abs(logitech.getRawAxis(0)) > 0.15
|| Math.abs(logitech.getRawAxis(2)) > 0.15) {
drive.mecanumDrive_Cartesian(logitech.getRawAxis(1), logitech.getRawAxis(0),
-logitech.getRawAxis(2) * 0.5, 0);
}
}
if (logitech.getRawButton(9)) {
shooter.set(1);
shooter2.set(1);
} else if (logitech.getRawButton(10)) {
shooter.set(-1);
shooter2.set(-1);
} else if (logitech.getRawButton(11)) {
shooter.set(0.5);
shooter2.set(0.5);
} else if (logitech.getRawButton(12)) {
shooter.set(0.75);
shooter2.set(0.75);
} else {
shooter.set(0);
shooter2.set(0);
}
if (logitech.getRawButton(4)) {
this.lineUpStraight();
} else if (logitech.getRawButton(5)) {
this.lineUpSide();
}
/*
* if (logitech.getRawButton(1)) { shooter.set(0.5325); } else if
* (logitech.getRawButton(2)) { shooter.set(0.45); } else if
* (logitech.getRawButton(3)) { shooter.set(0.50); } else if
* (logitech.getRawButton(4)) { shooter.set(0.51); } else if
* (logitech.getRawButton(7)) { shooter.set(0.53); } else if
* (logitech.getRawButton(8)) { shooter.set(0.535); } else {
* shooter.set(0); }
*/
Timer.delay(0.005);
}
}
// Check to see if the target is in sight. If the target is visible, line up
// the robot.
// Line up straight from the target.
public void lineUpStraight() {
this.updateDashboard();
if (!isShotReady) {
// Line up logic; if the shot is not lined up, true.
while (!isShotReady) {
this.updateDashboard();
double turn = centerX - imageCenter;
drive.mecanumDrive_Cartesian(0.1, 0, turn * 0.005, 0);
if (width > distanceFront) {
drive.mecanumDrive_Cartesian(-width * 0.0025, 0, 0, 0);
} else if (width < distanceBack) {
drive.mecanumDrive_Cartesian(width * 0.005, 0, 0, 0);
}
}
} else {
this.updateDashboard();
}
}
public void lineUpSide() {
// TODO: code
}
public void updateDashboard() {
try {
double centerX;
double width;
synchronized (imgLock) {
centerX = this.centerX;
width = this.width;
}
if ((centerX + gripTolerance >= imageCenter || centerX - gripTolerance <= imageCenter)
&& (width < distanceFront || width > distanceBack)) {
isShotReady = true;
SmartDashboard.putString(shotReady, "true");
} else {
isShotReady = false;
SmartDashboard.putString(shotReady, "false");
}
} catch (Exception e) {
SmartDashboard.putString(shotReady, "no target found");
}
}
@Override
public void test() {
}
}
Thanks!