import frc.robot.util.ShooterPhysics.TurretState;
public class Superstructure extends Command {
- private Turret turret;
- private Drivetrain drivetrain;
- private Hood hood;
- private Shooter shooter;
- private Spindexer spindexer;
+ private Turret turret;
+ private Drivetrain drivetrain;
+ private Hood hood;
+ private Shooter shooter;
+ private Spindexer spindexer;
- private double turretSetpoint;
- private double hoodSetpoint;
+ private double turretSetpoint;
+ private double hoodSetpoint;
- private Pose2d drivepose;
+ private Pose2d drivepose;
- private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
- private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
-
- private Rotation2d lastTurretAngle;
- private Rotation2d turretAngle;
- private double turretVelocity;
+ private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
+ private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
- private double lastHoodAngle;
- private double hoodAngle;
- private double hoodVelocity;
+ private Rotation2d lastTurretAngle;
+ private Rotation2d turretAngle;
+ private double turretVelocity;
- private TurretState goalState;
+ private double lastHoodAngle;
+ private double hoodAngle;
+ private double hoodVelocity;
- private LoggedNetworkNumber phaseDelay = new LoggedNetworkNumber("/Tuning/OPERATOR/Phase Delay", 0.03); //Extrapolation delay due to latency
+ private TurretState goalState;
- private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d();
+ private LoggedNetworkNumber phaseDelay = new LoggedNetworkNumber("/Tuning/OPERATOR/Phase Delay", 0.03); // Extrapolation
+ // delay due
+ // to latency
- private PhaseManager phaseManager = new PhaseManager();
+ private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d();
- private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Hood Offset", 0.0);
+ private PhaseManager phaseManager = new PhaseManager();
- private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Turret Offet",0.0);
+ private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Hood Offset", 0.0);
- private double distanceFromTarget = 0.0;
- private LoggedNetworkNumber shuttlingTOFMultiplier = new LoggedNetworkNumber("/Tuning/OPERATOR/Shuttling TOF Multiplier",0.8);
+ private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Turret Offet", 0.0);
- // private double TOFAdjustment = 0.85;
- // private double TOFAdjustment = 1.1;
- private LoggedNetworkNumber TOFAdjustment = new LoggedNetworkNumber("/Tuning/OPERATOR/TOF Adjustment", 1.1);
+ private double distanceFromTarget = 0.0;
+ private LoggedNetworkNumber shuttlingTOFMultiplier = new LoggedNetworkNumber(
+ "/Tuning/OPERATOR/Shuttling TOF Multiplier", 0.8);
- public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
- this.turret = turret;
- this.drivetrain = drivetrain;
- this.hood = hood;
- this.shooter = shooter;
- this.spindexer = spindexer;
- drivepose = drivetrain.getPose();
+ // private double TOFAdjustment = 0.85;
+ // private double TOFAdjustment = 1.1;
+ private LoggedNetworkNumber TOFAdjustment = new LoggedNetworkNumber("/Tuning/OPERATOR/TOF Adjustment", 1.1);
- goalState = ShooterPhysics.getShotParams(
- Translation2d.kZero,
- FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
- 8.0); // Random initial goalState to prevent it being null
-
- addRequirements(turret, shooter);
- }
+ public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
+ this.turret = turret;
+ this.drivetrain = drivetrain;
+ this.hood = hood;
+ this.shooter = shooter;
+ this.spindexer = spindexer;
+ drivepose = drivetrain.getPose();
- public void updateSetpoints(Pose2d drivepose) {
- Pose2d turretPosition = drivepose.transformBy(
- new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero));
-
- // If the robot is moving, adjust the target position based on velocity
- ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
- ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
-
- // Rotational adjustment is not being used, since turret is in center of robot
- double turretVelocityX =
- fieldRelVel.vxMetersPerSecond;
- double turretVelocityY =
- fieldRelVel.vyMetersPerSecond;
-
- double timeOfFlight = 0;
- Pose2d lookaheadPose = turretPosition;
-
- /*
- * Loop (max 20) until lookaheadPose converges BECAUSE -->
- * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s)
- * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s
- * So we make a bunch of guesses until it converges
- * Early exit when change < 1mm to avoid unnecessary iterations
- */
- boolean shuttling = target.equals(FieldConstants.getHubTranslation().toTranslation2d());
- for (int i = 0; i < 20; i++) {
- Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
-
- Translation3d target3d = new Translation3d(target.getX(), target.getY(),
- shuttling ?
- FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub
-
- goalState = ShooterPhysics.getShotParams(
- Translation2d.kZero,
- target3d.minus(lookahead3d),
- 2.0);
-
- if (!shuttling) {
- timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get();
- } else {
- double distance = target.getDistance(lookaheadPose.getTranslation());
- timeOfFlight = distance * shuttlingTOFMultiplier.get();
- }
-
- double offsetX = turretVelocityX * timeOfFlight;
- double offsetY = turretVelocityY * timeOfFlight;
- Pose2d newLookaheadPose =
- new Pose2d(
- turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
- turretPosition.getRotation());
-
- // early exit if converged (change < 1mm)
- if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) {
- lookaheadPose = newLookaheadPose;
- break;
- }
- lookaheadPose = newLookaheadPose;
- }
-
- // Get the field angle relative to the target pose
- turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
- if (lastTurretAngle == null) {
- lastTurretAngle = turretAngle;
- }
-
- // Take the filtered average as the turret's velocity when robot is moving translationally
- turretVelocity =
- turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
-
- lastTurretAngle = turretAngle;
-
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Turret/Target Pose", target);
- Logger.recordOutput("Lookahead Pose", lookaheadPose);
- }
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putNumber("Time of flight", timeOfFlight);
- SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX);
- SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY);
- }
-
- // Subtract the rotational angle of the robot from the setpoint
- double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
-
- // Shortest path
- double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
- double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset.get();
-
- // Stay within physical limits -- if shortest path is past max angle, we go long way around
- if (potentialSetpoint > TurretConstants.MAX_ANGLE) {
- potentialSetpoint -= 360;
- } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) {
- potentialSetpoint += 360;
- }
-
- turretSetpoint = potentialSetpoint;
-
- // Pitch is in radians
- hoodAngle = goalState.pitch();
- hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
- hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
- lastHoodAngle = hoodAngle;
-
- distanceFromTarget = target.getDistance(lookaheadPose.getTranslation());
- Logger.recordOutput("Shooting/distanceToTarget", distanceFromTarget);
- }
+ goalState = ShooterPhysics.getShotParams(
+ Translation2d.kZero,
+ FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
+ 8.0); // Random initial goalState to prevent it being null
- public void updateDrivePose(){
- Pose2d currentPose = drivetrain.getPose();
-
- drivepose = new Pose2d(
- currentPose.getTranslation(),
- // Uncomment this if robot is backwards
- currentPose.getRotation()//.plus(new Rotation2d(Math.PI))
- );
- ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
-
- // Add a phase delay extrapolation component for latency delay
- drivepose = drivepose.exp(
- new Twist2d(
- robotRelVel.vxMetersPerSecond * phaseDelay.get(),
- robotRelVel.vyMetersPerSecond * phaseDelay.get(),
- robotRelVel.omegaRadiansPerSecond * phaseDelay.get()));
- }
+ addRequirements(turret, shooter);
+ }
+
+ public void updateSetpoints(Pose2d drivepose) {
+ Pose2d turretPosition = drivepose.transformBy(
+ new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero));
+
+ // If the robot is moving, adjust the target position based on velocity
+ ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
+ ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
+
+ // Rotational adjustment is not being used, since turret is in center of robot
+ double turretVelocityX = fieldRelVel.vxMetersPerSecond;
+ double turretVelocityY = fieldRelVel.vyMetersPerSecond;
+
+ double timeOfFlight = 0;
+ Pose2d lookaheadPose = turretPosition;
- /**
- * Stops and stows all subsystems involved in the command
+ /*
+ * Loop (max 20) until lookaheadPose converges BECAUSE -->
+ * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate
+ * for 6m (t = 0.8s)
+ * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was
+ * derived using t = 1.0s
+ * So we make a bunch of guesses until it converges
+ * Early exit when change < 1mm to avoid unnecessary iterations
*/
- public void stowEverything(){
- turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
- hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
- shooter.setShooter(0.0);
- spindexer.noIndexing = true;
+ boolean shuttling = target.equals(FieldConstants.getHubTranslation().toTranslation2d());
+ for (int i = 0; i < 20; i++) {
+ Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(),
+ TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
+
+ Translation3d target3d = new Translation3d(target.getX(), target.getY(),
+ shuttling ? FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub
+
+ goalState = ShooterPhysics.getShotParams(
+ Translation2d.kZero,
+ target3d.minus(lookahead3d),
+ 2.0);
+
+ if (!shuttling) {
+ timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get();
+ } else {
+ double distance = target.getDistance(lookaheadPose.getTranslation());
+ timeOfFlight = distance * shuttlingTOFMultiplier.get();
+ }
+
+ double offsetX = turretVelocityX * timeOfFlight;
+ double offsetY = turretVelocityY * timeOfFlight;
+ Pose2d newLookaheadPose = new Pose2d(
+ turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
+ turretPosition.getRotation());
+
+ // early exit if converged (change < 1mm)
+ if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) {
+ lookaheadPose = newLookaheadPose;
+ break;
+ }
+ lookaheadPose = newLookaheadPose;
}
- public void underLadder(){
- spindexer.noIndexing = true;
+ // Get the field angle relative to the target pose
+ turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
+ if (lastTurretAngle == null) {
+ lastTurretAngle = turretAngle;
}
- // shoot higher
- public void bumpUpHoodOffset() {
- hoodOffset.set(hoodOffset.get() + 1.0); //1 deg
- }
+ // Take the filtered average as the turret's velocity when robot is moving
+ // translationally
+ turretVelocity = turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
+
+ lastTurretAngle = turretAngle;
- // shoot lower
- public void bumpDownHoodOffset() {
- hoodOffset.set(hoodOffset.get() - 1.0); //1 deg
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Turret/Target Pose", target);
+ Logger.recordOutput("Lookahead Pose", lookaheadPose);
+ }
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("Time of flight", timeOfFlight);
+ SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX);
+ SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY);
}
- // aim more left
- public void bumpUpTurretOffset() {
- turretOffset.set(turretOffset.get() + 2.5); //2.5 deg
+ // Subtract the rotational angle of the robot from the setpoint
+ double adjustedTurretSetpoint = MathUtil
+ .angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
+
+ // Shortest path
+ double error = MathUtil.inputModulus(
+ Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
+ double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset.get();
+
+ // Stay within physical limits -- if shortest path is past max angle, we go long
+ // way around
+ if (potentialSetpoint > TurretConstants.MAX_ANGLE) {
+ potentialSetpoint -= 360;
+ } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) {
+ potentialSetpoint += 360;
}
- // aim more right
- public void bumpDownTurretOffset() {
- turretOffset.set(turretOffset.get() - 2.5); //2.5 deg
+ turretSetpoint = potentialSetpoint;
+
+ // Pitch is in radians
+ hoodAngle = goalState.pitch();
+ hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
+ hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
+ lastHoodAngle = hoodAngle;
+
+ distanceFromTarget = target.getDistance(lookaheadPose.getTranslation());
+ Logger.recordOutput("Shooting/distanceToTarget", distanceFromTarget);
+ }
+
+ public void updateDrivePose() {
+ Pose2d currentPose = drivetrain.getPose();
+
+ drivepose = new Pose2d(
+ currentPose.getTranslation(),
+ // Uncomment this if robot is backwards
+ currentPose.getRotation()// .plus(new Rotation2d(Math.PI))
+ );
+ ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
+
+ // Add a phase delay extrapolation component for latency delay
+ drivepose = drivepose.exp(
+ new Twist2d(
+ robotRelVel.vxMetersPerSecond * phaseDelay.get(),
+ robotRelVel.vyMetersPerSecond * phaseDelay.get(),
+ robotRelVel.omegaRadiansPerSecond * phaseDelay.get()));
+ }
+
+ /**
+ * Stops and stows all subsystems involved in the command
+ */
+ public void stowEverything() {
+ turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
+ shooter.setShooter(0.0);
+ spindexer.noIndexing = true;
+ }
+
+ public void underLadder() {
+ spindexer.noIndexing = true;
+ }
+
+ // shoot higher
+ public void bumpUpHoodOffset() {
+ hoodOffset.set(hoodOffset.get() + 1.0); // 1 deg
+ }
+
+ // shoot lower
+ public void bumpDownHoodOffset() {
+ hoodOffset.set(hoodOffset.get() - 1.0); // 1 deg
+ }
+
+ // aim more left
+ public void bumpUpTurretOffset() {
+ turretOffset.set(turretOffset.get() + 2.5); // 2.5 deg
+ }
+
+ // aim more right
+ public void bumpDownTurretOffset() {
+ turretOffset.set(turretOffset.get() - 2.5); // 2.5 deg
+ }
+
+ @Override
+ public void execute() {
+ // Phase manager stuff
+ phaseManager.update(drivepose, shooter, turret);
+ target = phaseManager.getTarget(drivepose);
+
+ updateDrivePose();
+ updateSetpoints(drivepose);
+
+ if (phaseManager.isIdle()) {
+ underLadder();
+ } else {
+ if (spindexer.noIndexing) {
+ spindexer.noIndexing = false;
+ }
+ turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint),
+ turretVelocity - drivetrain.getAngularRate(2));
+
+ boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at
+ // the hub, we're not
+ // shuttling
+
+ // shuttling will move the hood so its angles very close to max (less arch)
+ if (shuttling) {
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)),
+ hoodVelocity);
+ } else {
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)),
+ hoodVelocity);
+ }
+
+ // if (FieldConstants.underTrench(x, y)) {
+ // System.out.println("Hood forced down");
+ // } else {
+ // hood.forceHoodDown(false);
+ // }
+
+ // different maps for shuttling vs shooting. Less powerful when shuttling.
+ if (shuttling) {
+ shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget));
+ } else {
+ shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
+ }
+
+ if (!Constants.DISABLE_LOGGING) {
+ // record when shuttling
+ Logger.recordOutput("Shuttling", shuttling);
+ // record distance for tuning if needed
+ Logger.recordOutput("Distance From Target", distanceFromTarget);
+ }
}
- @Override
- public void execute() {
- // Phase manager stuff
- phaseManager.update(drivepose, shooter, turret);
- target = phaseManager.getTarget(drivepose);
-
- updateDrivePose();
- updateSetpoints(drivepose);
-
- if (phaseManager.isIdle()) {
- underLadder();
- } else {
- if (spindexer.noIndexing) {
- spindexer.noIndexing = false;
- }
- turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2));
-
- boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at the hub, we're not shuttling
-
- // shuttling will move the hood so its angles very close to max (less arch)
- if (shuttling) {
- hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
- } else {
- hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
- }
-
- // if (FieldConstants.underTrench(x, y)) {
- // System.out.println("Hood forced down");
- // } else {
- // hood.forceHoodDown(false);
- // }
-
- // different maps for shuttling vs shooting. Less powerful when shuttling.
- if (shuttling) {
- shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget));
- } else {
- shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
- }
-
- if (!Constants.DISABLE_LOGGING) {
- // record when shuttling
- Logger.recordOutput("Shuttling", shuttling);
- // record distance for tuning if needed
- Logger.recordOutput("Distance From Target", distanceFromTarget);
- }
- }
-
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint);
- Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint);
- Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel());
-
- Logger.recordOutput("DistanceToTarget", target.getDistance(drivepose.getTranslation()));
- }
-
- // for operator
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString());
-
- } else {
- phaseDelay.set(0.03);
- }
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint);
+ Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint);
+ Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel());
+
+ Logger.recordOutput("DistanceToTarget", target.getDistance(drivepose.getTranslation()));
}
- @Override
- public void end(boolean interrupted) {
- stowEverything();
+ // for operator
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString());
+
+ } else {
+ phaseDelay.set(0.03);
}
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ stowEverything();
+ }
}
*/
public class Drivetrain extends SubsystemBase {
- protected final Module[] modules;
+ protected final Module[] modules;
- private final GyroIO gyroIO;
- private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
+ private final GyroIO gyroIO;
+ private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
- public static Lock odometryLock = new ReentrantLock();
+ public static Lock odometryLock = new ReentrantLock();
- private SwerveSetpoint currentSetpoint = new SwerveSetpoint(
- new ChassisSpeeds(),
- new SwerveModuleState[] {
- new SwerveModuleState(),
- new SwerveModuleState(),
- new SwerveModuleState(),
- new SwerveModuleState()
- });
- // Odometry
- private final SwerveDrivePoseEstimator poseEstimator;
+ private SwerveSetpoint currentSetpoint = new SwerveSetpoint(
+ new ChassisSpeeds(),
+ new SwerveModuleState[] {
+ new SwerveModuleState(),
+ new SwerveModuleState(),
+ new SwerveModuleState(),
+ new SwerveModuleState()
+ });
+ // Odometry
+ private final SwerveDrivePoseEstimator poseEstimator;
- // Vision
- private final Vision vision;
+ // Vision
+ private final Vision vision;
- // PID Controllers for chassis movement
- private final PIDController xController;
- private final PIDController yController;
- private final PIDController rotationController;
+ // PID Controllers for chassis movement
+ private final PIDController xController;
+ private final PIDController yController;
+ private final PIDController rotationController;
- // If vision is enabled for drivetrain odometry updating
- // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED
- // instead
- private boolean visionEnabled = true;
+ // If vision is enabled for drivetrain odometry updating
+ // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED
+ // instead
+ private boolean visionEnabled = true;
- // Disables vision for the first few seconds after deploying
- private Timer visionEnableTimer = new Timer();
+ // Disables vision for the first few seconds after deploying
+ private Timer visionEnableTimer = new Timer();
- // If the robot should algin to the angle
- private boolean isAlign = false;
- // Angle to align to, can be null
- private Double alignAngle = null;
- // used for drift control
- private double currentHeading = 0;
- // used for drift control
- private boolean drive_turning = false;
+ // If the robot should algin to the angle
+ private boolean isAlign = false;
+ // Angle to align to, can be null
+ private Double alignAngle = null;
+ // used for drift control
+ private double currentHeading = 0;
+ // used for drift control
+ private boolean drive_turning = false;
- private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
+ private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
- // The pose supplier to drive to
-
private Supplier<Pose2d> desiredPoSupplier = () -> null;
+ // The pose supplier to drive to
+ private Supplier<Pose2d> desiredPoSupplier = () -> null;
- private SwerveModulePose modulePoses;
+ private SwerveModulePose modulePoses;
- // The previous pose to reset to if the current pose gets too far off the field
- private Pose2d prevPose = new Pose2d();
+ // The previous pose to reset to if the current pose gets too far off the field
+ private Pose2d prevPose = new Pose2d();
- private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];;
+ private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];;
- private boolean slipped = false;
+ private boolean slipped = false;
- private double previousAngularVelocity = 0;
+ private double previousAngularVelocity = 0;
- private double centerOfMassHeight = 0;
+ private double centerOfMassHeight = 0;
- private Rotation2d rawGyroRotation = new Rotation2d();
+ private Rotation2d rawGyroRotation = new Rotation2d();
- // for vision yaw correction
- private GyroBiasEstimator gyroBiasEstimator = new GyroBiasEstimator();
+ // for vision yaw correction
+ private GyroBiasEstimator gyroBiasEstimator = new GyroBiasEstimator();
- private final Field2d field = new Field2d();
+ private final Field2d field = new Field2d();
- /**
- * Creates a new Swerve Style Drivetrain.
- */
- public Drivetrain(Vision vision, GyroIO gyroIO) {
- this.vision = vision;
-
- modules = new Module[4];
- this.gyroIO = gyroIO;
- ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4);
-
- if (RobotBase.isReal()) {
- Arrays.stream(constants).forEach(moduleConstants -> {
- modules[moduleConstants.ordinal()] = new Module(moduleConstants);
- });
- } else {
- Arrays.stream(constants).forEach(moduleConstants -> {
- modules[moduleConstants.ordinal()] = new ModuleSim(moduleConstants);
- });
- }
-
- /*
- * By pausing init for a second before setting module offsets, we avoid a bug
- * with inverting motors.
- * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info.
- */
- Timer.delay(1.0);
- resetModulesToAbsolute();
- gyroIO.updateInputs(gyroInputs);
- poseEstimator = new SwerveDrivePoseEstimator(
- DriveConstants.KINEMATICS,
- gyroInputs.yawPosition,
- updateModulePositions(),
- new Pose2d(),
- // Defaults, except trust pigeon more
- VecBuilder.fill(0.1, 0.1, 0),
- VisionConstants.VISION_STD_DEVS);
- poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS);
-
- // initialize PID controllers
- xController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
- yController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
- rotationController = new PIDController(DriveConstants.HEADING_P, 0, DriveConstants.HEADING_D);
- rotationController.enableContinuousInput(-Math.PI, Math.PI);
- rotationController.setTolerance(Units.degreesToRadians(0.25), Units.degreesToRadians(0.25));
-
- PhoenixOdometryThread.getInstance().start();
-
- modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS);
-
- PathPlannerLogging.setLogActivePathCallback(
- (activePath) -> {
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput(
- "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
- }
- });
- PathPlannerLogging.setLogTargetPoseCallback(
- (targetPose) -> {
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
- }
- });
-
- // PPLibTelemetry.enableCompetitionMode();
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putData("Field", field);
- }
- }
-
- public void close() {
- // close each of the modules
- for (int i = 0; i < modules.length; i++) {
- modules[i].close();
- }
- }
-
- @Override
- public void periodic() {
- odometryLock.lock(); // Prevents odometry updates while reading data
- gyroIO.updateInputs(gyroInputs);
- Logger.processInputs("Drive/Gyro", gyroInputs);
- for (var module : modules) {
- module.periodic();
- }
- odometryLock.unlock();
- // Update odometry
- double[] sampleTimestamps = gyroInputs.odometryYawTimestamps; // All signals are sampled together
- int sampleCount = sampleTimestamps.length;
- SwerveModulePosition[][] positions = new SwerveModulePosition[4][];
- for (int i = 0; i < modules.length; i++) {
- positions[i] = modules[i].getOdometryPositions();
- sampleCount = Math.min(sampleCount, positions[i].length);
- }
-
- // cap samples per cycle, more gives little benefit
- final int MAX_SAMPLES_PER_CYCLE = 10;
- if (sampleCount > MAX_SAMPLES_PER_CYCLE) {
- sampleCount = MAX_SAMPLES_PER_CYCLE;
- }
-
- for (int i = 0; i < sampleCount; i++) {
- // Read wheel positions and deltas from each module
- SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
- for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
- modulePositions[moduleIndex] = positions[moduleIndex][i];
- }
- // Use the real gyro angle
- rawGyroRotation = gyroInputs.odometryYawPositions[i];
- // Apply update
- poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
- }
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses());
- }
- updateOdometryVision();
-
- field.setRobotPose(getPose());
- }
-
- // DRIVE
- /**
- * Method to drive the robot using joystick info.
- *
- * @param xSpeed speed of the robot in the x direction (forward) in m/s
- * @param ySpeed speed of the robot in the y direction (sideways) in m/s
- * @param rot angular rate of the robot in rad/s
- * @param fieldRelative whether the provided x and y speeds are relative to the
- * field
- * @param isOpenLoop whether to use velocity control for the drive motors
- */
- public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) {
- // rot = headingControl(rot, xSpeed, ySpeed);
- ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME);
- if (fieldRelative) {
- speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
- }
- setChassisSpeeds(speeds, isOpenLoop);
- }
+ /**
+ * Creates a new Swerve Style Drivetrain.
+ */
+ public Drivetrain(Vision vision, GyroIO gyroIO) {
+ this.vision = vision;
- /**
- * Drives the robot using the provided x speed, y speed, and positional heading.
- *
- * @param xSpeed speed of the robot in the x direction (forward)
- * @param ySpeed speed of the robot in the y direction (sideways)
- * @param heading target heading of the robot in radians
- * @param fieldRelative whether the provided x and y speeds are relative to the
- * field
- */
- public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) {
- double rot = rotationController.calculate(getYaw().getRadians(), heading);
- ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
- if (fieldRelative) {
- speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
- }
- setChassisSpeeds(speeds, false);
- }
+ modules = new Module[4];
+ this.gyroIO = gyroIO;
+ ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4);
- /**
- * Runs the PID controllers with the provided x, y, and rot values. Then, calls
- * {@link #drive(double, double, double, boolean, boolean)} using the PID
- * outputs.
- * This is based on the odometry of the chassis.
- *
- * @param x the position to move to in the x, in meters
- * @param y the position to move to in the y, in meters
- * @param rot the angle to move to, in radians
- */
- public void driveWithPID(double x, double y, double rot) {
- Pose2d pose = getPose();
- double xSpeed = xController.calculate(pose.getX(), x);
- double ySpeed = yController.calculate(pose.getY(), y);
- double rotRadians = rotationController.calculate(pose.getRotation().getRadians(), rot);
- drive(xSpeed, ySpeed, rotRadians, true, false);
+ if (RobotBase.isReal()) {
+ Arrays.stream(constants).forEach(moduleConstants -> {
+ modules[moduleConstants.ordinal()] = new Module(moduleConstants);
+ });
+ } else {
+ Arrays.stream(constants).forEach(moduleConstants -> {
+ modules[moduleConstants.ordinal()] = new ModuleSim(moduleConstants);
+ });
}
- /**
- * Updates odometry using vision
+ /*
+ * By pausing init for a second before setting module offsets, we avoid a bug
+ * with inverting motors.
+ * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info.
*/
- public void updateOdometryVision() {
- // Start the timer if it hasn't started yet
- visionEnableTimer.start();
-
- // Update the swerve module poses
- modulePoses.update();
-
- if (modulePoses.slipped()) {
- slipped = true;
- }
-
- Pose2d pose2 = getPose();
-
- // Even if vision is disabled, it should still update inputs
- // This prevents it from storing a lot of unread results, and it could be useful
- // for replays
- if (vision != null) {
- vision.updateInputs();
- }
-
- if (VisionConstants.ENABLED) {
- if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) {
- vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped);
-
- if (vision.canSeeTag()) {
- slipped = false;
- modulePoses.reset();
-
- double currentGyroYaw = gyroInputs.yawPosition.getRadians();
-
- // to compare bias
- ArrayList<EstimatedRobotPose> visionPoses = vision.getEstimatedPoses(getPose());
-
- for (EstimatedRobotPose visionPose : visionPoses) {
- if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) {
- double visionYaw = visionPose.estimatedPose.getRotation().getZ();
-
- // gets at vision timestamp, not current gyro yaw
- double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds);
-
- if (!Double.isNaN(gyroYawAtTimestamp)) {
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp));
- Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw));
- }
- // use weighted observation
- gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0);
- }
- }
- }
-
- // check if we have enough samples
- if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) {
- double fullBias = gyroBiasEstimator.getAndResetBias();
- double bias = gyroBiasEstimator.applyPartialCorrection(fullBias);
-
- if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) {
- gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias));
- }
- }
+ Timer.delay(1.0);
+ resetModulesToAbsolute();
+ gyroIO.updateInputs(gyroInputs);
+ poseEstimator = new SwerveDrivePoseEstimator(
+ DriveConstants.KINEMATICS,
+ gyroInputs.yawPosition,
+ updateModulePositions(),
+ new Pose2d(),
+ // Defaults, except trust pigeon more
+ VecBuilder.fill(0.1, 0.1, 0),
+ VisionConstants.VISION_STD_DEVS);
+ poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS);
+
+ // initialize PID controllers
+ xController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
+ yController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
+ rotationController = new PIDController(DriveConstants.HEADING_P, 0, DriveConstants.HEADING_D);
+ rotationController.enableContinuousInput(-Math.PI, Math.PI);
+ rotationController.setTolerance(Units.degreesToRadians(0.25), Units.degreesToRadians(0.25));
+
+ PhoenixOdometryThread.getInstance().start();
+
+ modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS);
+
+ PathPlannerLogging.setLogActivePathCallback(
+ (activePath) -> {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput(
+ "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
+ }
+ });
+ PathPlannerLogging.setLogTargetPoseCallback(
+ (targetPose) -> {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
+ }
+ });
+
+ // PPLibTelemetry.enableCompetitionMode();
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putData("Field", field);
+ }
+ }
+
+ public void close() {
+ // close each of the modules
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].close();
+ }
+ }
+
+ @Override
+ public void periodic() {
+ odometryLock.lock(); // Prevents odometry updates while reading data
+ gyroIO.updateInputs(gyroInputs);
+ Logger.processInputs("Drive/Gyro", gyroInputs);
+ for (var module : modules) {
+ module.periodic();
+ }
+ odometryLock.unlock();
+ // Update odometry
+ double[] sampleTimestamps = gyroInputs.odometryYawTimestamps; // All signals are sampled together
+ int sampleCount = sampleTimestamps.length;
+ SwerveModulePosition[][] positions = new SwerveModulePosition[4][];
+ for (int i = 0; i < modules.length; i++) {
+ positions[i] = modules[i].getOdometryPositions();
+ sampleCount = Math.min(sampleCount, positions[i].length);
+ }
+
+ // cap samples per cycle, more gives little benefit
+ final int MAX_SAMPLES_PER_CYCLE = 10;
+ if (sampleCount > MAX_SAMPLES_PER_CYCLE) {
+ sampleCount = MAX_SAMPLES_PER_CYCLE;
+ }
+
+ for (int i = 0; i < sampleCount; i++) {
+ // Read wheel positions and deltas from each module
+ SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+ for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
+ modulePositions[moduleIndex] = positions[moduleIndex][i];
+ }
+ // Use the real gyro angle
+ rawGyroRotation = gyroInputs.odometryYawPositions[i];
+ // Apply update
+ poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
+ }
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses());
+ }
+ updateOdometryVision();
+
+ field.setRobotPose(getPose());
+ }
+
+ // DRIVE
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed speed of the robot in the x direction (forward) in m/s
+ * @param ySpeed speed of the robot in the y direction (sideways) in m/s
+ * @param rot angular rate of the robot in rad/s
+ * @param fieldRelative whether the provided x and y speeds are relative to the
+ * field
+ * @param isOpenLoop whether to use velocity control for the drive motors
+ */
+ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) {
+ // rot = headingControl(rot, xSpeed, ySpeed);
+ ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME);
+ if (fieldRelative) {
+ speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+ }
+ setChassisSpeeds(speeds, isOpenLoop);
+ }
+
+ /**
+ * Drives the robot using the provided x speed, y speed, and positional heading.
+ *
+ * @param xSpeed speed of the robot in the x direction (forward)
+ * @param ySpeed speed of the robot in the y direction (sideways)
+ * @param heading target heading of the robot in radians
+ * @param fieldRelative whether the provided x and y speeds are relative to the
+ * field
+ */
+ public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) {
+ double rot = rotationController.calculate(getYaw().getRadians(), heading);
+ ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
+ if (fieldRelative) {
+ speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+ }
+ setChassisSpeeds(speeds, false);
+ }
+
+ /**
+ * Runs the PID controllers with the provided x, y, and rot values. Then, calls
+ * {@link #drive(double, double, double, boolean, boolean)} using the PID
+ * outputs.
+ * This is based on the odometry of the chassis.
+ *
+ * @param x the position to move to in the x, in meters
+ * @param y the position to move to in the y, in meters
+ * @param rot the angle to move to, in radians
+ */
+ public void driveWithPID(double x, double y, double rot) {
+ Pose2d pose = getPose();
+ double xSpeed = xController.calculate(pose.getX(), x);
+ double ySpeed = yController.calculate(pose.getY(), y);
+ double rotRadians = rotationController.calculate(pose.getRotation().getRadians(), rot);
+ drive(xSpeed, ySpeed, rotRadians, true, false);
+ }
+
+ /**
+ * Updates odometry using vision
+ */
+ public void updateOdometryVision() {
+ // Start the timer if it hasn't started yet
+ visionEnableTimer.start();
+
+ // Update the swerve module poses
+ modulePoses.update();
+
+ if (modulePoses.slipped()) {
+ slipped = true;
+ }
+
+ Pose2d pose2 = getPose();
+
+ // Even if vision is disabled, it should still update inputs
+ // This prevents it from storing a lot of unread results, and it could be useful
+ // for replays
+ if (vision != null) {
+ vision.updateInputs();
+ }
+
+ if (VisionConstants.ENABLED) {
+ if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) {
+ vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped);
+
+ if (vision.canSeeTag()) {
+ slipped = false;
+ modulePoses.reset();
+
+ double currentGyroYaw = gyroInputs.yawPosition.getRadians();
+
+ // to compare bias
+ ArrayList<EstimatedRobotPose> visionPoses = vision.getEstimatedPoses(getPose());
+
+ for (EstimatedRobotPose visionPose : visionPoses) {
+ if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) {
+ double visionYaw = visionPose.estimatedPose.getRotation().getZ();
+
+ // gets at vision timestamp, not current gyro yaw
+ double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds);
+
+ if (!Double.isNaN(gyroYawAtTimestamp)) {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp));
+ Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw));
}
+ // use weighted observation
+ gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0);
+ }
}
- }
-
- Pose2d pose3 = getPose();
-
- // Reset the pose to a position on the field if it is too far off the field
- // This uses nearField() instead of onField() so we don't reset the odometry
- // when the wheels slip near the edge of the field
- // This is meant for poses that are caused by errors
- if (!Vision.nearField(prevPose)) {
- // If the pose at the beginning of the method is off the field, reset to a
- // position in the middle of the field
- // Use the rotation of the pose after updating odometry so the yaw is right
- prevPose = new Pose2d(FieldConstants.field.getFieldLength() / 2, FieldConstants.field.getFieldWidth() / 2,
- pose2.getRotation());
- resetOdometry(prevPose);
- } else if (!Vision.nearField(pose2)) {
- // if the drivetrain pose is off the field, reset our odometry to the pose
- // before(this is the right pose)
- // Keep the rotation from pose2 so yaw is correct for driver
- prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation());
- resetOdometry(prevPose);
- } else if (!Vision.nearField(pose3)) {
- // if our vision+drivetrain odometry isn't near the field, reset our odometry to
- // the pose before(this is the right pose)
- resetOdometry(pose2);
- prevPose = pose2;
- } else {
- // Set the previous pose to the current pose if we need to return to that
- prevPose = pose3;
- }
-
- // if (Robot.isSimulation()) {
- // pigeon.getSimState().addYaw(
- // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond
- // * Constants.LOOP_TIME));
- // }
- }
-
- /**
- * Stops all swerve modules.
- */
- public void stop() {
- Arrays.stream(modules).forEach(Module::stop);
- }
-
- // GETTERS AND SETTERS
-
- private boolean trenchAssist = false;
- private boolean trenchAlign = false;
-
- public boolean getTrenchAssist() {
- return trenchAssist;
- }
+ }
- public boolean getTrenchAlign() {
- return trenchAlign;
- }
-
- public void setTrenchAssist(boolean target) {
- trenchAssist = target;
- }
-
- public void setTrenchAlign(boolean target) {
- trenchAlign = target;
- }
-
- // for current limit setting (brownout protection)
- public void applyNewModuleCurrents(
- double steerCurrentStator, double steerCurrentSupply,
- double driveCurrentStator, double driveCurrentSupply) {
- for (Module module : modules) { // iterate over our modules
- module.setNewCurrentLimit(steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply);
- }
- }
-
- public double getSubsystemStatorCurrent() {
- double sum = 0;
- for (Module module : modules) {
- sum += module.getModuleStatorCurrent();
- }
- return sum;
- }
-
- public double getSubsystemSupplyCurrent() {
- double sum = 0;
- for (Module module : modules) {
- sum += module.getModuleSupplyCurrent();
- }
- return sum;
- }
-
- /**
- * Sets the desired states for all swerve modules.
- *
- * @param swerveModuleStates an array of module states to set swerve modules to.
- * Order of the array matters here!
- */
- public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) {
- // makes sure speeds of modules don't exceed maximum allowed
- SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED);
-
- for (int i = 0; i < 4; i++) {
- modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop);
- }
- }
-
- /**
- * Sets the chassis speeds of the robot.
- *
- * @param chassisSpeeds the target chassis speeds
- * @param isOpenLoop if open loop control should be used for the drive
- * velocity
- */
- public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) {
-
- if (DriveConstants.USE_ACTUAL_SPEED) {
- SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates());
- currentSetpoint = setpointGenerator.generateSetpoint(
- DriveConstants.MODULE_LIMITS,
- centerOfMassHeight,
- currentState, chassisSpeeds,
- Constants.LOOP_TIME);
- } else {
- currentSetpoint = setpointGenerator.generateSetpoint(
- DriveConstants.MODULE_LIMITS,
- centerOfMassHeight,
- currentSetpoint, chassisSpeeds,
- Constants.LOOP_TIME);
- }
-
- SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates();
- setModuleStates(swerveModuleStates, isOpenLoop);
- }
+ // check if we have enough samples
+ if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) {
+ double fullBias = gyroBiasEstimator.getAndResetBias();
+ double bias = gyroBiasEstimator.applyPartialCorrection(fullBias);
- public void setDriveVoltages(Voltage voltage) {
- for (int i = 0; i < modules.length; i++) {
- modules[i].setDriveVoltage(voltage);
- }
- }
-
- public void setAngleMotors(Rotation2d[] angles) {
- for (int i = 0; i < modules.length; i++) {
- modules[i].setAngle(angles[i]);
- }
- }
-
- /**
- * Returns the angular rate from the pigeon.
- *
- * @param id 0 for x, 1 for y, 2 for z
- * @return the rate in rads/s from the pigeon
- */
- public double getAngularRate(int id) {
- // double speed = 0;
- // switch(id){
- // case 0:
- // speed = gyroInputs..getAngularVelocityXWorld().getValueAsDouble();
- // break;
- // case 1:
- // speed = pigeon.getAngularVelocityYWorld().getValueAsDouble();
- // break;
- // case 2:
- // speed = pigeon.getAngularVelocityZWorld().getValueAsDouble();
- // break;
- // }
- // outputs in deg/s, so convert to rad/s
- return gyroInputs.yawVelocityRadPerSec;
- }
-
- /**
- * Updates and returns the array of SwerveModulePositions, which store the
- * distance travleled by the drive and the steer angle.
- *
- * @return An array of all swerve module positions
- */
- private SwerveModulePosition[] updateModulePositions() {
- return modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new);
- }
-
- /**
- * Gets an array of SwerveModulePositions, which store the distance travleled by
- * the drive and the steer angle.
- *
- * @return An array of all swerve module positions
- */
- public SwerveModulePosition[] getModulePositions() {
- return modulePositions;
- }
-
- /**
- * Enables or disables the state deadband for all swerve modules.
- * The state deadband determines if the robot will stop drive and steer motors
- * when inputted drive velocity is low.
- * It should be enabled for all regular driving, to prevent releasing the
- * controls from setting the angles.
- */
- public void setStateDeadband(boolean stateDeadBand) {
- Arrays.stream(modules).forEach(module -> module.setStateDeadband(stateDeadBand));
- }
-
- public void setOptimized(boolean optimized) {
- Arrays.stream(modules).forEach(module -> module.setOptimize(optimized));
- }
-
- public void setVisionEnabled(boolean enabled) {
- visionEnabled = enabled;
- }
-
- public void setIsAlign(boolean isAlign) {
- this.isAlign = isAlign;
- }
-
- public boolean getIsAlign() {
- return isAlign;
- }
-
- /**
- * Calculates chassis speed of drivetrain using the current SwerveModuleStates
- *
- * @return ChassisSpeeds object
- * This is often used as an input for other methods
- */
- public ChassisSpeeds getChassisSpeeds() {
- return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates());
- }
-
- /**
- * Gets the state of each module
- *
- * @return An array of 4 SwerveModuleStates
- */
- public SwerveModuleState[] getModuleStates() {
- return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new);
- }
-
- public SwerveSetpoint getCurrSetpoint() {
- return currentSetpoint;
- }
-
- /**
- * @return the yaw of the robot, aka heading, the direction it is facing
- */
- public Rotation2d getYaw() {
- return getPose().getRotation();
- }
-
- /**
- * @return an array of modules
- */
- public Module[] getModules() {
- return modules;
- }
-
- /**
- * gets gyro yaw at a specific timestamp with interpolation
- * this is used for timestamp-synchronized gyro/vision comparison.
- *
- * @param timestampSeconds the timestamp to get the gyro yaw at
- * @return the gyro yaw in radians, or Double.NaN if no valid data
- */
- private double
- getGyroYawAtTimestamp(double timestampSeconds) {
- return getPose().getRotation().getRadians();
- }
-
- /**
- * Resets the yaw of the robot.
- *
- * @param rotation the new yaw angle as Rotation2d
- */
- public void setYaw(Rotation2d rotation) {
- resetOdometry(new Pose2d(getPose().getTranslation(), rotation));
- }
-
- /**
- * Resets the odometry to the given pose.
- *
- * @param pose the pose to reset to.
- */
- public void resetOdometry(Pose2d pose) {
- // NOTE: must use pigeon yaw for odometer!
- currentHeading = pose.getRotation().getRadians();
- poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose);
- modulePoses.reset();
- }
-
- /**
- * @return the pose of the robot as estimated by the odometry
- */
- @AutoLogOutput(key = "Odometry/Robot")
- public Pose2d getPose() {
- return poseEstimator.getEstimatedPosition();
- }
-
- /**
- * Sets the angle to align to
- *
- * @param newAngle The new angle in radians, can be set to null
- */
- public void setAlignAngle(Double newAngle) {
- alignAngle = newAngle;
- }
-
- /**
- * Returns whether or not the robot is at the input align angle
- *
- * @return true if it within tolerance the align angle, false otherwise
- */
- public boolean atAlignAngle() {
- if (alignAngle == null) {
- return false;
- }
- double diff = Math.abs(alignAngle - getYaw().getRadians());
- return diff < DriveConstants.HEADING_TOLERANCE || diff > 2 * Math.PI - DriveConstants.HEADING_TOLERANCE;
- }
-
- /**
- * Gets the angle to align to
- *
- * @return The angle in radians
- */
- public double getAlignAngle() {
- if (alignAngle != null) {
- return alignAngle;
- }
- return 0;
- }
-
- /**
- * Sets vision to only use certain April tags
- *
- * @param ids An array of the tags to only use
- */
- public void onlyUseTags(int[] ids) {
- if (vision != null) {
- vision.onlyUse(ids);
- }
- }
-
- /**
- * Returns if vision has seen an April tag in the last frame
- *
- * @return true if vision saw a tag last frame or if vision is disabled
- */
- public boolean canSeeTag() {
- // if no vision system, then return true
- if (vision == null)
- return true;
-
- return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED;
- }
-
- /**
- * Gets the pose at a previous time
- *
- * @param timestamp The timestamp of the pose to get
- * @return The pose, null if there are no poses yet, or the current pose if
- * timestamp < 0
- */
- public Pose2d getPoseAt(double timestamp) {
- if (timestamp < 0) {
- return getPose();
- }
- Optional<Pose2d> pose = poseEstimator.sampleAt(timestamp);
- if (pose.isPresent()) {
- return pose.get();
- } else {
- return null;
- }
- }
-
- /**
- * Uses pigeon and rotational input to return a rotation that accounts for drift
- *
- * @return A rotation
- */
- public double headingControl(double rot, double xSpeed, double ySpeed) {
- if ((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004)
- && EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed), 0, 0.1))
- || !EqualsUtil.epsilonEquals(rot, 0, 0.0004)) {
- drive_turning = true;
- currentHeading = getYaw().getRadians();
- } else {
- drive_turning = false;
- }
- if (!drive_turning) {
- rotationController.setSetpoint(currentHeading);
- double output = rotationController.calculate(getYaw().getRadians());
- rot = Math.abs(output) > Math.abs(rot) ? output : rot;
+ if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) {
+ gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias));
+ }
+ }
}
- return rot;
- }
-
- /**
- * Resets the swerve modules from the absolute encoders
- */
- public void resetModulesToAbsolute() {
- Arrays.stream(modules).forEach(Module::resetToAbsolute);
- }
-
- // getters for the PID Controllers
- public PIDController getXController() {
- return xController;
- }
-
- public PIDController getYController() {
- return yController;
- }
-
- public PIDController getRotationController() {
- return rotationController;
- }
-
- /**
- * Set the desired pose to drive to
- * This will enable driver assist to go to the pose
- *
- * @param supplier The supplier for the desired pose, use ()->null to not use a
- * desired pose
- */
- public void setDesiredPose(Supplier<Pose2d> supplier) {
- desiredPoSupplier = supplier;
- }
-
- /**
- * Set the desired pose to drive to
- * This will enable driver assist to go to the pose
- *
- * @param pose The Pose2d to drive to
- */
- public void setDesiredPose(Pose2d pose) {
- setDesiredPose(() -> pose);
- }
-
- /**
- * Gets the current desired pose, or null if there is no desired pose
- *
- * @return The Pose2d if it exists, null otherwise
- */
- public Pose2d getDesiredPose() {
- return desiredPoSupplier.get();
- }
-
- public boolean atSetpoint() {
- Pose2d pose = getDesiredPose();
- return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025;
- }
-
- public SwerveModulePose getSwerveModulePose() {
- return modulePoses;
- }
-
- public double getAcceleration() {
- double accelX = gyroInputs.accelerationX;
- double accelY = gyroInputs.accelerationY;
-
- double angularVelocity = getAngularRate(3);
- double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME;
- previousAngularVelocity = angularVelocity;
-
- double pigeonOffsetX = 0.082677;
- double pigeonOffsetY = 0.030603444;
-
- double totalX = accelX + Math.pow(angularVelocity, 2) * pigeonOffsetX + angularAccel * pigeonOffsetY;
- double totalY = accelY + Math.pow(angularVelocity, 2) * pigeonOffsetY - angularAccel * pigeonOffsetX;
-
- return Math.hypot(totalX, totalY);
- }
-
- @AutoLogOutput(key = "Drivetrain/AccelerationFaults")
- public boolean accelerationOverMax() {
- return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL;
- }
-
- public void setCenterOfMass(double height) {
- centerOfMassHeight = height;
- }
-
- public void alignWheels() {
- SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0));
- setModuleStates(new SwerveModuleState[] {
- state, state, state, state
- }, false);
- }
+ }
+ }
+
+ Pose2d pose3 = getPose();
+
+ // Reset the pose to a position on the field if it is too far off the field
+ // This uses nearField() instead of onField() so we don't reset the odometry
+ // when the wheels slip near the edge of the field
+ // This is meant for poses that are caused by errors
+ if (!Vision.nearField(prevPose)) {
+ // If the pose at the beginning of the method is off the field, reset to a
+ // position in the middle of the field
+ // Use the rotation of the pose after updating odometry so the yaw is right
+ prevPose = new Pose2d(FieldConstants.field.getFieldLength() / 2, FieldConstants.field.getFieldWidth() / 2,
+ pose2.getRotation());
+ resetOdometry(prevPose);
+ } else if (!Vision.nearField(pose2)) {
+ // if the drivetrain pose is off the field, reset our odometry to the pose
+ // before(this is the right pose)
+ // Keep the rotation from pose2 so yaw is correct for driver
+ prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation());
+ resetOdometry(prevPose);
+ } else if (!Vision.nearField(pose3)) {
+ // if our vision+drivetrain odometry isn't near the field, reset our odometry to
+ // the pose before(this is the right pose)
+ resetOdometry(pose2);
+ prevPose = pose2;
+ } else {
+ // Set the previous pose to the current pose if we need to return to that
+ prevPose = pose3;
+ }
+
+ // if (Robot.isSimulation()) {
+ // pigeon.getSimState().addYaw(
+ // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond
+ // * Constants.LOOP_TIME));
+ // }
+ }
+
+ /**
+ * Stops all swerve modules.
+ */
+ public void stop() {
+ Arrays.stream(modules).forEach(Module::stop);
+ }
+
+ // GETTERS AND SETTERS
+
+ private boolean trenchAssist = false;
+ private boolean trenchAlign = false;
+
+ public boolean getTrenchAssist() {
+ return trenchAssist;
+ }
+
+ public boolean getTrenchAlign() {
+ return trenchAlign;
+ }
+
+ public void setTrenchAssist(boolean target) {
+ trenchAssist = target;
+ }
+
+ public void setTrenchAlign(boolean target) {
+ trenchAlign = target;
+ }
+
+ // for current limit setting (brownout protection)
+ public void applyNewModuleCurrents(
+ double steerCurrentStator, double steerCurrentSupply,
+ double driveCurrentStator, double driveCurrentSupply) {
+ for (Module module : modules) { // iterate over our modules
+ module.setNewCurrentLimit(steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply);
+ }
+ }
+
+ public double getSubsystemStatorCurrent() {
+ double sum = 0;
+ for (Module module : modules) {
+ sum += module.getModuleStatorCurrent();
+ }
+ return sum;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ double sum = 0;
+ for (Module module : modules) {
+ sum += module.getModuleSupplyCurrent();
+ }
+ return sum;
+ }
+
+ /**
+ * Sets the desired states for all swerve modules.
+ *
+ * @param swerveModuleStates an array of module states to set swerve modules to.
+ * Order of the array matters here!
+ */
+ public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) {
+ // makes sure speeds of modules don't exceed maximum allowed
+ SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED);
+
+ for (int i = 0; i < 4; i++) {
+ modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop);
+ }
+ }
+
+ /**
+ * Sets the chassis speeds of the robot.
+ *
+ * @param chassisSpeeds the target chassis speeds
+ * @param isOpenLoop if open loop control should be used for the drive
+ * velocity
+ */
+ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) {
+
+ if (DriveConstants.USE_ACTUAL_SPEED) {
+ SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates());
+ currentSetpoint = setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ centerOfMassHeight,
+ currentState, chassisSpeeds,
+ Constants.LOOP_TIME);
+ } else {
+ currentSetpoint = setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ centerOfMassHeight,
+ currentSetpoint, chassisSpeeds,
+ Constants.LOOP_TIME);
+ }
+
+ SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates();
+ setModuleStates(swerveModuleStates, isOpenLoop);
+ }
+
+ public void setDriveVoltages(Voltage voltage) {
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].setDriveVoltage(voltage);
+ }
+ }
+
+ public void setAngleMotors(Rotation2d[] angles) {
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].setAngle(angles[i]);
+ }
+ }
+
+ /**
+ * Returns the angular rate from the pigeon.
+ *
+ * @param id 0 for x, 1 for y, 2 for z
+ * @return the rate in rads/s from the pigeon
+ */
+ public double getAngularRate(int id) {
+ // double speed = 0;
+ // switch(id){
+ // case 0:
+ // speed = gyroInputs..getAngularVelocityXWorld().getValueAsDouble();
+ // break;
+ // case 1:
+ // speed = pigeon.getAngularVelocityYWorld().getValueAsDouble();
+ // break;
+ // case 2:
+ // speed = pigeon.getAngularVelocityZWorld().getValueAsDouble();
+ // break;
+ // }
+ // outputs in deg/s, so convert to rad/s
+ return gyroInputs.yawVelocityRadPerSec;
+ }
+
+ /**
+ * Updates and returns the array of SwerveModulePositions, which store the
+ * distance travleled by the drive and the steer angle.
+ *
+ * @return An array of all swerve module positions
+ */
+ private SwerveModulePosition[] updateModulePositions() {
+ return modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new);
+ }
+
+ /**
+ * Gets an array of SwerveModulePositions, which store the distance travleled by
+ * the drive and the steer angle.
+ *
+ * @return An array of all swerve module positions
+ */
+ public SwerveModulePosition[] getModulePositions() {
+ return modulePositions;
+ }
+
+ /**
+ * Enables or disables the state deadband for all swerve modules.
+ * The state deadband determines if the robot will stop drive and steer motors
+ * when inputted drive velocity is low.
+ * It should be enabled for all regular driving, to prevent releasing the
+ * controls from setting the angles.
+ */
+ public void setStateDeadband(boolean stateDeadBand) {
+ Arrays.stream(modules).forEach(module -> module.setStateDeadband(stateDeadBand));
+ }
+
+ public void setOptimized(boolean optimized) {
+ Arrays.stream(modules).forEach(module -> module.setOptimize(optimized));
+ }
+
+ public void setVisionEnabled(boolean enabled) {
+ visionEnabled = enabled;
+ }
+
+ public void setIsAlign(boolean isAlign) {
+ this.isAlign = isAlign;
+ }
+
+ public boolean getIsAlign() {
+ return isAlign;
+ }
+
+ /**
+ * Calculates chassis speed of drivetrain using the current SwerveModuleStates
+ *
+ * @return ChassisSpeeds object
+ * This is often used as an input for other methods
+ */
+ public ChassisSpeeds getChassisSpeeds() {
+ return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates());
+ }
+
+ /**
+ * Gets the state of each module
+ *
+ * @return An array of 4 SwerveModuleStates
+ */
+ public SwerveModuleState[] getModuleStates() {
+ return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new);
+ }
+
+ public SwerveSetpoint getCurrSetpoint() {
+ return currentSetpoint;
+ }
+
+ /**
+ * @return the yaw of the robot, aka heading, the direction it is facing
+ */
+ public Rotation2d getYaw() {
+ return getPose().getRotation();
+ }
+
+ /**
+ * @return an array of modules
+ */
+ public Module[] getModules() {
+ return modules;
+ }
+
+ /**
+ * gets gyro yaw at a specific timestamp with interpolation
+ * this is used for timestamp-synchronized gyro/vision comparison.
+ *
+ * @param timestampSeconds the timestamp to get the gyro yaw at
+ * @return the gyro yaw in radians, or Double.NaN if no valid data
+ */
+ private double getGyroYawAtTimestamp(double timestampSeconds) {
+ return getPose().getRotation().getRadians();
+ }
+
+ /**
+ * Resets the yaw of the robot.
+ *
+ * @param rotation the new yaw angle as Rotation2d
+ */
+ public void setYaw(Rotation2d rotation) {
+ resetOdometry(new Pose2d(getPose().getTranslation(), rotation));
+ }
+
+ /**
+ * Resets the odometry to the given pose.
+ *
+ * @param pose the pose to reset to.
+ */
+ public void resetOdometry(Pose2d pose) {
+ // NOTE: must use pigeon yaw for odometer!
+ currentHeading = pose.getRotation().getRadians();
+ poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose);
+ modulePoses.reset();
+ }
+
+ /**
+ * @return the pose of the robot as estimated by the odometry
+ */
+ @AutoLogOutput(key = "Odometry/Robot")
+ public Pose2d getPose() {
+ return poseEstimator.getEstimatedPosition();
+ }
+
+ /**
+ * Sets the angle to align to
+ *
+ * @param newAngle The new angle in radians, can be set to null
+ */
+ public void setAlignAngle(Double newAngle) {
+ alignAngle = newAngle;
+ }
+
+ /**
+ * Returns whether or not the robot is at the input align angle
+ *
+ * @return true if it within tolerance the align angle, false otherwise
+ */
+ public boolean atAlignAngle() {
+ if (alignAngle == null) {
+ return false;
+ }
+ double diff = Math.abs(alignAngle - getYaw().getRadians());
+ return diff < DriveConstants.HEADING_TOLERANCE || diff > 2 * Math.PI - DriveConstants.HEADING_TOLERANCE;
+ }
+
+ /**
+ * Gets the angle to align to
+ *
+ * @return The angle in radians
+ */
+ public double getAlignAngle() {
+ if (alignAngle != null) {
+ return alignAngle;
+ }
+ return 0;
+ }
+
+ /**
+ * Sets vision to only use certain April tags
+ *
+ * @param ids An array of the tags to only use
+ */
+ public void onlyUseTags(int[] ids) {
+ if (vision != null) {
+ vision.onlyUse(ids);
+ }
+ }
+
+ /**
+ * Returns if vision has seen an April tag in the last frame
+ *
+ * @return true if vision saw a tag last frame or if vision is disabled
+ */
+ public boolean canSeeTag() {
+ // if no vision system, then return true
+ if (vision == null)
+ return true;
+
+ return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED;
+ }
+
+ /**
+ * Gets the pose at a previous time
+ *
+ * @param timestamp The timestamp of the pose to get
+ * @return The pose, null if there are no poses yet, or the current pose if
+ * timestamp < 0
+ */
+ public Pose2d getPoseAt(double timestamp) {
+ if (timestamp < 0) {
+ return getPose();
+ }
+ Optional<Pose2d> pose = poseEstimator.sampleAt(timestamp);
+ if (pose.isPresent()) {
+ return pose.get();
+ } else {
+ return null;
+ }
+ }
+
+ /**
+ * Uses pigeon and rotational input to return a rotation that accounts for drift
+ *
+ * @return A rotation
+ */
+ public double headingControl(double rot, double xSpeed, double ySpeed) {
+ if ((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004)
+ && EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed), 0, 0.1))
+ || !EqualsUtil.epsilonEquals(rot, 0, 0.0004)) {
+ drive_turning = true;
+ currentHeading = getYaw().getRadians();
+ } else {
+ drive_turning = false;
+ }
+ if (!drive_turning) {
+ rotationController.setSetpoint(currentHeading);
+ double output = rotationController.calculate(getYaw().getRadians());
+ rot = Math.abs(output) > Math.abs(rot) ? output : rot;
+ }
+ return rot;
+ }
+
+ /**
+ * Resets the swerve modules from the absolute encoders
+ */
+ public void resetModulesToAbsolute() {
+ Arrays.stream(modules).forEach(Module::resetToAbsolute);
+ }
+
+ // getters for the PID Controllers
+ public PIDController getXController() {
+ return xController;
+ }
+
+ public PIDController getYController() {
+ return yController;
+ }
+
+ public PIDController getRotationController() {
+ return rotationController;
+ }
+
+ /**
+ * Set the desired pose to drive to
+ * This will enable driver assist to go to the pose
+ *
+ * @param supplier The supplier for the desired pose, use ()->null to not use a
+ * desired pose
+ */
+ public void setDesiredPose(Supplier<Pose2d> supplier) {
+ desiredPoSupplier = supplier;
+ }
+
+ /**
+ * Set the desired pose to drive to
+ * This will enable driver assist to go to the pose
+ *
+ * @param pose The Pose2d to drive to
+ */
+ public void setDesiredPose(Pose2d pose) {
+ setDesiredPose(() -> pose);
+ }
+
+ /**
+ * Gets the current desired pose, or null if there is no desired pose
+ *
+ * @return The Pose2d if it exists, null otherwise
+ */
+ public Pose2d getDesiredPose() {
+ return desiredPoSupplier.get();
+ }
+
+ public boolean atSetpoint() {
+ Pose2d pose = getDesiredPose();
+ return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025;
+ }
+
+ public SwerveModulePose getSwerveModulePose() {
+ return modulePoses;
+ }
+
+ public double getAcceleration() {
+ double accelX = gyroInputs.accelerationX;
+ double accelY = gyroInputs.accelerationY;
+
+ double angularVelocity = getAngularRate(3);
+ double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME;
+ previousAngularVelocity = angularVelocity;
+
+ double pigeonOffsetX = 0.082677;
+ double pigeonOffsetY = 0.030603444;
+
+ double totalX = accelX + Math.pow(angularVelocity, 2) * pigeonOffsetX + angularAccel * pigeonOffsetY;
+ double totalY = accelY + Math.pow(angularVelocity, 2) * pigeonOffsetY - angularAccel * pigeonOffsetX;
+
+ return Math.hypot(totalX, totalY);
+ }
+
+ @AutoLogOutput(key = "Drivetrain/AccelerationFaults")
+ public boolean accelerationOverMax() {
+ return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL;
+ }
+
+ public void setCenterOfMass(double height) {
+ centerOfMassHeight = height;
+ }
+
+ public void alignWheels() {
+ SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0));
+ setModuleStates(new SwerveModuleState[] {
+ state, state, state, state
+ }, false);
+ }
}
import frc.robot.util.PhoenixOdometryThread;
import lib.CTREModuleState;
-
-public class Module implements ModuleIO{
- private final ModuleType type;
-
- // Degrees
- private final double angleOffset;
-
- private final TalonFX angleMotor;
- private final TalonFX driveMotor;
- private final CANcoder CANcoder;
- private SwerveModuleState desiredState;
-
- protected boolean stateDeadband = true;
-
- private boolean optimizeStates = true;
-
- // Inputs from drive motor
- private final StatusSignal<Angle> drivePosition;
- private final StatusSignal<AngularVelocity> driveVelocity;
- private final StatusSignal<Voltage> driveAppliedVolts;
- private final StatusSignal<Current> driveCurrent;
-
- // Inputs from turn motor
- private final StatusSignal<Angle> turnAbsolutePosition;
- private final StatusSignal<Angle> turnPosition;
- private final StatusSignal<AngularVelocity> turnVelocity;
- private final StatusSignal<Voltage> turnAppliedVolts;
- private final StatusSignal<Current> turnCurrent;
-
- // Timestamp inputs from Phoenix thread
- protected final Queue<Double> timestampQueue;
- protected final Queue<Double> drivePositionQueue;
- protected final Queue<Double> turnPositionQueue;
-
- private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
-
- // Connection debouncers
- private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
- private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
- private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
-
- private final Alert driveDisconnectedAlert;
- private final Alert turnDisconnectedAlert;
- private final Alert turnEncoderDisconnectedAlert;
-
- protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
-
- private ModuleConstants moduleConstants;
- private final MotionMagicVelocityVoltage velocityRequest =
- new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
-
-
- public Module(ModuleConstants moduleConstants) {
- this.moduleConstants = moduleConstants;
-
- type = moduleConstants.getType();
- angleOffset = moduleConstants.getSteerOffset();
-
- /* Angle Encoder Config */
- CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
- /* Angle Motor Config */
- angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
- driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
- // Create drive status signals
- drivePosition = driveMotor.getPosition();
- driveVelocity = driveMotor.getVelocity();
- driveAppliedVolts = driveMotor.getMotorVoltage();
- driveCurrent = driveMotor.getStatorCurrent();
-
- // Create turn status signals
- turnAbsolutePosition = CANcoder.getAbsolutePosition();
- turnPosition = angleMotor.getPosition();
- turnVelocity = angleMotor.getVelocity();
- turnAppliedVolts = angleMotor.getMotorVoltage();
- turnCurrent = angleMotor.getStatorCurrent();
-
- // Create timestamp queue
- timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
- drivePositionQueue =
- PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
- turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
- updateInputs();
-
- configCANcoder();
- configAngleMotor();
- configDriveMotor();
-
- driveDisconnectedAlert =
- new Alert(
- "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".",
- AlertType.kError);
- turnDisconnectedAlert =
- new Alert(
- "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError);
- turnEncoderDisconnectedAlert =
- new Alert(
- "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".",
- AlertType.kError);
-
-
- // Configure periodic frames
- BaseStatusSignal.setUpdateFrequencyForAll(
- 250, drivePosition, turnPosition);
- BaseStatusSignal.setUpdateFrequencyForAll(
- 50.0,
- driveVelocity,
- driveAppliedVolts,
- driveCurrent,
- turnAbsolutePosition,
- turnVelocity,
- turnAppliedVolts,
- turnCurrent);
-
- setDesiredState(new SwerveModuleState(0, getAngle()), false);
- }
-
- public void close() {
- angleMotor.close();
- driveMotor.close();
- CANcoder.close();
- }
-
- @Override
- public void updateInputs() {
- // Refresh all signals
- var driveStatus =
- BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
- var turnStatus =
- BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
- var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
-
- // Update drive inputs
- inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
- inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
- inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
- inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
- inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
-
- // Update turn inputs
- inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
- inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
- inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
- inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
- inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
-
- // Update encoder inputs
- inputs.encoderOffset = Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble());
-
- // Update odometry inputs
- inputs.odometryTimestamps =
- timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
- inputs.odometryDrivePositionsRad =
- drivePositionQueue.stream()
- .mapToDouble((Double value) -> Units.rotationsToRadians(value))
- .toArray();
- inputs.odometryTurnPositions =
- turnPositionQueue.stream()
- .map((Double value) -> Rotation2d.fromRotations(value))
- .toArray(Rotation2d[]::new);
- timestampQueue.clear();
- drivePositionQueue.clear();
- turnPositionQueue.clear();
-
- inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble();
- inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble();
- inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble();
- inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble();
- }
-
- public void periodic() {
- updateInputs();
- Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
-
- // Calculate positions for odometry
- int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
- odometryPositions = new SwerveModulePosition[sampleCount];
- for (int i = 0; i < sampleCount; i++) {
- double positionMeters = inputs.odometryDrivePositionsRad[i]/DriveConstants.DRIVE_GEAR_RATIO * DriveConstants.WHEEL_RADIUS;
- Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
- }
- // Update alerts
- driveDisconnectedAlert.set(!inputs.driveConnected);
- turnDisconnectedAlert.set(!inputs.turnConnected);
- turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
- }
- }
-
- public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
- // Separate if here and in setAngle() to avoid warning
- if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
- /*
- * This is a custom optimize function, since default WPILib optimize assumes
- * continuous controller which CTRE and Rev onboard is not
- */
- desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
- }else{
- desiredState = wantedState;
- }
- setAngle();
- setSpeed(isOpenLoop);
- }
-
- public void setSpeed(boolean isOpenLoop) {
- if(desiredState == null){
- return;
- }
- if (isOpenLoop) {
- double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
- driveMotor.setControl(new DutyCycleOut(percentOutput));
- } else {
- double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO;
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
- }
-
- double feedforward = velocity * moduleConstants.getDriveV();
- driveMotor.setControl(
- velocityRequest
- .withVelocity(velocity)
- .withFeedForward(feedforward));
- }
- }
-
- private void setAngle() {
- if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
- // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement.
- if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
- stop();
- return;
- }
- }
- if(desiredState == null){
- return;
- }
- angleMotor.setControl(new PositionDutyCycle(desiredState.angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
- }
-
- public void setDriveVoltage(Voltage voltage){
- driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude()));
- }
- public void setAngle(Rotation2d angle){
- angleMotor.setControl(new PositionDutyCycle(angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
- }
-
- public void setOptimize(boolean enable) {
- optimizeStates = enable;
- }
-
- public byte getModuleIndex() {
- return type.id;
- }
-
- public Rotation2d getAngle() {
- return inputs.turnPosition;
- }
-
- public Rotation2d getCANcoder() {
- return inputs.turnAbsolutePosition;
- }
-
- public void resetToAbsolute() {
- // Sensor ticks
- double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
- angleMotor.setPosition(absolutePosition*DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- }
-
- private void configCANcoder() {
- CANcoder.getConfigurator().apply(new CANcoderConfiguration());
- CANcoder.getConfigurator().apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).
- withSensorDirection(DriveConstants.MODULE_CONSTANTS.canCoderInvert?SensorDirectionValue.Clockwise_Positive:SensorDirectionValue.CounterClockwise_Positive));
- }
-
- private void configAngleMotor() {
- angleMotor.getConfigurator().apply(new TalonFXConfiguration());
-
- CurrentLimitsConfigs config = new CurrentLimitsConfigs();
- config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
- config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
- config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
- config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
- angleMotor.getConfigurator().apply(config);
- angleMotor.getConfigurator().apply(new Slot0Configs()
- .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
- .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
- .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
- angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
- angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
- angleMotor.setPosition(0);
-
- // optimize bus utilization for angle motor
- angleMotor.optimizeBusUtilization();
-
- resetToAbsolute();
- }
-
- /**
- * @return Speed in RPM
- */
- public double getDriveVelocity() {
- return inputs.driveVelocityRadPerSec*60/DriveConstants.MODULE_CONSTANTS.driveGearRatio/2/Math.PI;
- }
-
- public double getDriveVoltage(){
- return inputs.driveAppliedVolts;
- }
-
- public double getDriveStatorCurrent(){
- return inputs.driveCurrentAmps;
- }
-
- public double getModuleStatorCurrent() {
- return inputs.steerStator + inputs.driveStator;
- }
-
- public double getModuleSupplyCurrent() {
- return inputs.steerSupply + inputs.driveSupply;
- }
-
- // I took the config things straight from this file
- public void setNewCurrentLimit(double currentSteerStator, double currentSteerSupply, double currentDriveStator, double currentDriveSupply) {
- CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
- // steer
- steerConfig.SupplyCurrentLimitEnable = true;
- steerConfig.StatorCurrentLimitEnable = true;
- steerConfig.StatorCurrentLimit = currentSteerSupply;
- steerConfig.SupplyCurrentLimit = currentSteerSupply;
- steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
- angleMotor.getConfigurator().apply(steerConfig); // apply
-
- // drive
- CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
- driveConfig.SupplyCurrentLimitEnable = true;
- driveConfig.StatorCurrentLimitEnable = true;
- driveConfig.SupplyCurrentLimit = currentDriveSupply;
- driveConfig.StatorCurrentLimit = currentDriveStator;
- driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- driveMotor.getConfigurator().apply(driveConfig); // apply
- }
-
- private void configDriveMotor() {
- var talonFXConfigs = new TalonFXConfiguration();
- // set Motion Magic settings
- var motionMagicConfigs = talonFXConfigs.MotionMagic;
- motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
- motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
- var slot0Configs = talonFXConfigs.Slot0;
- slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
- slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
- slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
- slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
- slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
- slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
- driveMotor.getConfigurator().apply(talonFXConfigs);
- CurrentLimitsConfigs config = new CurrentLimitsConfigs();
- config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
- config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
- config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
- config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- driveMotor.getConfigurator().apply(config);
- driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
- driveMotor.getConfigurator().apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
- driveMotor.getConfigurator().apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
- driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
-
- // optimize bus utilization for drive motor
- driveMotor.optimizeBusUtilization();
-
- }
-
- public SwerveModuleState getState() {
- return new SwerveModuleState(
- inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS,
- getAngle());
- }
-
- public SwerveModulePosition getPosition() {
- return new SwerveModulePosition(
- inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS,
- getAngle());
- }
-
- public SwerveModuleState getDesiredState() {
- return desiredState;
- }
-
-
- public double getDriveVelocityError() {
- return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
- }
-
- public void stop() {
- driveMotor.set(0);
- angleMotor.set(0);
- }
-
- public ModuleType getModuleType(){
- return type;
- }
-
- public void setStateDeadband(boolean enabled) {
- stateDeadband = enabled;
- }
-
- public double getDesiredVelocity() {
- return getDesiredState().speedMetersPerSecond;
+public class Module implements ModuleIO {
+ private final ModuleType type;
+
+ // Degrees
+ private final double angleOffset;
+
+ private final TalonFX angleMotor;
+ private final TalonFX driveMotor;
+ private final CANcoder CANcoder;
+ private SwerveModuleState desiredState;
+
+ protected boolean stateDeadband = true;
+
+ private boolean optimizeStates = true;
+
+ // Inputs from drive motor
+ private final StatusSignal<Angle> drivePosition;
+ private final StatusSignal<AngularVelocity> driveVelocity;
+ private final StatusSignal<Voltage> driveAppliedVolts;
+ private final StatusSignal<Current> driveCurrent;
+
+ // Inputs from turn motor
+ private final StatusSignal<Angle> turnAbsolutePosition;
+ private final StatusSignal<Angle> turnPosition;
+ private final StatusSignal<AngularVelocity> turnVelocity;
+ private final StatusSignal<Voltage> turnAppliedVolts;
+ private final StatusSignal<Current> turnCurrent;
+
+ // Timestamp inputs from Phoenix thread
+ protected final Queue<Double> timestampQueue;
+ protected final Queue<Double> drivePositionQueue;
+ protected final Queue<Double> turnPositionQueue;
+
+ private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+
+ // Connection debouncers
+ private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
+
+ private final Alert driveDisconnectedAlert;
+ private final Alert turnDisconnectedAlert;
+ private final Alert turnEncoderDisconnectedAlert;
+
+ protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
+
+ private ModuleConstants moduleConstants;
+ private final MotionMagicVelocityVoltage velocityRequest = new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
+
+ public Module(ModuleConstants moduleConstants) {
+ this.moduleConstants = moduleConstants;
+
+ type = moduleConstants.getType();
+ angleOffset = moduleConstants.getSteerOffset();
+
+ /* Angle Encoder Config */
+ CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
+ /* Angle Motor Config */
+ angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
+ driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
+ // Create drive status signals
+ drivePosition = driveMotor.getPosition();
+ driveVelocity = driveMotor.getVelocity();
+ driveAppliedVolts = driveMotor.getMotorVoltage();
+ driveCurrent = driveMotor.getStatorCurrent();
+
+ // Create turn status signals
+ turnAbsolutePosition = CANcoder.getAbsolutePosition();
+ turnPosition = angleMotor.getPosition();
+ turnVelocity = angleMotor.getVelocity();
+ turnAppliedVolts = angleMotor.getMotorVoltage();
+ turnCurrent = angleMotor.getStatorCurrent();
+
+ // Create timestamp queue
+ timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+ drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
+ turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
+ updateInputs();
+
+ configCANcoder();
+ configAngleMotor();
+ configDriveMotor();
+
+ driveDisconnectedAlert = new Alert(
+ "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".",
+ AlertType.kError);
+ turnDisconnectedAlert = new Alert(
+ "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError);
+ turnEncoderDisconnectedAlert = new Alert(
+ "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".",
+ AlertType.kError);
+
+ // Configure periodic frames
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 250, drivePosition, turnPosition);
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 50.0,
+ driveVelocity,
+ driveAppliedVolts,
+ driveCurrent,
+ turnAbsolutePosition,
+ turnVelocity,
+ turnAppliedVolts,
+ turnCurrent);
+
+ setDesiredState(new SwerveModuleState(0, getAngle()), false);
+ }
+
+ public void close() {
+ angleMotor.close();
+ driveMotor.close();
+ CANcoder.close();
+ }
+
+ @Override
+ public void updateInputs() {
+ // Refresh all signals
+ var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
+ var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
+ var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
+
+ // Update drive inputs
+ inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
+ inputs.drivePositionRad = Units
+ .rotationsToRadians(drivePosition.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveVelocityRadPerSec = Units
+ .rotationsToRadians(driveVelocity.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
+ inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
+
+ // Update turn inputs
+ inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
+ inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
+ inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
+ inputs.turnPosition = Rotation2d
+ .fromRotations(turnPosition.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnVelocityRadPerSec = Units
+ .rotationsToRadians(turnVelocity.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
+ inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
+
+ // Update encoder inputs
+ inputs.encoderOffset = Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble());
+
+ // Update odometry inputs
+ inputs.odometryTimestamps = timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryDrivePositionsRad = drivePositionQueue.stream()
+ .mapToDouble((Double value) -> Units.rotationsToRadians(value))
+ .toArray();
+ inputs.odometryTurnPositions = turnPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromRotations(value))
+ .toArray(Rotation2d[]::new);
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+
+ inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble();
+ inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble();
+ inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble();
+ inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble();
+ }
+
+ public void periodic() {
+ updateInputs();
+ Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
+
+ // Calculate positions for odometry
+ int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
+ odometryPositions = new SwerveModulePosition[sampleCount];
+ for (int i = 0; i < sampleCount; i++) {
+ double positionMeters = inputs.odometryDrivePositionsRad[i] / DriveConstants.DRIVE_GEAR_RATIO
+ * DriveConstants.WHEEL_RADIUS;
+ Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
+ }
+ // Update alerts
+ driveDisconnectedAlert.set(!inputs.driveConnected);
+ turnDisconnectedAlert.set(!inputs.turnConnected);
+ turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Angle " + moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
+ }
+ }
+
+ public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
+ // Separate if here and in setAngle() to avoid warning
+ if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
+ /*
+ * This is a custom optimize function, since default WPILib optimize assumes
+ * continuous controller which CTRE and Rev onboard is not
+ */
+ desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
+ } else {
+ desiredState = wantedState;
+ }
+ setAngle();
+ setSpeed(isOpenLoop);
+ }
+
+ public void setSpeed(boolean isOpenLoop) {
+ if (desiredState == null) {
+ return;
+ }
+ if (isOpenLoop) {
+ double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
+ driveMotor.setControl(new DutyCycleOut(percentOutput));
+ } else {
+ double velocity = desiredState.speedMetersPerSecond / DriveConstants.WHEEL_RADIUS / 2 / Math.PI
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
}
-
- public Rotation2d getDesiredAngle() {
- return getDesiredState().angle;
- }
-
- /** Returns the module positions received this cycle. */
- public SwerveModulePosition[] getOdometryPositions() {
- return odometryPositions;
- }
- /** Returns the timestamps of the samples received this cycle. */
- public double[] getOdometryTimestamps() {
- return inputs.odometryTimestamps;
- }
-
- /** returns the drive position status signal for time-synced odometry. */
- public StatusSignal<Angle> getDrivePositionSignal() {
- return drivePosition;
- }
-
- /** returns the turn position status signal for time-synced odometry. */
- public StatusSignal<Angle> getTurnPositionSignal() {
- return turnPosition;
- }
-
- /** returns the turn absolute position status signal for time-synced odometry. */
- public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
- return turnAbsolutePosition;
- }
-
- public TalonFX[] getMotors() {
- return new TalonFX[]{angleMotor, driveMotor};
+ double feedforward = velocity * moduleConstants.getDriveV();
+ driveMotor.setControl(
+ velocityRequest
+ .withVelocity(velocity)
+ .withFeedForward(feedforward));
+ }
+ }
+
+ private void setAngle() {
+ if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
+ // Prevent rotating module if desired speed < 1%. Prevents jittering and
+ // unnecessary movement.
+ if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
+ stop();
+ return;
+ }
}
+ if (desiredState == null) {
+ return;
+ }
+ angleMotor.setControl(
+ new PositionDutyCycle(desiredState.angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setDriveVoltage(Voltage voltage) {
+ driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude()));
+ }
+
+ public void setAngle(Rotation2d angle) {
+ angleMotor.setControl(new PositionDutyCycle(angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setOptimize(boolean enable) {
+ optimizeStates = enable;
+ }
+
+ public byte getModuleIndex() {
+ return type.id;
+ }
+
+ public Rotation2d getAngle() {
+ return inputs.turnPosition;
+ }
+
+ public Rotation2d getCANcoder() {
+ return inputs.turnAbsolutePosition;
+ }
+
+ public void resetToAbsolute() {
+ // Sensor ticks
+ double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
+ angleMotor.setPosition(absolutePosition * DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ }
+
+ private void configCANcoder() {
+ CANcoder.getConfigurator().apply(new CANcoderConfiguration());
+ CANcoder.getConfigurator()
+ .apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).withSensorDirection(
+ DriveConstants.MODULE_CONSTANTS.canCoderInvert ? SensorDirectionValue.Clockwise_Positive
+ : SensorDirectionValue.CounterClockwise_Positive));
+ }
+
+ private void configAngleMotor() {
+ angleMotor.getConfigurator().apply(new TalonFXConfiguration());
+
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(config);
+ angleMotor.getConfigurator().apply(new Slot0Configs()
+ .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
+ .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
+ .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
+ angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
+ angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
+ angleMotor.setPosition(0);
+
+ // optimize bus utilization for angle motor
+ angleMotor.optimizeBusUtilization();
+
+ resetToAbsolute();
+ }
+
+ /**
+ * @return Speed in RPM
+ */
+ public double getDriveVelocity() {
+ return inputs.driveVelocityRadPerSec * 60 / DriveConstants.MODULE_CONSTANTS.driveGearRatio / 2 / Math.PI;
+ }
+
+ public double getDriveVoltage() {
+ return inputs.driveAppliedVolts;
+ }
+
+ public double getDriveStatorCurrent() {
+ return inputs.driveCurrentAmps;
+ }
+
+ public double getModuleStatorCurrent() {
+ return inputs.steerStator + inputs.driveStator;
+ }
+
+ public double getModuleSupplyCurrent() {
+ return inputs.steerSupply + inputs.driveSupply;
+ }
+
+ // I took the config things straight from this file
+ public void setNewCurrentLimit(double currentSteerStator, double currentSteerSupply, double currentDriveStator,
+ double currentDriveSupply) {
+ CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
+ // steer
+ steerConfig.SupplyCurrentLimitEnable = true;
+ steerConfig.StatorCurrentLimitEnable = true;
+ steerConfig.StatorCurrentLimit = currentSteerSupply;
+ steerConfig.SupplyCurrentLimit = currentSteerSupply;
+ steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(steerConfig); // apply
+
+ // drive
+ CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
+ driveConfig.SupplyCurrentLimitEnable = true;
+ driveConfig.StatorCurrentLimitEnable = true;
+ driveConfig.SupplyCurrentLimit = currentDriveSupply;
+ driveConfig.StatorCurrentLimit = currentDriveStator;
+ driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ driveMotor.getConfigurator().apply(driveConfig); // apply
+ }
+
+ private void configDriveMotor() {
+ var talonFXConfigs = new TalonFXConfiguration();
+ // set Motion Magic settings
+ var motionMagicConfigs = talonFXConfigs.MotionMagic;
+ motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED / DriveConstants.WHEEL_CIRCUMFERENCE
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL / DriveConstants.WHEEL_CIRCUMFERENCE
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ var slot0Configs = talonFXConfigs.Slot0;
+ slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
+ slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
+ slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
+ slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
+ slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
+ slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
+ driveMotor.getConfigurator().apply(talonFXConfigs);
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveMotor.getConfigurator().apply(config);
+ driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
+ driveMotor.getConfigurator()
+ .apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
+ driveMotor.getConfigurator()
+ .apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
+ driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
+
+ // optimize bus utilization for drive motor
+ driveMotor.optimizeBusUtilization();
+
+ }
+
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(
+ inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS,
+ getAngle());
+ }
+
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(
+ inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS,
+ getAngle());
+ }
+
+ public SwerveModuleState getDesiredState() {
+ return desiredState;
+ }
+
+ public double getDriveVelocityError() {
+ return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
+ }
+
+ public void stop() {
+ driveMotor.set(0);
+ angleMotor.set(0);
+ }
+
+ public ModuleType getModuleType() {
+ return type;
+ }
+
+ public void setStateDeadband(boolean enabled) {
+ stateDeadband = enabled;
+ }
+
+ public double getDesiredVelocity() {
+ return getDesiredState().speedMetersPerSecond;
+ }
+
+ public Rotation2d getDesiredAngle() {
+ return getDesiredState().angle;
+ }
+
+ /** Returns the module positions received this cycle. */
+ public SwerveModulePosition[] getOdometryPositions() {
+ return odometryPositions;
+ }
+
+ /** Returns the timestamps of the samples received this cycle. */
+ public double[] getOdometryTimestamps() {
+ return inputs.odometryTimestamps;
+ }
+
+ /** returns the drive position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getDrivePositionSignal() {
+ return drivePosition;
+ }
+
+ /** returns the turn position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getTurnPositionSignal() {
+ return turnPosition;
+ }
+
+ /**
+ * returns the turn absolute position status signal for time-synced odometry.
+ */
+ public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
+ return turnAbsolutePosition;
+ }
+
+ public TalonFX[] getMotors() {
+ return new TalonFX[] { angleMotor, driveMotor };
+ }
}
import frc.robot.util.GeomUtil;
/**
- * "Inspired" by FRC team 254. See the license file in the root directory of this project.
+ * "Inspired" by FRC team 254. See the license file in the root directory of
+ * this project.
*
- * <p>Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path
- * follower), and outputs a new setpoint that respects all of the kinematic constraints on module
- * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the
- * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is
- * kinematically infeasible (and can result in wheel slip or robot heading drift as a result).
+ * <p>
+ * Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or
+ * from a path
+ * follower), and outputs a new setpoint that respects all of the kinematic
+ * constraints on module
+ * rotation speed and wheel velocity/acceleration. By generating a new setpoint
+ * every iteration, the
+ * robot will converge to the desired setpoint quickly while avoiding any
+ * intermediate state that is
+ * kinematically infeasible (and can result in wheel slip or robot heading drift
+ * as a result).
*/
public class SwerveSetpointGenerator {
- private final SwerveDriveKinematics kinematics= DriveConstants.KINEMATICS;
+ private final SwerveDriveKinematics kinematics = DriveConstants.KINEMATICS;
private final Translation2d[] moduleLocations = DriveConstants.MODULE_LOCATIONS;
/**
- * Check if it would be faster to go to the opposite of the goal heading (and reverse drive
+ * Check if it would be faster to go to the opposite of the goal heading (and
+ * reverse drive
* direction).
*
- * @param prevToGoal The rotation from the previous state to the goal state (i.e.
- * prev.inverse().rotateBy(goal)).
- * @return True if the shortest path to achieve this rotation involves flipping the drive
- * direction.
+ * @param prevToGoal The rotation from the previous state to the goal state
+ * (i.e.
+ * prev.inverse().rotateBy(goal)).
+ * @return True if the shortest path to achieve this rotation involves flipping
+ * the drive
+ * direction.
*/
private boolean flipHeading(Rotation2d prevToGoal) {
return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0;
}
/**
- * Find the root of the generic 2D parametric function 'func' using the regula falsi technique.
- * This is a pretty naive way to do root finding, but it's usually faster than simple bisection
+ * Find the root of the generic 2D parametric function 'func' using the regula
+ * falsi technique.
+ * This is a pretty naive way to do root finding, but it's usually faster than
+ * simple bisection
* while being robust in ways that e.g. the Newton-Raphson method isn't.
*
- * @param func The Function2d to take the root of.
- * @param x_0 x value of the lower bracket.
- * @param y_0 y value of the lower bracket.
- * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during
- * recursion)
- * @param x_1 x value of the upper bracket.
- * @param y_1 y value of the upper bracket.
- * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during
- * recursion)
+ * @param func The Function2d to take the root of.
+ * @param x_0 x value of the lower bracket.
+ * @param y_0 y value of the lower bracket.
+ * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to
+ * save a call to 'func' during
+ * recursion)
+ * @param x_1 x value of the upper bracket.
+ * @param y_1 y value of the upper bracket.
+ * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to
+ * save a call to 'func' during
+ * recursion)
* @param iterations_left Number of iterations of root finding left.
- * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the
- * (approximate) root.
+ * @return The parameter value 's' that interpolating between 0 and 1 that
+ * corresponds to the
+ * (approximate) root.
*/
private double findRoot(
Function2d func,
return 1.0;
}
double offset = f_0 + Math.signum(diff) * max_deviation;
- Function2d func =
- (x, y) -> {
- return unwrapAngle(f_0, Math.atan2(y, x)) - offset;
- };
+ Function2d func = (x, y) -> {
+ return unwrapAngle(f_0, Math.atan2(y, x)) - offset;
+ };
return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations);
}
return 1.0;
}
double offset = f_0 + Math.signum(diff) * max_vel_step;
- Function2d func =
- (x, y) -> {
- return Math.hypot(x, y) - offset;
- };
+ Function2d func = (x, y) -> {
+ return Math.hypot(x, y) - offset;
+ };
return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations);
}
/**
* Limits the acceleration in all directions.
- * This is different from findDriveMaxS because it includes the acceleration perpendicular to the wheel as it rotates.
- * Given the same velocity step, this will return a lower S value than findDriveMaxS.
- * @param x_0 The initial x velocity
- * @param y_0 The initial y velocity
- * @param x_1 The final x velocity
- * @param y_1 The final y velocity
- * @param max_vel_step The maxiumum amount the velocity can change this frame
+ * This is different from findDriveMaxS because it includes the acceleration
+ * perpendicular to the wheel as it rotates.
+ * Given the same velocity step, this will return a lower S value than
+ * findDriveMaxS.
+ *
+ * @param x_0 The initial x velocity
+ * @param y_0 The initial y velocity
+ * @param x_1 The final x velocity
+ * @param y_1 The final y velocity
+ * @param max_vel_step The maxiumum amount the velocity can change this frame
* @param max_iterations The maximum number of iterations to use in findRoot
* @return The maximum interpolation value
*/
double y_1,
double max_vel_step,
int max_iterations) {
- double dist = Math.hypot(x_1-x_0, y_1-y_0);
- if(dist <= max_vel_step){
+ double dist = Math.hypot(x_1 - x_0, y_1 - y_0);
+ if (dist <= max_vel_step) {
return 1;
}
return Math.max(0.0, Math.min(1.0, max_vel_step / dist));
// length of P(s) is the target T. P(s) is linearly interpolated between the
// points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s).
// Then,
- // T = sqrt(P(s).x^2 + P(s).y^2)
- // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2
- // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2
- // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2
- // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2
- // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2
- // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2
- // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s
- // + (x_0^2 + y_0^2 - T^2).
+ // T = sqrt(P(s).x^2 + P(s).y^2)
+ // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2
+ // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2
+ // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2
+ // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2
+ // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2
+ // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2
+ // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s
+ // + (x_0^2 + y_0^2 - T^2).
//
// To simplify, we can factor out some common parts:
// Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and
// p = x_0 * x_1 + y_0 * y_1.
// Then we have
- // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2),
+ // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2),
// with which we can solve for s using the quadratic formula.
double l_0 = x_0 * x_0 + y_0 * y_0;
/**
* Generate a new setpoint.
*
- * @param limits The kinematic limits to respect for this setpoint.
- * @param centerOfMassHeight The height of the robot's center of mass, in meters, off the ground.
- * This assumes that the center of mass is in the center of the robot in the x and y directions.
- * If tipping is not a potential problem this year, set this to 0.
- * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous
- * iteration setpoint instead of the actual measured/estimated kinematic state.
- * @param desiredState The desired state of motion, such as from the driver sticks or a path
- * following algorithm.
- * @param dt The loop time.
- * @return A Setpoint object that satisfies all of the KinematicLimits while converging to
- * desiredState quickly.
+ * @param limits The kinematic limits to respect for this setpoint.
+ * @param centerOfMassHeight The height of the robot's center of mass, in
+ * meters, off the ground.
+ * This assumes that the center of mass is in the
+ * center of the robot in the x and y directions.
+ * If tipping is not a potential problem this year,
+ * set this to 0.
+ * @param prevSetpoint The previous setpoint motion. Normally, you'd pass
+ * in the previous
+ * iteration setpoint instead of the actual
+ * measured/estimated kinematic state.
+ * @param desiredState The desired state of motion, such as from the
+ * driver sticks or a path
+ * following algorithm.
+ * @param dt The loop time.
+ * @return A Setpoint object that satisfies all of the KinematicLimits while
+ * converging to
+ * desiredState quickly.
*/
public SwerveSetpoint generateSetpoint(
final ModuleLimits limits,
desiredState = kinematics.toChassisSpeeds(desiredModuleState);
}
- // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so
+ // Special case: desiredState is a complete stop. In this case, module angle is
+ // arbitrary, so
// just use the previous angle.
boolean need_to_steer = true;
- if (EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) {
+ if (EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState), new Twist2d())) {
need_to_steer = false;
for (int i = 0; i < modules.length; ++i) {
desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle;
Rotation2d[] desired_heading = new Rotation2d[modules.length];
boolean all_modules_should_flip = true;
for (int i = 0; i < modules.length; ++i) {
- prev_vx[i] =
- prevSetpoint.moduleStates()[i].angle.getCos()
- * prevSetpoint.moduleStates()[i].speedMetersPerSecond;
- prev_vy[i] =
- prevSetpoint.moduleStates()[i].angle.getSin()
- * prevSetpoint.moduleStates()[i].speedMetersPerSecond;
+ prev_vx[i] = prevSetpoint.moduleStates()[i].angle.getCos()
+ * prevSetpoint.moduleStates()[i].speedMetersPerSecond;
+ prev_vy[i] = prevSetpoint.moduleStates()[i].angle.getSin()
+ * prevSetpoint.moduleStates()[i].speedMetersPerSecond;
prev_heading[i] = prevSetpoint.moduleStates()[i].angle;
if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) {
prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI));
}
- desired_vx[i] =
- desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond;
- desired_vy[i] =
- desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond;
+ desired_vx[i] = desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond;
+ desired_vy[i] = desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond;
desired_heading[i] = desiredModuleState[i].angle;
if (desiredModuleState[i].speedMetersPerSecond < 0.0) {
desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI));
}
if (all_modules_should_flip) {
- double required_rotation_rad =
- Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians());
+ double required_rotation_rad = Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians());
if (required_rotation_rad < Math.PI / 2.0) {
all_modules_should_flip = false;
}
}
}
if (all_modules_should_flip
- && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(prevSetpoint.chassisSpeeds()),new Twist2d())
- && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) {
- // It will (likely) be faster to stop the robot, rotate the modules in place to the complement
+ && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(prevSetpoint.chassisSpeeds()), new Twist2d())
+ && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState), new Twist2d())) {
+ // It will (likely) be faster to stop the robot, rotate the modules in place to
+ // the complement
// of the desired
// angle, and accelerate again.
return generateSetpoint(limits, centerOfMassHeight, prevSetpoint, new ChassisSpeeds(), dt);
}
- // Compute the deltas between start and goal. We can then interpolate from the start state to
+ // Compute the deltas between start and goal. We can then interpolate from the
+ // start state to
// the goal state; then
- // find the amount we can move from start towards goal in this cycle such that no kinematic
+ // find the amount we can move from start towards goal in this cycle such that
+ // no kinematic
// limit is exceeded.
double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond;
double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond;
- double dtheta =
- desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond;
+ double dtheta = desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond;
- // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at
+ // 's' interpolates between start and goal. At 0, we are at prevState and at 1,
+ // we are at
// desiredState.
double min_s = 1.0;
- // In cases where an individual module is stopped, we want to remember the right steering angle
+ // In cases where an individual module is stopped, we want to remember the right
+ // steering angle
// to command (since
- // inverse kinematics doesn't care about angle, we can be opportunistically lazy).
+ // inverse kinematics doesn't care about angle, we can be opportunistically
+ // lazy).
List<Optional<Rotation2d>> overrideSteering = new ArrayList<>(modules.length);
- // Enforce steering velocity limits. We do this by taking the derivative of steering angle at
+ // Enforce steering velocity limits. We do this by taking the derivative of
+ // steering angle at
// the current angle,
- // and then backing out the maximum interpolant between start and goal states. We remember the
+ // and then backing out the maximum interpolant between start and goal states.
+ // We remember the
// minimum across all modules, since
// that is the active constraint.
final double max_theta_step = dt * limits.maxSteeringVelocity();
}
overrideSteering.add(Optional.empty());
if (epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) {
- // If module is stopped, we know that we will need to move straight to the final steering
+ // If module is stopped, we know that we will need to move straight to the final
+ // steering
// angle, so limit based
// purely on rotation in place.
if (epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) {
continue;
}
- var necessaryRotation =
- prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle);
+ var necessaryRotation = prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle);
if (flipHeading(necessaryRotation)) {
necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI));
}
}
final int kMaxIterations = 8;
- double s =
- findSteeringMaxS(
- prev_vx[i],
- prev_vy[i],
- prev_heading[i].getRadians(),
- desired_vx[i],
- desired_vy[i],
- desired_heading[i].getRadians(),
- max_theta_step,
- kMaxIterations);
+ double s = findSteeringMaxS(
+ prev_vx[i],
+ prev_vy[i],
+ prev_heading[i].getRadians(),
+ desired_vx[i],
+ desired_vy[i],
+ desired_heading[i].getRadians(),
+ max_theta_step,
+ kMaxIterations);
min_s = Math.min(min_s, s);
}
// No need to carry on.
break;
}
- double vx_min_s =
- min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i];
- double vy_min_s =
- min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i];
- // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we
+ double vx_min_s = min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i];
+ double vy_min_s = min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i];
+ // Find the max s for this drive wheel. Search on the interval between 0 and
+ // min_s, because we
// already know we can't go faster
// than that.
final int kMaxIterations = 10;
- double s = min_s*findDriveMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step);
-
+ double s = min_s * findDriveMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step);
+
// Limit the overall acceleration of this wheel
- double s2 = min_s*findAccelerationMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step_2, kMaxIterations);
+ double s2 = min_s
+ * findAccelerationMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step_2, kMaxIterations);
min_s = Math.min(Math.min(min_s, s), s2);
}
- if(centerOfMassHeight > 0.02){
- // Limit the acceleration in the x and y directions separately based on the center of mass.
- // To make the torque on the robot 0, we can assume all of the mass is on the back wheel, where the front is the direction the robot is accelerating toward
- // Torque is equal to the force times the component of the radius perpendicular to the force
- // T = torque, m = mass, a = acceleration, g = gravity acceleration, x = distance from center to wheel
+ if (centerOfMassHeight > 0.02) {
+ // Limit the acceleration in the x and y directions separately based on the
+ // center of mass.
+ // To make the torque on the robot 0, we can assume all of the mass is on the
+ // back wheel, where the front is the direction the robot is accelerating toward
+ // Torque is equal to the force times the component of the radius perpendicular
+ // to the force
+ // T = torque, m = mass, a = acceleration, g = gravity acceleration, x =
+ // distance from center to wheel
// T = mgx - mah = 0
// a = gx/h
- double maxAccel = Constants.GRAVITY_ACCELERATION*(DriveConstants.TRACK_WIDTH/2)/centerOfMassHeight;
+ double maxAccel = Constants.GRAVITY_ACCELERATION * (DriveConstants.TRACK_WIDTH / 2) / centerOfMassHeight;
// Limit based on this calculated value
- // x and y are limited separately because, when tipping in a diagonal direction, the distance is longer
+ // x and y are limited separately because, when tipping in a diagonal direction,
+ // the distance is longer
double xAccel = Math.abs(desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond) / dt;
double yAccel = Math.abs(desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond) / dt;
- if(!epsilonEquals(xAccel, 0)){
+ if (!epsilonEquals(xAccel, 0)) {
double s = maxAccel / xAccel;
min_s = Math.min(min_s, s);
}
- if(!epsilonEquals(yAccel, 0)){
+ if (!epsilonEquals(yAccel, 0)) {
double s = maxAccel / yAccel;
min_s = Math.min(min_s, s);
}
}
- ChassisSpeeds retSpeeds =
- new ChassisSpeeds(
- prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx,
- prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy,
- prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta);
+ ChassisSpeeds retSpeeds = new ChassisSpeeds(
+ prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx,
+ prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy,
+ prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta);
var retStates = kinematics.toSwerveModuleStates(retSpeeds);
for (int i = 0; i < modules.length; ++i) {
final var maybeOverride = overrideSteering.get(i);
retStates[i].speedMetersPerSecond *= -1.0;
}
retStates[i].angle = override;
- }
- final var deltaRotation =
- prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle);
+ }
+ final var deltaRotation = prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle);
if (flipHeading(deltaRotation)) {
retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI));
retStates[i].speedMetersPerSecond *= -1.0;
private NetworkTableEntry objectDistance;
private NetworkTableEntry objectClass;
private NetworkTableEntry cameraIndex;
-
+
// A list of the cameras on the robot.
private ArrayList<VisionCamera> cameras = new ArrayList<>();
// Sets the origin to the right side of the blue alliance wall
FieldConstants.field.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
- if(VisionConstants.ENABLED){
+ if (VisionConstants.ENABLED) {
// Puts the cameras in an array list
for (int i = 0; i < camList.size(); i++) {
cameras.add(new VisionCamera(camList.get(i).getFirst(), camList.get(i).getSecond()));
}
- if(RobotBase.isSimulation()){
+ if (RobotBase.isSimulation()) {
visionSim = new VisionSystemSim("Vision");
visionSim.addAprilTags(FieldConstants.field);
- for(VisionCamera c : cameras){
+ for (VisionCamera c : cameras) {
PhotonCameraSim cameraSim = new PhotonCameraSim(c.camera);
cameraSim.enableDrawWireframe(true);
cameraSim.prop.setAvgLatencyMs(30);
Pose3d[] tags = new Pose3d[FieldConstants.field.getTags().size()];
for (int i = 0; i < FieldConstants.field.getTags().size(); i++) {
- tags[i] = (FieldConstants.field.getTagPose(i+1).get());
+ tags[i] = (FieldConstants.field.getTagPose(i + 1).get());
}
if (!Constants.DISABLE_LOGGING) {
Logger.recordOutput("AprilTags", tags);
}
}
-
/**
* Get the horizontal offsets from the crosshair to the targets
+ *
* @return An array of offsets in degrees
*/
- public double[] getHorizontalOffset(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ public double[] getHorizontalOffset() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
return new double[0];
}
return xOffset.getDoubleArray(new double[0]);
/**
* Get the vertical offsets from the crosshair to the targets
+ *
* @return An array of offsets in degrees
*/
- public double[] getVerticalOffset(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ public double[] getVerticalOffset() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
return new double[0];
}
return yOffset.getDoubleArray(new double[0]);
/**
* Get the target distances
+ *
* @return Distance in meters
*/
@SuppressWarnings("unused")
- public double[] getDistance(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+ public double[] getDistance() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) {
return new double[0];
}
return objectDistance.getDoubleArray(new double[0]);
/**
* Returns whether or not a valid object is detected
+ *
* @return true or false
*/
- public boolean validObjectDetected(){
+ public boolean validObjectDetected() {
return getHorizontalOffset().length > 0;
}
/**
* Returns what types of object are detected
+ *
* @return The object types as a String array
*/
@SuppressWarnings("unused")
- public String[] getDetectedObjectClass(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+ public String[] getDetectedObjectClass() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) {
return new String[0];
}
return objectClass.getStringArray(new String[0]);
/**
* Gets the camera indices (which camera sees the object)
- * @return The indices as a long array (method returns long array instead of int array)
+ *
+ * @return The indices as a long array (method returns long array instead of int
+ * array)
*/
@SuppressWarnings("unused")
- public long[] getCameraIndex(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+ public long[] getCameraIndex() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) {
return new long[0];
}
return cameraIndex.getIntegerArray(new long[0]);
/**
* Stores all of the detected objects in an array
+ *
* @return The array of DetectedObjects
*/
- public DetectedObject[] getDetectedObjects(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ public DetectedObject[] getDetectedObjects() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
return new DetectedObject[0];
}
double[] xOffset = getHorizontalOffset();
String[] objectClass = getDetectedObjectClass();
// long[] cameraIndex = getCameraIndex();
DetectedObject[] objects = new DetectedObject[Math.min(xOffset.length, yOffset.length)];
- for(int i = 0; i < objects.length; i++){
+ for (int i = 0; i < objects.length; i++) {
objects[i] = new DetectedObject(
- Units.degreesToRadians(xOffset[i]),
- -Units.degreesToRadians(yOffset[i]),
- // distance[i],
- objectClass[i],
- // VisionConstants.OBJECT_DETECTION_CAMERAS.get((int)cameraIndex[i]).getSecond()
- VisionConstants.OBJECT_DETECTION_CAMERAS.get(0)
- );
+ Units.degreesToRadians(xOffset[i]),
+ -Units.degreesToRadians(yOffset[i]),
+ // distance[i],
+ objectClass[i],
+ // VisionConstants.OBJECT_DETECTION_CAMERAS.get((int)cameraIndex[i]).getSecond()
+ VisionConstants.OBJECT_DETECTION_CAMERAS.get(0));
}
return objects;
}
/**
* Returns the closest game piece in front of the robot
- * @param maxAngle The maximum angle between the angle to the object and the robot's heading or rotation to use, in radians
- * @param relativeToVelocity Whether to compare the angle to the robot's heading or rotation, true for heading
+ *
+ * @param maxAngle The maximum angle between the angle to the object
+ * and the robot's heading or rotation to use, in
+ * radians
+ * @param relativeToVelocity Whether to compare the angle to the robot's heading
+ * or rotation, true for heading
* @return The best DetectedObject
*/
- public DetectedObject getBestGamePiece(double maxAngle, boolean relativeToVelocity){
+ public DetectedObject getBestGamePiece(double maxAngle, boolean relativeToVelocity) {
DetectedObject[] objects = getDetectedObjects();
DetectedObject best = null;
double closest = Double.POSITIVE_INFINITY;
- for(DetectedObject object : objects){
+ for (DetectedObject object : objects) {
double dist = object.getDistance();
- if(object.isGamePiece() && Math.abs(relativeToVelocity ? object.getVelocityRelativeAngle() : object.getAngle()) < maxAngle && dist < closest){
+ if (object.isGamePiece()
+ && Math.abs(relativeToVelocity ? object.getVelocityRelativeAngle() : object.getAngle()) < maxAngle
+ && dist < closest) {
closest = dist;
best = object;
}
/**
* Gets the pose as a Pose2d using PhotonVision
- * @param referencePoses The reference poses in order of preference, null poses will be skipped
+ *
+ * @param referencePoses The reference poses in order of preference, null poses
+ * will be skipped
* @return The pose of the robot, or null if it can't see april tags
*/
- public Pose2d getPose2d(Pose2d... referencePoses){
+ public Pose2d getPose2d(Pose2d... referencePoses) {
Pose2d referencePose = new Pose2d();
- for (Pose2d checkReferencePose:referencePoses){
+ for (Pose2d checkReferencePose : referencePoses) {
if (checkReferencePose != null) {
referencePose = checkReferencePose;
break;
}
}
ArrayList<EstimatedRobotPose> estimatedPoses = getEstimatedPoses(referencePose);
-
- if (estimatedPoses.size() == 0) return null;
- if (estimatedPoses.size() == 1) return estimatedPoses.get(0).estimatedPose.toPose2d();
-
+ if (estimatedPoses.size() == 0)
+ return null;
+
+ if (estimatedPoses.size() == 1)
+ return estimatedPoses.get(0).estimatedPose.toPose2d();
+
if (estimatedPoses.size() == 2) {
return new Pose2d(
- estimatedPoses.get(0).estimatedPose.toPose2d().getTranslation()
- .plus(estimatedPoses.get(1).estimatedPose.toPose2d().getTranslation())
- .div(2),
-
- new Rotation2d(MathUtils.modulusMidpoint(
- estimatedPoses.get(0).estimatedPose.toPose2d().getRotation().getRadians(),
- estimatedPoses.get(1).estimatedPose.toPose2d().getRotation().getRadians(),
- -Math.PI, Math.PI)
- )
- );
+ estimatedPoses.get(0).estimatedPose.toPose2d().getTranslation()
+ .plus(estimatedPoses.get(1).estimatedPose.toPose2d().getTranslation())
+ .div(2),
+
+ new Rotation2d(MathUtils.modulusMidpoint(
+ estimatedPoses.get(0).estimatedPose.toPose2d().getRotation().getRadians(),
+ estimatedPoses.get(1).estimatedPose.toPose2d().getRotation().getRadians(),
+ -Math.PI, Math.PI)));
}
-
- // The average translation is just the average of all of the translations (sum divided by total)
- // Average angle is similar, except every step needs to use a modulus, since -π is the same angle as π
- // This calculation is essentially newAverage = (oldAverage * valuesInOldAverage + nextValue) / newNumberOfValues
+
+ // The average translation is just the average of all of the translations (sum
+ // divided by total)
+ // Average angle is similar, except every step needs to use a modulus, since -Ï€
+ // is the same angle as π
+ // This calculation is essentially newAverage = (oldAverage * valuesInOldAverage
+ // + nextValue) / newNumberOfValues
Translation2d translation = new Translation2d();
double angle = 0;
- for(int i = 0; i < estimatedPoses.size(); i ++){
+ for (int i = 0; i < estimatedPoses.size(); i++) {
translation = translation.plus(estimatedPoses.get(i).estimatedPose.toPose2d().getTranslation());
- angle = MathUtils.modulusInterpolate(angle, estimatedPoses.get(i).estimatedPose.toPose2d().getRotation().getRadians(), 1.0/(i+1), -Math.PI, Math.PI);
+ angle = MathUtils.modulusInterpolate(angle,
+ estimatedPoses.get(i).estimatedPose.toPose2d().getRotation().getRadians(), 1.0 / (i + 1), -Math.PI, Math.PI);
}
return new Pose2d(translation.div(estimatedPoses.size()), new Rotation2d(angle));
}
- public AprilTagFieldLayout getAprilTagFieldLayout(){
+ public AprilTagFieldLayout getAprilTagFieldLayout() {
return FieldConstants.field;
}
/**
* Gets the pose of an april tag
+ *
* @param id AprilTag id (1-8)
* @return Pose3d of the AprilTag
*/
- public Pose3d getTagPose(int id){
- if(id < 1 || id > getAprilTagFieldLayout().getTags().size()){
- System.out.println("Tried to find the pose of april tag "+id);
+ public Pose3d getTagPose(int id) {
+ if (id < 1 || id > getAprilTagFieldLayout().getTags().size()) {
+ System.out.println("Tried to find the pose of april tag " + id);
return null;
}
- return getAprilTagFieldLayout().getTags().get(id-1).pose;
+ return getAprilTagFieldLayout().getTags().get(id - 1).pose;
}
/**
* Returns where it thinks the robot is
- * @param referencePose The pose to use as a reference, usually the previous robot pose
- * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
- * @return An array list of estimated poses, one for each camera that can see an april tag
+ *
+ * @param referencePose The pose to use as a reference, usually the previous
+ * robot pose
+ * @param yawFunction A unary operator that takes a timestamp and returns the
+ * yaw at that time
+ * @return An array list of estimated poses, one for each camera that can see an
+ * april tag
*/
public ArrayList<EstimatedRobotPose> getEstimatedPoses(Pose2d referencePose) {
- return getEstimatedPoses(referencePose, ignoree->referencePose.getRotation().getRadians());
+ return getEstimatedPoses(referencePose, ignoree -> referencePose.getRotation().getRadians());
}
/**
* Returns where it thinks the robot is
- * @param referencePose The pose to use as a reference, usually the previous robot pose
- * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
- * @return An array list of estimated poses, one for each camera that can see an april tag
+ *
+ * @param referencePose The pose to use as a reference, usually the previous
+ * robot pose
+ * @param yawFunction A unary operator that takes a timestamp and returns the
+ * yaw at that time
+ * @return An array list of estimated poses, one for each camera that can see an
+ * april tag
*/
public ArrayList<EstimatedRobotPose> getEstimatedPoses(Pose2d referencePose, DoubleUnaryOperator yawFunction) {
ArrayList<EstimatedRobotPose> estimatedPoses = new ArrayList<>();
for (int i = 0; i < cameras.size(); i++) {
- if(VisionConstants.USE_MANUAL_CALCULATIONS){
- for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(yawFunction)){
- if(pose != null){
+ if (VisionConstants.USE_MANUAL_CALCULATIONS) {
+ for (EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(yawFunction)) {
+ if (pose != null) {
estimatedPoses.add(pose);
}
}
- }else{
- for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(referencePose)){
+ } else {
+ for (EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(referencePose)) {
// If the camera can see an april tag that exists, add it to the array list
- // April tags that don't exist might return a result that is present but doesn't have a pose
+ // April tags that don't exist might return a result that is present but doesn't
+ // have a pose
if (pose.estimatedPose != null) {
estimatedPoses.add(pose);
}
}
}
- if(estimatedPoses.size() > 1){
+ if (estimatedPoses.size() > 1) {
Translation2d average = new Translation2d();
- for(EstimatedRobotPose pose : estimatedPoses){
+ for (EstimatedRobotPose pose : estimatedPoses) {
average = average.plus(pose.estimatedPose.getTranslation().toTranslation2d());
}
average = average.div(estimatedPoses.size());
- for(int i = estimatedPoses.size()-1; i>=0; i--){
- if(estimatedPoses.get(i).estimatedPose.getTranslation().toTranslation2d().getDistance(average) > VisionConstants.MAX_POSE_DIFFERENCE/2){
+ for (int i = estimatedPoses.size() - 1; i >= 0; i--) {
+ if (estimatedPoses.get(i).estimatedPose.getTranslation().toTranslation2d()
+ .getDistance(average) > VisionConstants.MAX_POSE_DIFFERENCE / 2) {
estimatedPoses.remove(i);
}
}
}
- return estimatedPoses;
+ return estimatedPoses;
}
/**
* Updates the robot's odometry with vision
+ *
* @param poseEstimator The pose estimator to update
- * @param yawFunction A function that returns the yaw as a double given the timestamp
- * @param slipped True if the wheels have slipped, false otherwise
+ * @param yawFunction A function that returns the yaw as a double given the
+ * timestamp
+ * @param slipped True if the wheels have slipped, false otherwise
* @return The list of estimated robot poses from vision
*/
- public ArrayList<EstimatedRobotPose> updateOdometry(SwerveDrivePoseEstimator poseEstimator, DoubleUnaryOperator yawFunction, boolean slipped){
+ public ArrayList<EstimatedRobotPose> updateOdometry(SwerveDrivePoseEstimator poseEstimator,
+ DoubleUnaryOperator yawFunction, boolean slipped) {
// Simulate vision
// 2 ifs to avoid warning
- if(VisionConstants.ENABLED_SIM){
- if(RobotBase.isSimulation()){
+ if (VisionConstants.ENABLED_SIM) {
+ if (RobotBase.isSimulation()) {
visionSim.update(poseEstimator.getEstimatedPosition());
}
}
ArrayList<EstimatedRobotPose> estimatedPoses = getEstimatedPoses(poseEstimator.getEstimatedPosition(), yawFunction);
for (EstimatedRobotPose estimatedPose : estimatedPoses) {
// Continue if this pose doesn't exist
- if(estimatedPose.timestampSeconds < 0 || !onField(estimatedPose.estimatedPose.toPose2d()) || Timer.getFPGATimestamp() < estimatedPose.timestampSeconds || Timer.getFPGATimestamp() > estimatedPose.timestampSeconds + 1){
+ if (estimatedPose.timestampSeconds < 0 || !onField(estimatedPose.estimatedPose.toPose2d())
+ || Timer.getFPGATimestamp() < estimatedPose.timestampSeconds
+ || Timer.getFPGATimestamp() > estimatedPose.timestampSeconds + 1) {
continue;
}
poseEstimator.addVisionMeasurement(
- estimatedPose.estimatedPose.toPose2d(),
- estimatedPose.timestampSeconds,
- slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS
- );
+ estimatedPose.estimatedPose.toPose2d(),
+ estimatedPose.timestampSeconds,
+ slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS);
sawTag = true;
}
return estimatedPoses;
/**
* Updates each camera's inputs for logging
*/
- public void updateInputs(){
- for(VisionCamera c : cameras){
+ public void updateInputs() {
+ for (VisionCamera c : cameras) {
c.updateInputs();
}
}
/**
* If vision saw any April tags last frame
+ *
* @return If vision saw an April tag last frame
*/
- public boolean canSeeTag(){
+ public boolean canSeeTag() {
return sawTag;
}
/**
* Enable or disable a single camera
- * @param index The camera index
+ *
+ * @param index The camera index
* @param enabled If it should be enabled or disabled
*/
- public void enableCamera(int index, boolean enabled){
- try{
+ public void enableCamera(int index, boolean enabled) {
+ try {
cameras.get(index).enable(enabled);
- }catch(IndexOutOfBoundsException e){
- DriverStation.reportWarning("Camera index "+index+" is out of bounds", false);
+ } catch (IndexOutOfBoundsException e) {
+ DriverStation.reportWarning("Camera index " + index + " is out of bounds", false);
}
}
+
/**
* Sets the cameras to only use April tag in the specified array
+ *
* @param ids The ids of the tags to use, null or empty array to use all
*/
- public void onlyUse(int[] ids){
+ public void onlyUse(int[] ids) {
onlyUse = ids;
}
/**
* Checks if one or more cameras are disconnected
+ *
* @return true if at least one camera is disconnected, false otherwise
*/
- public boolean oneCameraDisconnected(){
- for(VisionCamera camera : cameras){
- if(!camera.inputs.connected){
+ public boolean oneCameraDisconnected() {
+ for (VisionCamera camera : cameras) {
+ if (!camera.inputs.connected) {
return true;
}
}
/**
* Checks if a pose is on the field
+ *
* @param pose The pose to check
* @return If the pose is on the field
*/
- public static boolean onField(Pose2d pose){
- return pose!=null && pose.getX()>0 && pose.getX()<FieldConstants.field.getFieldLength() && pose.getY()>0 && pose.getY()<FieldConstants.field.getFieldWidth();
+ public static boolean onField(Pose2d pose) {
+ return pose != null && pose.getX() > 0 && pose.getX() < FieldConstants.field.getFieldLength() && pose.getY() > 0
+ && pose.getY() < FieldConstants.field.getFieldWidth();
}
/**
* Checks if a pose is on or near the field
+ *
* @param pose The pose to check
- * @return If the pose is within an area with twice the length and width of the field
+ * @return If the pose is within an area with twice the length and width of the
+ * field
*/
- public static boolean nearField(Pose2d pose){
- return pose!=null && pose.getX()>-FieldConstants.field.getFieldLength()/2 && pose.getX()<FieldConstants.field.getFieldLength()*1.5 && pose.getY()>-FieldConstants.field.getFieldWidth()/2 && pose.getY()<FieldConstants.field.getFieldWidth()*1.5;
+ public static boolean nearField(Pose2d pose) {
+ return pose != null && pose.getX() > -FieldConstants.field.getFieldLength() / 2
+ && pose.getX() < FieldConstants.field.getFieldLength() * 1.5
+ && pose.getY() > -FieldConstants.field.getFieldWidth() / 2
+ && pose.getY() < FieldConstants.field.getFieldWidth() * 1.5;
}
-
+
private class VisionCamera implements VisionIO {
private PhotonCamera camera;
private PhotonPoseEstimator photonPoseEstimator;
private double lastTimestamp = 0;
private boolean enabled = true;
private final VisionIOInputs inputs = new VisionIOInputs();
-
+
/**
* Stores information about a camera
+ *
* @param cameraName The name of the camera on PhotonVision
* @param robotToCam The transformation from the robot to the camera
*/
public VisionCamera(String cameraName, Transform3d robotToCam) {
camera = new PhotonCamera(cameraName);
photonPoseEstimator = new PhotonPoseEstimator(
- FieldConstants.field,
- robotToCam
- );
+ FieldConstants.field,
+ robotToCam);
lastPose = null;
}
-
+
/**
* Gets the estimated poses from the camera
- * @param referencePose Pose to use for reference, usually the previous estimated robot pose
+ *
+ * @param referencePose Pose to use for reference, usually the previous
+ * estimated robot pose
* @return estimated robot poses
*/
public ArrayList<EstimatedRobotPose> getEstimatedPose(Pose2d referencePose) {
ArrayList<EstimatedRobotPose> list = new ArrayList<>();
- if(!enabled){
+ if (!enabled) {
return list;
}
- for(PhotonPipelineResult cameraResult : inputs.results){
- if(!cameraResult.hasTargets() || cameraResult.getTimestampSeconds()<0){
- continue;
+ for (PhotonPipelineResult cameraResult : inputs.results) {
+ if (!cameraResult.hasTargets() || cameraResult.getTimestampSeconds() < 0) {
+ continue;
}
-
- // if there is a target detected and the timestamp exists,
+
+ // if there is a target detected and the timestamp exists,
// check the ambiguity isn't too high
List<PhotonTrackedTarget> targetsUsed = cameraResult.targets;
- for (int i = targetsUsed.size()-1; i >= 0; i--) {
- // Remove it from the list if it should not be used or if it has too high of an ambiguity
- if(!useTag(targetsUsed.get(i).getFiducialId()) || targetsUsed.get(i).getPoseAmbiguity() > VisionConstants.HIGHEST_AMBIGUITY || targetsUsed.get(i).bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE){
+ for (int i = targetsUsed.size() - 1; i >= 0; i--) {
+ // Remove it from the list if it should not be used or if it has too high of an
+ // ambiguity
+ if (!useTag(targetsUsed.get(i).getFiducialId())
+ || targetsUsed.get(i).getPoseAmbiguity() > VisionConstants.HIGHEST_AMBIGUITY
+ || targetsUsed.get(i).bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE) {
targetsUsed.remove(i);
}
}
- // If there are no targets, the timestamp doesn't exist, or there there is only 1 tag and the constant is set to only use 2 tags, continue
- if(targetsUsed.size() == 0 || cameraResult.getTimestampSeconds()<0 || targetsUsed.size()==1 && VisionConstants.ONLY_USE_2_TAGS){
+ // If there are no targets, the timestamp doesn't exist, or there there is only
+ // 1 tag and the constant is set to only use 2 tags, continue
+ if (targetsUsed.size() == 0 || cameraResult.getTimestampSeconds() < 0
+ || targetsUsed.size() == 1 && VisionConstants.ONLY_USE_2_TAGS) {
continue;
}
// Set strategy to single tag if there is only 1 good tag and update
- PhotonPoseEstimator.PoseStrategy poseStrategy = targetsUsed.size() > 1 ? VisionConstants.POSE_STRATEGY : VisionConstants.MULTITAG_FALLBACK_STRATEGY;
- Optional<EstimatedRobotPose> pose;
- switch (poseStrategy) {
- case AVERAGE_BEST_TARGETS:
- pose = photonPoseEstimator.estimateAverageBestTargetsPose(cameraResult);
- break;
- case CLOSEST_TO_CAMERA_HEIGHT:
- pose = photonPoseEstimator.estimateClosestToCameraHeightPose(cameraResult);
- break;
- case CLOSEST_TO_REFERENCE_POSE:
- pose = photonPoseEstimator.estimateClosestToReferencePose(cameraResult, new Pose3d(referencePose));
- break;
- case LOWEST_AMBIGUITY:
- pose = photonPoseEstimator.estimateLowestAmbiguityPose(cameraResult);
- break;
- case MULTI_TAG_PNP_ON_COPROCESSOR:
- pose = photonPoseEstimator.estimateCoprocMultiTagPose(cameraResult);
- break;
- case PNP_DISTANCE_TRIG_SOLVE:
- pose = photonPoseEstimator.estimatePnpDistanceTrigSolvePose(cameraResult);
- break;
- case CLOSEST_TO_LAST_POSE:
- case CONSTRAINED_SOLVEPNP:
- case MULTI_TAG_PNP_ON_RIO:
- default:
- throw new RuntimeException("Pose estimation method " + poseStrategy.toString() + " is not supported.");
- }
-
- if(pose.isPresent() && pose.get()!=null && onField(pose.get().estimatedPose.toPose2d())){
+ PhotonPoseEstimator.PoseStrategy poseStrategy = targetsUsed.size() > 1 ? VisionConstants.POSE_STRATEGY
+ : VisionConstants.MULTITAG_FALLBACK_STRATEGY;
+ Optional<EstimatedRobotPose> pose;
+ switch (poseStrategy) {
+ case AVERAGE_BEST_TARGETS:
+ pose = photonPoseEstimator.estimateAverageBestTargetsPose(cameraResult);
+ break;
+ case CLOSEST_TO_CAMERA_HEIGHT:
+ pose = photonPoseEstimator.estimateClosestToCameraHeightPose(cameraResult);
+ break;
+ case CLOSEST_TO_REFERENCE_POSE:
+ pose = photonPoseEstimator.estimateClosestToReferencePose(cameraResult, new Pose3d(referencePose));
+ break;
+ case LOWEST_AMBIGUITY:
+ pose = photonPoseEstimator.estimateLowestAmbiguityPose(cameraResult);
+ break;
+ case MULTI_TAG_PNP_ON_COPROCESSOR:
+ pose = photonPoseEstimator.estimateCoprocMultiTagPose(cameraResult);
+ break;
+ case PNP_DISTANCE_TRIG_SOLVE:
+ pose = photonPoseEstimator.estimatePnpDistanceTrigSolvePose(cameraResult);
+ break;
+ case CLOSEST_TO_LAST_POSE:
+ case CONSTRAINED_SOLVEPNP:
+ case MULTI_TAG_PNP_ON_RIO:
+ default:
+ throw new RuntimeException("Pose estimation method " + poseStrategy.toString() + " is not supported.");
+ }
+
+ if (pose.isPresent() && pose.get() != null && onField(pose.get().estimatedPose.toPose2d())) {
double timestamp = cameraResult.getTimestampSeconds();
// If the pose moved too much, don't use it
- if(lastPose==null || lastPose.getTranslation().getDistance(pose.get().estimatedPose.toPose2d().getTranslation()) > DriveConstants.MAX_SPEED*1.25*(timestamp-lastTimestamp) || timestamp < lastTimestamp){
+ if (lastPose == null
+ || lastPose.getTranslation().getDistance(pose.get().estimatedPose.toPose2d()
+ .getTranslation()) > DriveConstants.MAX_SPEED * 1.25 * (timestamp - lastTimestamp)
+ || timestamp < lastTimestamp) {
lastPose = pose.get().estimatedPose.toPose2d();
lastTimestamp = timestamp;
continue;
return list;
}
- /**
- * Updates the VisionIOInputs object with the results from PhotonVision for logging
- */
- @Override
+ /**
+ * Updates the VisionIOInputs object with the results from PhotonVision for
+ * logging
+ */
+ @Override
public void updateInputs() {
inputs.connected = camera.isConnected();
inputs.results = camera.getAllUnreadResults();
- Logger.processInputs("Vision/"+camera.getName(), inputs);
+ Logger.processInputs("Vision/" + camera.getName(), inputs);
// Mechanical Advantage's vision logging
// // Read new camera observations
// Set<Short> tagIds = new HashSet<>();
// List<PoseObservation> poseObservations = new LinkedList<>();
// for (var result : camera.getAllUnreadResults()) {
- // // Update latest target observation
- // if (result.hasTargets()) {
- // inputs.latestTargetObservation =
- // new TargetObservation(
- // Rotation2d.fromDegrees(result.getBestTarget().getYaw()),
- // Rotation2d.fromDegrees(result.getBestTarget().getPitch()));
- // } else {
- // inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d());
- // }
+ // // Update latest target observation
+ // if (result.hasTargets()) {
+ // inputs.latestTargetObservation =
+ // new TargetObservation(
+ // Rotation2d.fromDegrees(result.getBestTarget().getYaw()),
+ // Rotation2d.fromDegrees(result.getBestTarget().getPitch()));
+ // } else {
+ // inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new
+ // Rotation2d());
+ // }
+ // }
+ // // Add pose observation
+ // if (result.multitagResult.isPresent()) { // Multitag result
+ // var multitagResult = result.multitagResult.get();
+
+ // // Calculate robot pose
+ // Transform3d fieldToCamera = multitagResult.estimatedPose.best;
+ // Transform3d fieldToRobot =
+ // fieldToCamera.plus(VisionConstants.APRIL_TAG_CAMERAS.get(0).getSecond().inverse());
+ // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(),
+ // fieldToRobot.getRotation());
+
+ // // Calculate average tag distance
+ // double totalTagDistance = 0.0;
+ // for (var target : result.targets) {
+ // totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm();
+ // }
+
+ // // Add tag IDs
+ // tagIds.addAll(multitagResult.fiducialIDsUsed);
+
+ // // Add observation
+ // poseObservations.add(
+ // new PoseObservation(
+ // result.getTimestampSeconds(), // Timestamp
+ // robotPose, // 3D pose estimate
+ // multitagResult.estimatedPose.ambiguity, // Ambiguity
+ // multitagResult.fiducialIDsUsed.size(), // Tag count
+ // totalTagDistance / result.targets.size(), // Average tag distance
+ // PoseObservationType.PHOTONVISION)); // Observation type
+
+ // } else if (!result.targets.isEmpty()) { // Single tag result
+ // var target = result.targets.get(0);
+
+ // // Calculate robot pose
+ // var tagPose = aprilTagLayout.getTagPose(target.fiducialId);
+ // if (tagPose.isPresent()) {
+ // Transform3d fieldToTarget =
+ // new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation());
+ // Transform3d cameraToTarget = target.bestCameraToTarget;
+ // Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse());
+ // Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse());
+ // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(),
+ // fieldToRobot.getRotation());
+
+ // // Add tag ID
+ // tagIds.add((short) target.fiducialId);
+
+ // // Add observation
+ // poseObservations.add(
+ // new PoseObservation(
+ // result.getTimestampSeconds(), // Timestamp
+ // robotPose, // 3D pose estimate
+ // target.poseAmbiguity, // Ambiguity
+ // 1, // Tag count
+ // cameraToTarget.getTranslation().getNorm(), // Average tag distance
+ // PoseObservationType.PHOTONVISION)); // Observation type
+ // }
// }
- // // Add pose observation
- // if (result.multitagResult.isPresent()) { // Multitag result
- // var multitagResult = result.multitagResult.get();
-
- // // Calculate robot pose
- // Transform3d fieldToCamera = multitagResult.estimatedPose.best;
- // Transform3d fieldToRobot = fieldToCamera.plus(VisionConstants.APRIL_TAG_CAMERAS.get(0).getSecond().inverse());
- // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation());
-
- // // Calculate average tag distance
- // double totalTagDistance = 0.0;
- // for (var target : result.targets) {
- // totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm();
- // }
-
- // // Add tag IDs
- // tagIds.addAll(multitagResult.fiducialIDsUsed);
-
- // // Add observation
- // poseObservations.add(
- // new PoseObservation(
- // result.getTimestampSeconds(), // Timestamp
- // robotPose, // 3D pose estimate
- // multitagResult.estimatedPose.ambiguity, // Ambiguity
- // multitagResult.fiducialIDsUsed.size(), // Tag count
- // totalTagDistance / result.targets.size(), // Average tag distance
- // PoseObservationType.PHOTONVISION)); // Observation type
-
- // } else if (!result.targets.isEmpty()) { // Single tag result
- // var target = result.targets.get(0);
-
- // // Calculate robot pose
- // var tagPose = aprilTagLayout.getTagPose(target.fiducialId);
- // if (tagPose.isPresent()) {
- // Transform3d fieldToTarget =
- // new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation());
- // Transform3d cameraToTarget = target.bestCameraToTarget;
- // Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse());
- // Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse());
- // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation());
-
- // // Add tag ID
- // tagIds.add((short) target.fiducialId);
-
- // // Add observation
- // poseObservations.add(
- // new PoseObservation(
- // result.getTimestampSeconds(), // Timestamp
- // robotPose, // 3D pose estimate
- // target.poseAmbiguity, // Ambiguity
- // 1, // Tag count
- // cameraToTarget.getTranslation().getNorm(), // Average tag distance
- // PoseObservationType.PHOTONVISION)); // Observation type
- // }
- // }
// }
}
-
+
/**
* Gets the pose using manual calculations
- * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
+ *
+ * @param yawFunction A unary operator that takes a timestamp and returns the
+ * yaw at that time
* @return A list of estimated poses as EstimatedRobotPoses
*/
- public ArrayList<EstimatedRobotPose> getEstimatedPose(DoubleUnaryOperator yawFunction){
+ public ArrayList<EstimatedRobotPose> getEstimatedPose(DoubleUnaryOperator yawFunction) {
ArrayList<EstimatedRobotPose> list = new ArrayList<>();
// Do nothing if this camera is disabled
- if(!enabled){
+ if (!enabled) {
return list;
}
// The latest camera results
- for(PhotonPipelineResult result : inputs.results){
+ for (PhotonPipelineResult result : inputs.results) {
// TODO: This could be improved by averaging all targets instead of only using 1
// Continue if the target doesn't exist or it should be ignored
- if (!result.hasTargets()) continue;
+ if (!result.hasTargets())
+ continue;
// Gets the best target to use for the calculations
PhotonTrackedTarget target = result.getBestTarget();
// I don't know why this would happen, but keep it in just in case
- if(target==null){
+ if (target == null) {
continue;
}
// Continue if the id is too high or too low
int id = target.getFiducialId();
- if(!useTag(id) || target.bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE || target.poseAmbiguity > VisionConstants.HIGHEST_AMBIGUITY){
+ if (!useTag(id) || target.bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE
+ || target.poseAmbiguity > VisionConstants.HIGHEST_AMBIGUITY) {
continue;
}
// Stores target pose and robot to camera transformation for easy access later
double timestamp = result.getTimestampSeconds();
double yaw = yawFunction.applyAsDouble(timestamp);
- // Get the tag position relative to the robot, assuming the robot is on the ground
+ // Get the tag position relative to the robot, assuming the robot is on the
+ // ground
Translation3d translation = target.getBestCameraToTarget().getTranslation()
- .rotateBy(robotToCamera.getRotation());
- translation = translation//.times((targetPose.getZ()-robotToCamera.getZ())/translation.getZ())
- .plus(robotToCamera.getTranslation())
- .rotateBy(new Rotation3d(0, 0, yaw))
-
- // Invert it to get the robot position relative to the April tag
- // Multiply by a constant. I don't know why this works, but it was consistently 10% off in 2023 Fall Semester
- .times(-VisionConstants.DISTANCE_SCALE)
- // Get the field relative robot pose
- .plus(targetPose.getTranslation());
- try{
+ .rotateBy(robotToCamera.getRotation());
+ translation = translation// .times((targetPose.getZ()-robotToCamera.getZ())/translation.getZ())
+ .plus(robotToCamera.getTranslation())
+ .rotateBy(new Rotation3d(0, 0, yaw))
+
+ // Invert it to get the robot position relative to the April tag
+ // Multiply by a constant. I don't know why this works, but it was consistently
+ // 10% off in 2023 Fall Semester
+ .times(-VisionConstants.DISTANCE_SCALE)
+ // Get the field relative robot pose
+ .plus(targetPose.getTranslation());
+ try {
// Adds an EstimatedRobotPose
list.add(new EstimatedRobotPose(
- new Pose3d(translation.getX(), translation.getY(), 0, new Rotation3d(0, 0, yaw)),
- timestamp,
- List.of(target),
- VisionConstants.POSE_STRATEGY
- ));
- }catch(Exception e){
+ new Pose3d(translation.getX(), translation.getY(), 0, new Rotation3d(0, 0, yaw)),
+ timestamp,
+ List.of(target),
+ VisionConstants.POSE_STRATEGY));
+ } catch (Exception e) {
DriverStation.reportError("Error creating EstimatedRobotPose", true);
}
}
return list;
}
- public boolean useTag(int id){
+ public boolean useTag(int id) {
// Never use tags that don't exist
- if(id <= 0 || id > FieldConstants.field.getTags().size()){
+ if (id <= 0 || id > FieldConstants.field.getTags().size()) {
return false;
}
// Return false if it is in the list of tags to ignore
- for(int id2 : VisionConstants.TAGS_TO_IGNORE){
- if(id == id2){
+ for (int id2 : VisionConstants.TAGS_TO_IGNORE) {
+ if (id == id2) {
return false;
}
}
// If it's in the array to only use and not in the array to ignore, return true
- for(int j = 0; onlyUse != null && j < onlyUse.length; j++){
- if(id == onlyUse[j]){
+ for (int j = 0; onlyUse != null && j < onlyUse.length; j++) {
+ if (id == onlyUse[j]) {
return true;
}
}
- // If it isn't in the array to only use, only reutrn true if the array is empty/null
+ // If it isn't in the array to only use, only reutrn true if the array is
+ // empty/null
return onlyUse == null || onlyUse.length == 0;
}
/**
* Enables or disables this camera
+ *
* @param enable If it should be enabled or disabled
*/
- public void enable(boolean enable){
+ public void enable(boolean enable) {
enabled = enable;
}
}