Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update to match new WPILib organization

Browse Source
This commit is contained in:
Gold856
2025-12-29 16:16:56 -05:00
committed by samfreund
parent c34c854583
commit 934eed21d2
264 changed files with 3440 additions and 3299 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java
View File

@@ -24,18 +24,18 @@
package frc.robot;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N3;
import org.wpilib.math.util.Units;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.apriltag.AprilTagFields;
public class Constants {
public static class Vision {

2
photonlib-java-examples/aimandrange/src/main/java/frc/robot/Main.java
View File

@@ -24,7 +24,7 @@
package frc.robot;
import edu.wpi.first.wpilibj.RobotBase;
import org.wpilib.opmode.RobotBase;
public final class Main {
private Main() {}

16
photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java
View File

@@ -26,17 +26,17 @@ package frc.robot;
import static frc.robot.Constants.Vision.*;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.simulation.BatterySim;
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.subsystems.drivetrain.SwerveDrive;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonUtils;
import org.wpilib.driverstation.XboxController;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.util.Units;
import org.wpilib.opmode.TimedRobot;
import org.wpilib.simulation.BatterySim;
import org.wpilib.simulation.RoboRioSim;
import org.wpilib.smartdashboard.SmartDashboard;
public class Robot extends TimedRobot {
private SwerveDrive drivetrain;

6
photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java
View File

@@ -26,13 +26,13 @@ package frc.robot;
import static frc.robot.Constants.Vision.*;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import org.photonvision.PhotonCamera;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.smartdashboard.Field2d;
public class VisionSim {
// Simulation

30
photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java
View File

@@ -26,22 +26,22 @@ package frc.robot.subsystems.drivetrain;
import static frc.robot.Constants.Swerve.*;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
import org.wpilib.hardware.imu.OnboardIMU;
import org.wpilib.hardware.imu.OnboardIMU.MountOrientation;
import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.*;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.smartdashboard.SmartDashboard;
public class SwerveDrive {
// Construct the swerve modules with their respective constants.

53
photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java
View File

@@ -24,27 +24,26 @@
package frc.robot.subsystems.drivetrain;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.RobotController;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.Discretization;
import org.wpilib.math.system.LinearSystem;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.util.Nat;
import org.wpilib.system.RobotController;
/**
* This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users
@@ -55,8 +54,8 @@ import java.util.Random;
* <p>In this class, distances are expressed with meters, angles with radians, and inputs with
* voltages.
*
* <p>Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on
* the Sim GUI's field.
* <p>Teams can use {@link org.wpilib.smartdashboard.Field2d} to visualize their robot on the Sim
* GUI's field.
*/
public class SwerveDriveSim {
private final LinearSystem<N2, N1, N2> drivePlant;
@@ -201,7 +200,7 @@ public class SwerveDriveSim {
final double battVoltage = RobotController.getBatteryVoltage();
for (int i = 0; i < driveInputs.length; i++) {
double input = inputs[i];
driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage);
driveInputs[i] = Math.clamp(input, -battVoltage, battVoltage);
}
}
@@ -214,7 +213,7 @@ public class SwerveDriveSim {
final double battVoltage = RobotController.getBatteryVoltage();
for (int i = 0; i < steerInputs.length; i++) {
double input = inputs[i];
steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage);
steerInputs[i] = Math.clamp(input, -battVoltage, battVoltage);
}
}
@@ -236,7 +235,7 @@ public class SwerveDriveSim {
// input required to make next state vel == 0
double inputToStop = nextStateVel / -discB.get(1, 0);
// ks effect on system velocity
double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks);
double ksSystemEffect = Math.clamp(inputToStop, -ks, ks);
// after ks system effect:
nextStateVel += discB.get(1, 0) * ksSystemEffect;
@@ -248,13 +247,13 @@ public class SwerveDriveSim {
// system velocity was reduced to 0, resist any input
if (Math.abs(ksSystemEffect) < ks) {
double absInput = Math.abs(input);
ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput);
ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput);
}
// non-zero system velocity, but input causes velocity sign change. Resist input after sign
// change
else if ((input * signToStop) > (inputToStop * signToStop)) {
double absInput = Math.abs(input - inputToStop);
ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput);
ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput);
}
// calculate next x including static friction
@@ -446,8 +445,8 @@ public class SwerveDriveSim {
DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) {
double effVolts = inputVolts - radiansPerSec / motor.Kv;
// ignore regeneration
if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts);
else effVolts = MathUtil.clamp(effVolts, inputVolts, 0);
if (inputVolts >= 0) effVolts = Math.clamp(effVolts, 0, inputVolts);
else effVolts = Math.clamp(effVolts, inputVolts, 0);
// calculate battery current
return (inputVolts / battVolts) * (effVolts / motor.R);
}

22
photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java
View File

@@ -26,17 +26,17 @@ package frc.robot.subsystems.drivetrain;
import static frc.robot.Constants.Swerve.*;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.wpilib.hardware.motor.PWMSparkMax;
import org.wpilib.hardware.rotation.Encoder;
import org.wpilib.math.controller.PIDController;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.util.MathUtil;
import org.wpilib.math.util.Units;
import org.wpilib.simulation.EncoderSim;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.RobotController;
public class SwerveModule {
// --- Module Constants

24
photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java
View File

@@ -24,18 +24,18 @@
package frc.robot;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N3;
import org.wpilib.math.util.Units;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.apriltag.AprilTagFields;
public class Constants {
public static class Vision {

2
photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java
View File

@@ -24,7 +24,7 @@
package frc.robot;
import edu.wpi.first.wpilibj.RobotBase;
import org.wpilib.opmode.RobotBase;
public final class Main {
private Main() {}

14
photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java
View File

@@ -26,15 +26,15 @@ package frc.robot;
import static frc.robot.Constants.Vision.*;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.simulation.BatterySim;
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.subsystems.drivetrain.SwerveDrive;
import org.photonvision.PhotonCamera;
import org.wpilib.driverstation.XboxController;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.opmode.TimedRobot;
import org.wpilib.simulation.BatterySim;
import org.wpilib.simulation.RoboRioSim;
import org.wpilib.smartdashboard.SmartDashboard;
public class Robot extends TimedRobot {
private SwerveDrive drivetrain;

6
photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java
View File

@@ -26,13 +26,13 @@ package frc.robot;
import static frc.robot.Constants.Vision.*;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import org.photonvision.PhotonCamera;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.smartdashboard.Field2d;
public class VisionSim {
// Simulation

30
photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java
View File

@@ -26,22 +26,22 @@ package frc.robot.subsystems.drivetrain;
import static frc.robot.Constants.Swerve.*;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
import org.wpilib.hardware.imu.OnboardIMU;
import org.wpilib.hardware.imu.OnboardIMU.MountOrientation;
import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.*;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.smartdashboard.SmartDashboard;
public class SwerveDrive {
// Construct the swerve modules with their respective constants.

53
photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java
View File

@@ -24,27 +24,26 @@
package frc.robot.subsystems.drivetrain;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.RobotController;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.Discretization;
import org.wpilib.math.system.LinearSystem;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.util.Nat;
import org.wpilib.system.RobotController;
/**
* This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users
@@ -55,8 +54,8 @@ import java.util.Random;
* <p>In this class, distances are expressed with meters, angles with radians, and inputs with
* voltages.
*
* <p>Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on
* the Sim GUI's field.
* <p>Teams can use {@link org.wpilib.smartdashboard.Field2d} to visualize their robot on the Sim
* GUI's field.
*/
public class SwerveDriveSim {
private final LinearSystem<N2, N1, N2> drivePlant;
@@ -201,7 +200,7 @@ public class SwerveDriveSim {
final double battVoltage = RobotController.getBatteryVoltage();
for (int i = 0; i < driveInputs.length; i++) {
double input = inputs[i];
driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage);
driveInputs[i] = Math.clamp(input, -battVoltage, battVoltage);
}
}
@@ -214,7 +213,7 @@ public class SwerveDriveSim {
final double battVoltage = RobotController.getBatteryVoltage();
for (int i = 0; i < steerInputs.length; i++) {
double input = inputs[i];
steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage);
steerInputs[i] = Math.clamp(input, -battVoltage, battVoltage);
}
}
@@ -236,7 +235,7 @@ public class SwerveDriveSim {
// input required to make next state vel == 0
double inputToStop = nextStateVel / -discB.get(1, 0);
// ks effect on system velocity
double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks);
double ksSystemEffect = Math.clamp(inputToStop, -ks, ks);
// after ks system effect:
nextStateVel += discB.get(1, 0) * ksSystemEffect;
@@ -248,13 +247,13 @@ public class SwerveDriveSim {
// system velocity was reduced to 0, resist any input
if (Math.abs(ksSystemEffect) < ks) {
double absInput = Math.abs(input);
ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput);
ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput);
}
// non-zero system velocity, but input causes velocity sign change. Resist input after sign
// change
else if ((input * signToStop) > (inputToStop * signToStop)) {
double absInput = Math.abs(input - inputToStop);
ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput);
ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput);
}
// calculate next x including static friction
@@ -446,8 +445,8 @@ public class SwerveDriveSim {
DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) {
double effVolts = inputVolts - radiansPerSec / motor.Kv;
// ignore regeneration
if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts);
else effVolts = MathUtil.clamp(effVolts, inputVolts, 0);
if (inputVolts >= 0) effVolts = Math.clamp(effVolts, 0, inputVolts);
else effVolts = Math.clamp(effVolts, inputVolts, 0);
// calculate battery current
return (inputVolts / battVolts) * (effVolts / motor.R);
}

22
photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java
View File

@@ -26,17 +26,17 @@ package frc.robot.subsystems.drivetrain;
import static frc.robot.Constants.Swerve.*;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.wpilib.hardware.motor.PWMSparkMax;
import org.wpilib.hardware.rotation.Encoder;
import org.wpilib.math.controller.PIDController;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.util.MathUtil;
import org.wpilib.math.util.Units;
import org.wpilib.simulation.EncoderSim;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.RobotController;
public class SwerveModule {
// --- Module Constants

4
photonlib-java-examples/aimattarget/src/test/java/frc/robot/JniLoadTest.java
View File

@@ -26,14 +26,14 @@ package frc.robot;
import static org.junit.jupiter.api.Assertions.fail;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import java.util.List;
import org.junit.jupiter.api.Test;
import org.photonvision.PhotonCamera;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.timesync.TimeSyncSingleton;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Rotation2d;
public class JniLoadTest {
@Test

24
photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java
View File

@@ -24,18 +24,18 @@
package frc.robot;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N3;
import org.wpilib.math.util.Units;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.apriltag.AprilTagFields;
public class Constants {
public static class Vision {

2
photonlib-java-examples/poseest/src/main/java/frc/robot/Main.java
View File

@@ -24,7 +24,7 @@
package frc.robot;
import edu.wpi.first.wpilibj.RobotBase;
import org.wpilib.opmode.RobotBase;
public final class Main {
private Main() {}

16
photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java
View File

@@ -24,16 +24,16 @@
package frc.robot;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.simulation.BatterySim;
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import frc.robot.subsystems.GamepieceLauncher;
import frc.robot.subsystems.drivetrain.SwerveDrive;
import org.wpilib.driverstation.XboxController;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.opmode.TimedRobot;
import org.wpilib.simulation.BatterySim;
import org.wpilib.simulation.RoboRioSim;
public class Robot extends TimedRobot {
private SwerveDrive drivetrain;

20
photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
View File

@@ -26,13 +26,6 @@ package frc.robot;
import static frc.robot.Constants.Vision.*;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import java.util.List;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
@@ -42,6 +35,13 @@ import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N3;
import org.wpilib.smartdashboard.Field2d;
public class Vision {
private final PhotonCamera camera;
@@ -55,7 +55,7 @@ public class Vision {
/**
* @param estConsumer Lamba that will accept a pose estimate and pass it to your desired {@link
* edu.wpi.first.math.estimator.SwerveDrivePoseEstimator}
* org.wpilib.math.estimator.SwerveDrivePoseEstimator}
*/
public Vision(EstimateConsumer estConsumer) {
this.estConsumer = estConsumer;
@@ -167,8 +167,8 @@ public class Vision {
/**
* Returns the latest standard deviations of the estimated pose from {@link
* #getEstimatedGlobalPose()}, for use with {@link
* edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
* only be used when there are targets visible.
* org.wpilib.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should only
* be used when there are targets visible.
*/
public Matrix<N3, N1> getEstimationStdDevs() {
return curStdDevs;

17
photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java
View File

@@ -24,14 +24,13 @@
package frc.robot.subsystems;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.wpilib.hardware.motor.PWMSparkMax;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.util.Units;
import org.wpilib.simulation.FlywheelSim;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.RobotController;
public class GamepieceLauncher {
private final PWMSparkMax motor;
@@ -52,7 +51,7 @@ public class GamepieceLauncher {
public void periodic() {
double maxRPM = Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeed);
curMotorCmd = curDesSpd / maxRPM;
curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0);
curMotorCmd = Math.clamp(curMotorCmd, 0.0, 1.0);
motor.set(curMotorCmd);
SmartDashboard.putNumber("GPLauncher Des Spd (RPM)", curDesSpd);

30
photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java
View File

@@ -26,22 +26,22 @@ package frc.robot.subsystems.drivetrain;
import static frc.robot.Constants.Swerve.*;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
import org.wpilib.hardware.imu.OnboardIMU;
import org.wpilib.hardware.imu.OnboardIMU.MountOrientation;
import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.*;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.smartdashboard.SmartDashboard;
public class SwerveDrive {
// Construct the swerve modules with their respective constants.

53
photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java
View File

@@ -24,27 +24,26 @@
package frc.robot.subsystems.drivetrain;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.RobotController;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.Discretization;
import org.wpilib.math.system.LinearSystem;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.util.Nat;
import org.wpilib.system.RobotController;
/**
* This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users
@@ -55,8 +54,8 @@ import java.util.Random;
* <p>In this class, distances are expressed with meters, angles with radians, and inputs with
* voltages.
*
* <p>Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on
* the Sim GUI's field.
* <p>Teams can use {@link org.wpilib.smartdashboard.Field2d} to visualize their robot on the Sim
* GUI's field.
*/
public class SwerveDriveSim {
private final LinearSystem<N2, N1, N2> drivePlant;
@@ -201,7 +200,7 @@ public class SwerveDriveSim {
final double battVoltage = RobotController.getBatteryVoltage();
for (int i = 0; i < driveInputs.length; i++) {
double input = inputs[i];
driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage);
driveInputs[i] = Math.clamp(input, -battVoltage, battVoltage);
}
}
@@ -214,7 +213,7 @@ public class SwerveDriveSim {
final double battVoltage = RobotController.getBatteryVoltage();
for (int i = 0; i < steerInputs.length; i++) {
double input = inputs[i];
steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage);
steerInputs[i] = Math.clamp(input, -battVoltage, battVoltage);
}
}
@@ -236,7 +235,7 @@ public class SwerveDriveSim {
// input required to make next state vel == 0
double inputToStop = nextStateVel / -discB.get(1, 0);
// ks effect on system velocity
double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks);
double ksSystemEffect = Math.clamp(inputToStop, -ks, ks);
// after ks system effect:
nextStateVel += discB.get(1, 0) * ksSystemEffect;
@@ -248,13 +247,13 @@ public class SwerveDriveSim {
// system velocity was reduced to 0, resist any input
if (Math.abs(ksSystemEffect) < ks) {
double absInput = Math.abs(input);
ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput);
ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput);
}
// non-zero system velocity, but input causes velocity sign change. Resist input after sign
// change
else if ((input * signToStop) > (inputToStop * signToStop)) {
double absInput = Math.abs(input - inputToStop);
ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput);
ksInputEffect = -Math.clamp(ks * inputSign, -absInput, absInput);
}
// calculate next x including static friction
@@ -446,8 +445,8 @@ public class SwerveDriveSim {
DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) {
double effVolts = inputVolts - radiansPerSec / motor.Kv;
// ignore regeneration
if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts);
else effVolts = MathUtil.clamp(effVolts, inputVolts, 0);
if (inputVolts >= 0) effVolts = Math.clamp(effVolts, 0, inputVolts);
else effVolts = Math.clamp(effVolts, inputVolts, 0);
// calculate battery current
return (inputVolts / battVolts) * (effVolts / motor.R);
}

22
photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java
View File

@@ -26,17 +26,17 @@ package frc.robot.subsystems.drivetrain;
import static frc.robot.Constants.Swerve.*;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.wpilib.hardware.motor.PWMSparkMax;
import org.wpilib.hardware.rotation.Encoder;
import org.wpilib.math.controller.PIDController;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.util.MathUtil;
import org.wpilib.math.util.Units;
import org.wpilib.simulation.EncoderSim;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.RobotController;
public class SwerveModule {
// --- Module Constants