Hello. I’m new to FRC, and as a mentor for Team SPORK - 3196. I am attempting a lesson in using AprilTags using Romi’s with Pi camera’s, with 3d PhotonVision calibrated AprilTag pipeline.
I am able to target an AprilTag in 3d on Photonlib (10.0.0.2:5800). However, I am unable to use the target data from the Photon Camera.
Ask: How does one use PhotonCamera data of a targeted AprilTag using Romi Timed Robot template?
Code sample below:
public class Robot extends TimedRobot {
private static final boolean runCameraTest = true;
private static final String kDefaultAuto = “Default”;
private static final String kCustomAuto = “My Auto”;
private String m_autoSelected;
private final SendableChooser m_chooser = new SendableChooser<>();
private final RomiDrivetrain m_drivetrain = new RomiDrivetrain();
// Constants such as camera and target height stored. Change per robot and goal!
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
final double TARGET_HEIGHT_METERS = Units.feetToMeters(5);
// Angle between horizontal and the camera.
final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0);
// How far from the target we want to be
final double GOAL_RANGE_METERS = Units.feetToMeters(3);
// Change this to match the name of your camera
//PhotonCamera camera = new PhotonCamera("phtonvision");
PhotonCamera camera = new PhotonCamera("photonvision");
// PID constants should be tuned per robot
final double LINEAR_P = 0.1;
final double LINEAR_D = 0.0;
PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
final double ANGULAR_P = 0.1;
final double ANGULAR_D = 0.0;
PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
//XboxController xboxController = new XboxController(0);
PS4Controller ps4Controller = new PS4Controller(1);
/**
- This function is run when the robot is first started up and should be used for any
- initialization code.
*/
@Override
public void robotInit() {
m_chooser.setDefaultOption(“Default Auto”, kDefaultAuto);
m_chooser.addOption(“My Auto”, kCustomAuto);
PhotonCamera.setVersionCheckEnabled(false);
//Get the default instance of NetworkTables that was created automatically
//when your program starts
camera.setDriverMode(false);
}
/**
-
This function is called every robot packet, no matter the mode. Use this for items like
-
diagnostics that you want ran during disabled, autonomous, teleoperated and test.
-
This runs after the mode specific periodic functions, but before LiveWindow and
-
SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
double forwardSpeed;
double rotationSpeed;forwardSpeed = -ps4Controller.getRightY();
//if (xboxController.getAButton()) {
if (ps4Controller.getCrossButton() && runCameraTest) {
// Vision-alignment mode
// Query the latest result from PhotonVision
var result = camera.getLatestResult();if (result.hasTargets()) { // Calculate angular turn power // -1.0 required to ensure positive PID controller effort _increases_ yaw rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0); } else { // If we have no targets, stay still. rotationSpeed = 0; }} else {
// Manual Driver Mode
//rotationSpeed = xboxController.getLeftX();
rotationSpeed = ps4Controller.getLeftX();
}// Use our forward/turn speeds to control the drivetrain
m_drivetrain.arcadeDrive(forwardSpeed, rotationSpeed);//let’s follow the health of the romi
SmartDashboard.putNumber("Drivetrain Left Enoder: ", m_drivetrain.getLeftDistanceInch());
SmartDashboard.putNumber("Drivetrain Left Enoder: ", m_drivetrain.getRightDistanceInch());
SmartDashboard.putString("PhotonVision Has Image: ", camera.toString());
SmartDashboard.putNumber("PhotonVision Pipeline Index: ", (double)camera.getPipelineIndex());
}
/**
- This autonomous (along with the chooser code above) shows how to select between different
- autonomous modes using the dashboard. The sendable chooser code works with the Java
- SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
- uncomment the getString line to get the auto name from the text box below the Gyro
-
You can add additional auto modes by adding additional comparisons to the switch structure
- below with additional strings. If using the SendableChooser make sure to add them to the
- chooser code above as well.
*/
@Override
public void autonomousInit() {
m_autoSelected = m_chooser.getSelected();
// m_autoSelected = SmartDashboard.getString(“Auto Selector”, kDefaultAuto);
System.out.println("Auto selected: " + m_autoSelected);
m_drivetrain.resetEncoders();
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {
switch (m_autoSelected) {
case kCustomAuto:
// Put custom auto code here
break;
case kDefaultAuto:
default:
// Put default auto code here
break;
}
}
/** This function is called once when teleop is enabled. */
@Override
public void teleopInit() {if(runCameraTest){this.cameraTest();}}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
/** This function is called once when the robot is disabled. */
@Override
public void disabledInit() {if(runCameraTest){this.cameraTest();}}
/** This function is called periodically when disabled. */
@Override
public void disabledPeriodic() {}
/** This function is called once when test mode is enabled. */
@Override
public void testInit() {if(runCameraTest){this.cameraTest();}}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {
if(runCameraTest){this.cameraTest();}
}
private void cameraTest(){
PhotonPipelineResult photonPipelineResult = camera.getLatestResult();
//let’s follow the health of the romi
SmartDashboard.putNumber("Drivetrain Left Enoder: ", m_drivetrain.getLeftDistanceInch());
SmartDashboard.putNumber("Drivetrain Left Enoder: ", m_drivetrain.getRightDistanceInch());
SmartDashboard.putString("PhotonVision Has Image: ", camera.toString());
SmartDashboard.putNumber("PhotonVision Pipeline Index: ", (double)camera.getPipelineIndex());
if (photonPipelineResult.hasTargets()){
var bestTarget = photonPipelineResult.getBestTarget();
SmartDashboard.putNumber("Area: ", bestTarget.getArea());
SmartDashboard.putNumber("Fidicual ID: ", (double)bestTarget.getFiducialId());
SmartDashboard.putNumber("Pitch: ", bestTarget.getPitch());
SmartDashboard.putNumber("Pose: ", bestTarget.getPoseAmbiguity());
SmartDashboard.putNumber("Skew: ", bestTarget.getSkew());
SmartDashboard.putNumber("Yaw: ", bestTarget.getYaw());
SmartDashboard.putString("Corners: ", bestTarget.getCorners().toString());
SmartDashboard.putString("Separator", "---------------------------------------------");
}
}
}