From e4d2a81736d8ba9cc632f75e8013029038a55a14 Mon Sep 17 00:00:00 2001 From: Justin E Date: Fri, 6 Mar 2026 16:36:45 -0500 Subject: [PATCH 01/29] Tune flywheel PID constants --- .../java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 6dbbd69..1e1fcc3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -50,11 +50,11 @@ public ShooterIOTalonFX() { this.flywheelMotor = new TalonFX(CanID.FLYWHEEL_MOTOR.getID()); var flywheelSlot0 = new Slot0Configs(); - flywheelSlot0.kP = 0.1; + flywheelSlot0.kP = 0.15; flywheelSlot0.kI = 0.0; flywheelSlot0.kD = 0.0; - flywheelSlot0.kS = 0.0; - flywheelSlot0.kV = 0.0; + flywheelSlot0.kS = 0.225; + flywheelSlot0.kV = 0.125; this.flywheelMotor.getConfigurator().apply(flywheelSlot0); From 9c552addf584ba9ec2689b9b7ecddae5ed16e58d Mon Sep 17 00:00:00 2001 From: Justin E Date: Sun, 8 Mar 2026 08:51:38 -0400 Subject: [PATCH 02/29] Add advantage kit annotation processor and fix vision IO serialization --- build.gradle | 11 +++++++++++ src/main/java/frc/robot/subsystems/vision/Vision.java | 2 +- .../java/frc/robot/subsystems/vision/VisionIO.java | 6 +++++- .../robot/subsystems/vision/VisionIOPhotonCamera.java | 11 ++++++++++- 4 files changed, 27 insertions(+), 3 deletions(-) diff --git a/build.gradle b/build.gradle index ca88947..ddf0ffe 100644 --- a/build.gradle +++ b/build.gradle @@ -1,3 +1,5 @@ +import groovy.json.JsonSlurper + plugins { id "java" id "edu.wpi.first.GradleRIO" version "2026.2.1" @@ -72,6 +74,10 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + def akitJson = new JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" + } test { @@ -102,3 +108,8 @@ wpi.java.configureTestTasks(test) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } + +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" + classpath = sourceSets.main.runtimeClasspath +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 5e068ec..63f406d 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -22,7 +22,7 @@ public Vision(CommandSwerveDrivetrain drive) { public void onPeriodic() { int index = 0; // update drive with vision measurements - for (EstimatedRobotPose pose : io.estimatedRobotPoses) { + for (EstimatedRobotPose pose : visionIO.getEstimatedRobotPoses()) { Logger.recordOutput("EstimatedPose/" + index, pose.estimatedPose.toPose2d()); double avgDist = 0; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 245ca9f..2d6f9f2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import frc.lib.subsystem.BaseIO; import frc.lib.subsystem.BaseInputClass; import frc.lib.subsystem.IORefresher; @@ -12,6 +14,8 @@ public interface VisionIO extends BaseIO, IORefresher { @AutoLog public static class VisionIOInputs extends BaseInputClass { - public List estimatedRobotPoses = List.of(); + public Pose3d[] estimatedRobotPoses = new Pose3d[0]; } + + List getEstimatedRobotPoses(); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java index e1b047e..8e05df6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.math.geometry.Pose3d; import frc.lib.subsystem.IORefresher; import frc.robot.subsystems.vision.photon.Camera; import frc.robot.subsystems.vision.photon.CameraManager; @@ -29,6 +30,14 @@ public void refreshData() { @Override public void updateInputs(VisionIOInputs inputs) { - inputs.estimatedRobotPoses = new ArrayList<>(estimatedRobotPoses); + inputs.estimatedRobotPoses = estimatedRobotPoses.stream() + .map(pose -> pose.estimatedPose) + .toArray(Pose3d[]::new); + } + + + @Override + public List getEstimatedRobotPoses() { + return estimatedRobotPoses; } } From b4b5bfabef173f9ac1a1a0851774bc6d783a2ef3 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Sun, 8 Mar 2026 13:07:13 -0400 Subject: [PATCH 03/29] Add other subsystem code and tune PIDs --- src/main/java/frc/robot/CanID.java | 16 ++--- src/main/java/frc/robot/RobotContainer.java | 26 ++++--- .../frc/robot/generated/TunerConstants.java | 68 ++++++++---------- .../drive/CommandSwerveDrivetrain.java | 3 + .../findexer/FindexerIOTalonFX.java | 6 ++ .../frc/robot/subsystems/intake/Intake.java | 43 ++++++------ .../subsystems/shooter/ShooterIOTalonFX.java | 34 +++++++-- .../robot/subsystems/targeting/Targeting.java | 4 +- .../frc/robot/subsystems/trigger/Trigger.java | 4 +- .../frc/robot/subsystems/turret/Turret.java | 5 ++ .../subsystems/turret/TurretIOTalonFX.java | 70 +++++++++++++------ .../subsystems/turret/calc/ShotData.java | 2 + .../subsystems/turret/calc/TurretMath.java | 11 +-- .../frc/robot/subsystems/vision/Vision.java | 16 +++++ .../vision/VisionIOPhotonCamera.java | 2 +- .../vision/photon/CameraManager.java | 12 ++-- 16 files changed, 206 insertions(+), 116 deletions(-) diff --git a/src/main/java/frc/robot/CanID.java b/src/main/java/frc/robot/CanID.java index a9d8f33..57b570c 100644 --- a/src/main/java/frc/robot/CanID.java +++ b/src/main/java/frc/robot/CanID.java @@ -4,21 +4,21 @@ * Holder for all CAN device IDs besides drivetrain devices */ public enum CanID { - TRIGGER_MOTOR(10), + TRIGGER_MOTOR(55), FLYWHEEL_MOTOR(20), - HOOD_MOTOR(12), - HOOD_ENCODER(14), + HOOD_MOTOR(50), + HOOD_ENCODER(31), - TURRET_MOTOR(13), + TURRET_MOTOR(0), - INTAKE_MOTOR(9), + INTAKE_MOTOR(6), INTAKE_DEPLOY_MOTOR(11), - TURRET_PINION_CANCODER(12), - TURRET_FOLLOWER_CANCODER(14), + TURRET_PINION_CANCODER(19), + TURRET_FOLLOWER_CANCODER(20), - FINDEXER_MOTOR(15); + FINDEXER_MOTOR(18); private final int deviceID; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a9af9be..2231cbf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,8 @@ import static edu.wpi.first.units.Units.*; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -80,7 +82,7 @@ private void configureBindings() { setupSwerveBindings(); setupIntakeBindings(); setupTargetingBindings(); - setupShooterBindings(); + // setupShooterBindings(); } private void setupSwerveBindings() { @@ -116,11 +118,19 @@ private void setupSwerveBindings() { // reset the field-centric heading on left bumper press driverController.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); + + operatorController.y().onTrue(vision.runOnce(() -> { + var estimatedPose = vision.getEstimatedPositionFromCameras(); + if (estimatedPose != null) { + Logger.recordOutput("Vision/SnapshotEstimate", estimatedPose); + drive.resetPose(estimatedPose); + } + })); } private void setupIntakeBindings() { // toggle intake on B press - operatorController.b().onTrue(intake.run(intake::toggleIntake)); + // operatorController.b().onTrue(intake.run(intake::toggleIntake)); } private void setupTargetingBindings() { @@ -128,12 +138,12 @@ private void setupTargetingBindings() { operatorController.leftBumper().onTrue(targeting.run(targeting::setTargetingShuttle)); } - private void setupShooterBindings() { - // toggle shooter on right trigger hold - driverController.rightTrigger() - .whileTrue(shooter.run(() -> shooter.setDriverRequestingShooting(true))) - .whileFalse(shooter.run(() -> shooter.setDriverRequestingShooting(false))); - } + // private void setupShooterBindings() { + // // toggle shooter on right trigger hold + // driverController.rightTrigger() + // .whileTrue(shooter.run(() -> shooter.setDriverRequestingShooting(true))) + // .whileFalse(shooter.run(() -> shooter.setDriverRequestingShooting(false))); + // } public Command getAutonomousCommand() { return autoChooser.getSelected(); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 02b995e..949bab5 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -13,10 +13,9 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; - import frc.robot.subsystems.drive.CommandSwerveDrivetrain; -// Generated by the Tuner X Swerve Project Generator +// Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { // Both sets of gains need to be tuned to your individual robot. @@ -24,7 +23,7 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(5).withKI(0).withKD(0) + .withKP(100).withKI(0).withKD(0.5) .withKS(0.1).withKV(1.59).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control @@ -51,7 +50,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private static final Current kSlipCurrent = Amps.of(120); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -70,11 +69,11 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); + public static final CANBus kCANBus = new CANBus("Canivore_Drivetrain", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(3.92); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(3.79); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot @@ -87,7 +86,7 @@ public class TunerConstants { private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 0; + private static final int kPigeonId = 62; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -126,48 +125,48 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 0; - private static final int kFrontLeftSteerMotorId = 50; - private static final int kFrontLeftEncoderId = 31; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.4404296875); + private static final int kFrontLeftDriveMotorId = 41; + private static final int kFrontLeftSteerMotorId = 27; + private static final int kFrontLeftEncoderId = 25; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.32666015625); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(11.5); - private static final Distance kFrontLeftYPos = Inches.of(11.5); + private static final Distance kFrontLeftXPos = Inches.of(10.25); + private static final Distance kFrontLeftYPos = Inches.of(10.25); // Front Right - private static final int kFrontRightDriveMotorId = 2; - private static final int kFrontRightSteerMotorId = 27; - private static final int kFrontRightEncoderId = 25; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.20263671875); + private static final int kFrontRightDriveMotorId = 42; + private static final int kFrontRightSteerMotorId = 7; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.48828125); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(11.5); - private static final Distance kFrontRightYPos = Inches.of(-11.5); + private static final Distance kFrontRightXPos = Inches.of(10.25); + private static final Distance kFrontRightYPos = Inches.of(-10.25); // Back Left - private static final int kBackLeftDriveMotorId = 1; + private static final int kBackLeftDriveMotorId = 43; private static final int kBackLeftSteerMotorId = 30; private static final int kBackLeftEncoderId = 22; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.088623046875); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.04833984375); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-11.5); - private static final Distance kBackLeftYPos = Inches.of(11.5); + private static final Distance kBackLeftXPos = Inches.of(-10.25); + private static final Distance kBackLeftYPos = Inches.of(10.25); // Back Right - private static final int kBackRightDriveMotorId = 3; - private static final int kBackRightSteerMotorId = 24; - private static final int kBackRightEncoderId = 0; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.466552734375); + private static final int kBackRightDriveMotorId = 46; + private static final int kBackRightSteerMotorId = 50; + private static final int kBackRightEncoderId = 31; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.49365234375); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-11.5); - private static final Distance kBackRightYPos = Inches.of(-11.5); + private static final Distance kBackRightXPos = Inches.of(-10.25); + private static final Distance kBackRightYPos = Inches.of(-10.25); public static final SwerveModuleConstants FrontLeft = @@ -197,12 +196,7 @@ public class TunerConstants { */ public static CommandSwerveDrivetrain createDrivetrain() { return new CommandSwerveDrivetrain( - DrivetrainConstants, - 250.0, // Run at 250 Hz for Canivore - FrontLeft, - FrontRight, - BackLeft, - BackRight + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight ); } @@ -267,10 +261,10 @@ public TunerSwerveDrivetrain( * unspecified or set to 0 Hz, this is 250 Hz on * CAN FD, and 100 Hz on CAN 2.0. * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta], with units in meters + * in the form [x, y, theta]áµ€, with units in meters * and radians * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta], with units in meters + * in the form [x, y, theta]áµ€, with units in meters * and radians * @param modules Constants for each specific module */ diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java index ddd6b29..b2185a4 100644 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java @@ -33,6 +33,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; + +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; /** @@ -353,6 +355,7 @@ private void configureAutoBuilder() { } } + @AutoLogOutput(key="Robot/Pose") public Pose2d getPose() { return getState().Pose; } diff --git a/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java index f2713cf..dd843cc 100644 --- a/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.CanID; @@ -14,6 +15,11 @@ public class FindexerIOTalonFX implements FindexerIO { public FindexerIOTalonFX() { this.motor = new TalonFX(CanID.FINDEXER_MOTOR.getID()); this.motorRps = motor.getVelocity(); + Slot0Configs config = new Slot0Configs(); + config.kP = 0.1; + config.kI = 0.0; + config.kD = 0.0; + this.motor.getConfigurator().apply(config); this.motor.optimizeBusUtilization(); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index a3fe496..77b800a 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -35,27 +35,28 @@ public Intake(CommandSwerveDrivetrain drivetrain) { public void onPeriodic() { // Only act on the intake motor if the intake is running if (running) { - switch (intakeIO.getIntakeState()) { - case DEPLOYING: // Intake is deploying - doDeployingState(); - break; - case RETRACTING: // Intake is retracting - doRetractingState(); - break; - case DEPLOYED: // Intake is fully deployed - doDeployedState(); - break; - case RETRACTED: // Intake is fully retracted - doRetractedState(); - break; - default: - // Error, unknown state! - // Turn off the deploy and intake motors! - intakeIO.setDeploySpeed(0.0); - intakeIO.setIntakeSpeed(0.0); - // Log unknown state - break; - } + // switch (intakeIO.getIntakeState()) { + // case DEPLOYING: // Intake is deploying + // doDeployingState(); + // break; + // case RETRACTING: // Intake is retracting + // doRetractingState(); + // break; + // case DEPLOYED: // Intake is fully deployed + // doDeployedState(); + // break; + // case RETRACTED: // Intake is fully retracted + // doRetractedState(); + // break; + // default: + // // Error, unknown state! + // // Turn off the deploy and intake motors! + // intakeIO.setDeploySpeed(0.0); + // intakeIO.setIntakeSpeed(0.0); + // // Log unknown state + // break; + // } + doDeployedState(); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 1e1fcc3..afd9de6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; @@ -24,8 +25,7 @@ public class ShooterIOTalonFX implements IORefresher, ShooterIO { private final double hoodMotorPositionOffset; // offset in motor rotations, calculated at startup, units in motor rotations private final VelocityVoltage flywheelVelocityControl = new VelocityVoltage(0); - private final PositionVoltage hoodPositionControl = new PositionVoltage(0); - + private final MotionMagicVelocityVoltage hoodVelocityControl = new MotionMagicVelocityVoltage(0); private final StatusSignal motorVelocity; // rps private final StatusSignal hoodMotorPosition; // motor rotations private final StatusSignal hoodAngle; // degrees @@ -33,6 +33,8 @@ public class ShooterIOTalonFX implements IORefresher, ShooterIO { private double hoodAngleSetPoint = 0.0; private double flywheelRPSSetPoint = 0.0; + private double hoodTargetEncoder = 0.0; + public ShooterIOTalonFX() { // hood configs this.hoodMotor = new TalonFXS(CanID.HOOD_MOTOR.getID()); @@ -87,6 +89,12 @@ public ShooterIOTalonFX() { @Override public void refreshData() { BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition); + + double current = hoodAngle.getValueAsDouble(); + + if (Math.abs(current - hoodTargetEncoder) < 0.005) { + hoodMotor.stopMotor(); + } } /** @@ -119,13 +127,23 @@ public void setFlywheelVelocity(double rps) { * Set the target hood angle * @param angle target angle TODO: relative to what */ - @Override + @Override public void setHoodAngle(double angle) { + angle = Math.max(15, Math.min(45, angle)); this.hoodAngleSetPoint = angle; - double motorRotations = angleToMotorRotations(angle); - this.hoodPositionControl.withPosition(hoodMotorPositionOffset + motorRotations); - this.hoodMotor.setControl(this.hoodPositionControl); + // convert target angle -> encoder rotations + this.hoodTargetEncoder = angleToEncoder(angle); + + double current = hoodAngle.getValueAsDouble(); + + double velocity = 0.5; // rps, tune later + + if (current > hoodTargetEncoder) { + velocity = -velocity; + } + + hoodMotor.setControl(hoodVelocityControl.withVelocity(velocity)); } /** @@ -137,4 +155,8 @@ private static double angleToMotorRotations(double angle) { // rotations = (angle_deg * gear_ratio) / 360 return angle * HOOD_GEAR_RATIO / 360.0; } + + private static double angleToEncoder(double angle) { + return (45.0 - angle) / 30.0; + } } diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index 86596dd..e30efdb 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -17,13 +17,13 @@ public class Targeting extends SubsystemBase { private static final double NOMINAL_SHOT_TIME_S = 0.3; // see github issue #23 (https://github.com/Team293/Rebuilt/issues/23) private static ShotCompensation.AdjustedShot shotData = new ShotCompensation.AdjustedShot(0.0, 0.0, 0.0, 0.0, 0.0); - private Translation2d targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + private Translation2d targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); private final CommandSwerveDrivetrain drive; public Targeting(CommandSwerveDrivetrain drive) { this.drive = drive; setTargetingHub(); - Logger.recordOutput("HubTarget", FieldConstants.Hub.innerCenterPoint); + Logger.recordOutput("HubTarget", FieldConstants.Hub.oppTopCenterPoint); Logger.recordOutput("ShuttleTarget", new Pose2d(0, 0, new Rotation2d())); } diff --git a/src/main/java/frc/robot/subsystems/trigger/Trigger.java b/src/main/java/frc/robot/subsystems/trigger/Trigger.java index 655a62e..37bf23b 100644 --- a/src/main/java/frc/robot/subsystems/trigger/Trigger.java +++ b/src/main/java/frc/robot/subsystems/trigger/Trigger.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.trigger; import frc.lib.subsystem.SpikeSystem; -import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.turret.Turret; @@ -43,7 +42,8 @@ public void onPeriodic() { * @return true if there is a ball in the indexer, false otherwise */ private boolean hasBallQueued() { - return super.io.proximitySensor; + // return super.io.proximitySensor; + return false; } /** diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 3e79448..de99f8a 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.turret; +import org.littletonrobotics.junction.Logger; + import frc.lib.subsystem.SpikeSystem; import frc.robot.RobotContainer; @@ -14,6 +16,7 @@ public class Turret extends SpikeSystem { public Turret() { super("Turret", new TurretIO.TurretIOInputs()); + System.out.println("Turret subsystem initialized"); } /** @@ -21,6 +24,8 @@ public Turret() { */ @Override public void onPeriodic() { + Logger.recordOutput("Turret/Degrees", io.turretAngleDegrees); + // compensate for robot movement ShotCompensation.AdjustedShot shotData = Targeting.getShotData(); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 905777b..5d4b245 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.turret; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.MotionMagicConfigs; @@ -19,15 +21,16 @@ public class TurretIOTalonFX implements TurretIO { private final TalonFX turretMotor; private final CommandSwerveDrivetrain drive; + private static final double TURRET_LIMIT_DEG = 160.0; + private final MotionMagicVoltage mmVoltage = new MotionMagicVoltage(0); private final StatusSignal encoderSignal; private final BaseStatusSignal pinionEncoderSignal; private final BaseStatusSignal followerEncoderSignal; - private static final double kTurretGearRatio = 140/10; // turret ring: 140 teeth, motor pinion: 10 teeth + private static final double kTurretGearRatio = 84.0/10.0; // turret ring: 84 teeth, motor pinion: 10 teeth - private final double turretRotationOffset; // offset in motor rotations, calculated at startup, units in motor rotations - private final double turretDegreesOffset = 246.7; // offset in degrees to align turret with front of robot + private final double turretDegreesOffset = 180.0; // offset in degrees to align turret with front of robot private double targetAngleDeg = 0.0; // target angle of the turret in degrees @@ -36,16 +39,11 @@ public TurretIOTalonFX(CommandSwerveDrivetrain drive) { this.turretMotor = new TalonFX(CanID.TURRET_MOTOR.getID()); MotionMagicConfigs mm = new MotionMagicConfigs(); - mm.MotionMagicAcceleration = 60; // rot/sec^2 - mm.MotionMagicCruiseVelocity = 30; // rot/sec + mm.MotionMagicAcceleration = 10; // rot/sec^2 + mm.MotionMagicCruiseVelocity = 10; // rot/sec // Motor configuration - Slot0Configs config = new Slot0Configs(); - config.kP = 1; - config.kI = 0.01; - config.kD = 0.3; - config.kS = 0.194; - config.kV = 0.1167; + Slot0Configs config = getTurretMotorConfig(); this.turretMotor.getConfigurator().apply(config); this.turretMotor.getConfigurator().apply(mm); @@ -72,7 +70,7 @@ public TurretIOTalonFX(CommandSwerveDrivetrain drive) { double currentMotorRevs = this.turretMotor.getPosition().getValueAsDouble(); double toZeroRevs = TurretMath.degreesToMotorPosition(turretDegrees); - this.turretRotationOffset = currentMotorRevs + toZeroRevs; + turretMotor.setPosition(-toZeroRevs); } /** @@ -81,6 +79,7 @@ public TurretIOTalonFX(CommandSwerveDrivetrain drive) { */ @Override public void setTurretAngle(double fieldTargetHeadingDeg) { + Logger.recordOutput("Turret/TargetAngle", fieldTargetHeadingDeg); this.targetAngleDeg = fieldTargetHeadingDeg; fieldTargetHeadingDeg = MathUtil.inputModulus(fieldTargetHeadingDeg, 0.0, 360.0); @@ -103,20 +102,27 @@ public void setTurretAngle(double fieldTargetHeadingDeg) { -180.0, 180.0 ); - // convert turret angle to motor rotations, accounting for gear ratio and offset - double rotations = (turretAngleDeg / 360.0) * kTurretGearRatio; - rotations = -rotations + this.turretRotationOffset; - - // calculate feedforward to counteract robot rotation, using a simple linear model with gain determined empirically - double motorRps = - -(this.drive.getRobotOmegaDegPerSec() / 360.0) * kTurretGearRatio; + if (turretAngleDeg > TURRET_LIMIT_DEG) { + turretAngleDeg -= 360.0; + } else if (turretAngleDeg < -TURRET_LIMIT_DEG) { + turretAngleDeg += 360.0; + } + // convert turret angle to motor rotations, accounting for gear ratio and offset + double turretAngleForMotor = turretAngleDeg; + if (turretAngleForMotor > 180.0) { + turretAngleForMotor -= 360.0; + } + Logger.recordOutput("Turret/TargetAngleForMotor", turretAngleForMotor); + double rotations = -(turretAngleForMotor / 360.0) * kTurretGearRatio; + + Logger.recordOutput("Turret/TargetRotations", rotations); // set the motor to the desired position with feedforward to counteract robot rotation this.turretMotor.setControl( this.mmVoltage .withPosition(rotations) // 0.1167 is an empirically determined gain to convert from motor RPS to voltage needed to hold position against rotation - .withFeedForward(motorRps * 0.1167) + // .withFeedForward(motorRps * 0.1167) ); } @@ -136,11 +142,20 @@ public void updateInputs(TurretIOInputs inputs) { inputs.robotOmegaDegPerSec = this.drive.getRobotOmegaDegPerSec(); // convert raw encoder readings to turret angle in degrees, accounting for gear ratio and offset - inputs.turretAngleDegrees = TurretMath.normalizeTurretHeading( + double turretAngleDegreesNonNormalized = TurretMath.normalizeTurretHeading( TurretMath.toDegreesWrapped(turretRotations), turretDegreesOffset ); + // normalize to -180 to 180 range + if (turretAngleDegreesNonNormalized > 180.0) { + inputs.turretAngleDegrees = turretAngleDegreesNonNormalized - 360.0; + } else if (turretAngleDegreesNonNormalized < -180.0) { + inputs.turretAngleDegrees = turretAngleDegreesNonNormalized + 360.0; + } else { + inputs.turretAngleDegrees = turretAngleDegreesNonNormalized; + } + // creates a position for the turret based on robot position, rotated by turret angle for logging inputs.turretPosition = new Pose2d(drive.getPose().getTranslation(), new Rotation2d(TurretMath.toRad(inputs.turretAngleDegrees))); } @@ -153,4 +168,17 @@ public void updateInputs(TurretIOInputs inputs) { public void refreshData() { BaseStatusSignal.refreshAll(encoderSignal, pinionEncoderSignal, followerEncoderSignal); } + + public static Slot0Configs getTurretMotorConfig() { + Slot0Configs config = new Slot0Configs(); + + config.kP = 1; + config.kI = 0.0; + config.kD = 0.0; + + config.kS = 0.25; + config.kV = 0.20; + + return config; + } } diff --git a/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java b/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java index 7b71344..5d6ac99 100644 --- a/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java +++ b/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java @@ -8,5 +8,7 @@ public class ShotData { static { // TODO: get data and fill these in + distanceToRPM.put(20.0, 3000.0); + distanceToHoodAngle.put(20.0, 15.0); } } diff --git a/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java b/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java index ab3d431..7887d70 100644 --- a/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java +++ b/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java @@ -15,9 +15,9 @@ public class TurretMath { // Hardware tooth counts - private static final double TURRET_GEAR_TEETH = 140.0; - private static final double ENCODER_A_TEETH = 13.0; - private static final double ENCODER_B_TEETH = 11.0; + private static final double TURRET_GEAR_TEETH = 84.0; + private static final double ENCODER_A_TEETH = 10.0; + private static final double ENCODER_B_TEETH = 13.0; // Derived encoder combination values private static final double ENCODER_COMBINED_TEETH = ENCODER_A_TEETH * ENCODER_B_TEETH; @@ -28,7 +28,7 @@ public class TurretMath { private static final double HALF_NORMALIZED_REV = NORMALIZED_REV / 2.0; private static final double DEGREES_PER_REV = 360.0; private static final double RAD_PER_REV = 2.0 * Math.PI; - private static final double MOTOR_UNITS_PER_REV = 14.0; // scale used in degreesToMotorPosition + private static final double MOTOR_UNITS_PER_REV = TURRET_GEAR_TEETH/ENCODER_A_TEETH; // scale used in degreesToMotorPosition // Defaults / initial values private static final double DEFAULT_OFFSET_DEGREES = 0.0; @@ -147,6 +147,9 @@ public static double toRadiansWrapped(double turretRevs) { } public static double degreesToMotorPosition(double turretDegrees) { + if (turretDegrees > 180.0) { + turretDegrees -= 360.0; + } return (turretDegrees * MOTOR_UNITS_PER_REV) / DEGREES_PER_REV; } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 63f406d..fc20ea9 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -7,6 +7,8 @@ import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; +import edu.wpi.first.math.geometry.Pose2d; + public class Vision extends SpikeSystem { private VisionIO visionIO; @@ -41,6 +43,7 @@ public void onPeriodic() { drive.addVisionMeasurement( pose.estimatedPose.toPose2d(), pose.timestampSeconds, + // CommandSwerveDrivetrain.kDefaultVisionStdDevs CommandSwerveDrivetrain.kDefaultVisionStdDevs.times(1 + ((avgDist * avgDist) / 30)) // scale the std devs based on the average distance to the targets (farther targets are less accurate) ); index++; @@ -52,4 +55,17 @@ protected Runnable setupDataRefresher() { this.visionIO = new VisionIOPhotonCamera(); return useAsyncDataRefresher(visionIO); } + + public Pose2d getEstimatedPositionFromCameras() { + if (visionIO.getEstimatedRobotPoses().isEmpty()) { + return null; + } + + for (EstimatedRobotPose pose : visionIO.getEstimatedRobotPoses()) { + if (pose != null) { + return pose.estimatedPose.toPose2d(); + } + } + return null; + } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java index 8e05df6..6d1fc1b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java @@ -38,6 +38,6 @@ public void updateInputs(VisionIOInputs inputs) { @Override public List getEstimatedRobotPoses() { - return estimatedRobotPoses; + return new ArrayList<>(estimatedRobotPoses); } } diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java index fc72fd5..9dc5534 100644 --- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java +++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java @@ -26,9 +26,9 @@ public static List getCameras() { new Camera( "left", new Transform3d( - Inches.of(29.5/2), - Inches.of(0), - Inches.of(6.875), + Inches.of(-13.8), + Inches.of(7.5), + Inches.of(22.3/4), new Rotation3d( Degrees.of(0), Degrees.of(60), @@ -42,9 +42,9 @@ public static List getCameras() { new Camera( "right", new Transform3d( - Inches.of(-29.5/2), - Inches.of(0), - Inches.of(6.875), + Inches.of(13.8), + Inches.of(5), + Inches.of(6.3/4), new Rotation3d( Degrees.of(0), Degrees.of(60), From a2dcb5cbd0ce51fc92253f2a177dfbf1d5a078f5 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Sun, 8 Mar 2026 13:30:56 -0400 Subject: [PATCH 04/29] Tune PIDs --- .../deploy/pathplanner/paths/New Path.path | 54 +++++++++++++++++++ .../robot/subsystems/findexer/Findexer.java | 2 +- .../findexer/FindexerIOTalonFX.java | 4 +- .../subsystems/turret/TurretIOTalonFX.java | 13 +++-- .../subsystems/turret/calc/ShotData.java | 3 +- .../frc/robot/subsystems/vision/Vision.java | 3 ++ 6 files changed, 73 insertions(+), 6 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..473a347 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5437831125827817, + "y": 7.367309602649005 + }, + "prevControl": null, + "nextControl": { + "x": 4.110057947019868, + "y": 6.641316225165561 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7850082781456953, + "y": 5.937102649006622 + }, + "prevControl": { + "x": -0.2149917218543047, + "y": 5.937102649006622 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -61.76255446183274 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/findexer/Findexer.java b/src/main/java/frc/robot/subsystems/findexer/Findexer.java index 24ec9c4..87404eb 100644 --- a/src/main/java/frc/robot/subsystems/findexer/Findexer.java +++ b/src/main/java/frc/robot/subsystems/findexer/Findexer.java @@ -4,7 +4,7 @@ import frc.robot.subsystems.trigger.Trigger; public class Findexer extends SpikeSystem { - private static final double FEEDING_RPS = 20.0; // feeding velocity in rotations per second + private static final double FEEDING_RPS = -20.0; // feeding velocity in rotations per second private final Trigger trigger; diff --git a/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java index dd843cc..495c9cd 100644 --- a/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java @@ -16,9 +16,11 @@ public FindexerIOTalonFX() { this.motor = new TalonFX(CanID.FINDEXER_MOTOR.getID()); this.motorRps = motor.getVelocity(); Slot0Configs config = new Slot0Configs(); - config.kP = 0.1; + config.kP = 0.5; config.kI = 0.0; config.kD = 0.0; + + config.kS = 0.0; this.motor.getConfigurator().apply(config); this.motor.optimizeBusUtilization(); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 5d4b245..8a636a0 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -172,12 +172,19 @@ public void refreshData() { public static Slot0Configs getTurretMotorConfig() { Slot0Configs config = new Slot0Configs(); - config.kP = 1; + // config.kP = 1; + // config.kI = 0.0; + // config.kD = 0.0; + + // config.kS = 0.25; + // config.kV = 0.20; + + config.kP = 0.0; config.kI = 0.0; config.kD = 0.0; - config.kS = 0.25; - config.kV = 0.20; + config.kS = 0.0; + config.kV = 0.0; return config; } diff --git a/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java b/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java index 5d6ac99..b5fb97a 100644 --- a/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java +++ b/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java @@ -8,7 +8,8 @@ public class ShotData { static { // TODO: get data and fill these in - distanceToRPM.put(20.0, 3000.0); + // distanceToRPM.put(20.0, 3000.0); + distanceToRPM.put(10.0, 2500.0); distanceToHoodAngle.put(20.0, 15.0); } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index fc20ea9..d5414b5 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -25,6 +25,9 @@ public void onPeriodic() { int index = 0; // update drive with vision measurements for (EstimatedRobotPose pose : visionIO.getEstimatedRobotPoses()) { + if (pose == null) { + continue; + } Logger.recordOutput("EstimatedPose/" + index, pose.estimatedPose.toPose2d()); double avgDist = 0; From 38df8cc205016a1eb0f50c27d319fa4b8867c199 Mon Sep 17 00:00:00 2001 From: nathan Date: Sun, 8 Mar 2026 20:09:30 -0400 Subject: [PATCH 05/29] Add region utilities and set hood angle when within trench bounds. Co-authored-by: PillageDev --- src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../calc => targeting}/ShotCompensation.java | 27 +- .../{turret/calc => targeting}/ShotData.java | 2 +- .../robot/subsystems/targeting/Targeting.java | 1 - .../frc/robot/subsystems/turret/Turret.java | 59 +-- .../frc/robot/subsystems/turret/TurretIO.java | 23 +- .../subsystems/turret/TurretIOTalonFX.java | 419 ++++++++++++------ .../subsystems/turret/calc/TurretMath.java | 192 -------- 9 files changed, 336 insertions(+), 390 deletions(-) rename src/main/java/frc/robot/subsystems/{turret/calc => targeting}/ShotCompensation.java (79%) rename src/main/java/frc/robot/subsystems/{turret/calc => targeting}/ShotData.java (92%) delete mode 100644 src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2231cbf..1e3e39b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d0aa767..ddf79eb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,7 +2,7 @@ import frc.lib.subsystem.SpikeSystem; import frc.robot.subsystems.targeting.Targeting; -import frc.robot.subsystems.turret.calc.ShotCompensation; +import frc.robot.subsystems.targeting.ShotCompensation; public class Shooter extends SpikeSystem { private static final double SHOOTER_READY_THRESHOLD_RPS = 0.5; // RPS threshold to consider the shooter ready diff --git a/src/main/java/frc/robot/subsystems/turret/calc/ShotCompensation.java b/src/main/java/frc/robot/subsystems/targeting/ShotCompensation.java similarity index 79% rename from src/main/java/frc/robot/subsystems/turret/calc/ShotCompensation.java rename to src/main/java/frc/robot/subsystems/targeting/ShotCompensation.java index f19f80e..140d63b 100644 --- a/src/main/java/frc/robot/subsystems/turret/calc/ShotCompensation.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotCompensation.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.turret.calc; +package frc.robot.subsystems.targeting; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -87,29 +87,4 @@ public static AdjustedShot compensateForMovement( return new AdjustedShot(baseRPM, hoodDeg, effectiveTurretDeg, turretFF_deg_s, rangeFF_m_s); } - - /** - * Calculate a feedforward term to compensate for the robot's velocity toward the target, which reduces effective range and thus shot time. - * @param velocity current robot velocity in field-relative coordinates - * @param directionToTarget unit vector pointing from robot to target in field-relative coordinates - * @return feedforward term to add to shot speed (in whatever units the caller expects, legacy scaling applies) - */ - private static double calculateVelocityCompensation( - ChassisSpeeds velocity, - Translation2d directionToTarget) { - - // project robot velocity onto the direction to target to get speed toward target - double robotSpeedTowardTarget = - velocity.vxMetersPerSecond * Math.cos(directionToTarget.getAngle().getRadians()) + - velocity.vyMetersPerSecond * Math.sin(directionToTarget.getAngle().getRadians()); - - // multiply by 100 to convert to whatever unit the caller expects (legacy scaling) - return robotSpeedTowardTarget * 100.0; - } - - private static double rpmToVelocity(double rpm) { - // convert wheel rpm to linear m/s - // wheel diameter 4 inches => 0.1016 m - return rpm * (Math.PI * 0.1016) / 60.0; - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java similarity index 92% rename from src/main/java/frc/robot/subsystems/turret/calc/ShotData.java rename to src/main/java/frc/robot/subsystems/targeting/ShotData.java index b5fb97a..4730a7c 100644 --- a/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.turret.calc; +package frc.robot.subsystems.targeting; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index e30efdb..c791192 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -11,7 +11,6 @@ import frc.lib.Elastic.NotificationLevel; import frc.lib.FieldConstants; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; -import frc.robot.subsystems.turret.calc.ShotCompensation; public class Targeting extends SubsystemBase { private static final double NOMINAL_SHOT_TIME_S = 0.3; // see github issue #23 (https://github.com/Team293/Rebuilt/issues/23) diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index de99f8a..dc31519 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,57 +1,60 @@ package frc.robot.subsystems.turret; -import org.littletonrobotics.junction.Logger; - import frc.lib.subsystem.SpikeSystem; import frc.robot.RobotContainer; - import frc.robot.subsystems.targeting.Targeting; -import frc.robot.subsystems.turret.calc.ShotCompensation; +import frc.robot.subsystems.targeting.ShotCompensation; public class Turret extends SpikeSystem { - private static final double TURRET_ANGLE_THRESHOLD_DEG = 3.0; // degrees within target angle to be considered "at target" - + public static final double TURRET_AIMING_TOLERANCE_DEGREES = 2.0; // degrees within which we consider the turret to be aimed at the target (+-) + + // HARDWARE CONSTANTS + // gearing + public static final double TURRET_GEAR_TEETH = 84.0; // number of teeth on fixed turret gear + public static final double PINION_ENCODER_TEETH = 10.0; // number of teeth on pinion gear (driving turret) + public static final double FOLLOWER_ENCODER_TEETH = 13.0; // number of teeth on follower gear + public static final double TURRET_GEAR_RATIO = TURRET_GEAR_TEETH / PINION_ENCODER_TEETH; // gear ratio from motor to turret + + public static final double DEGREES_PER_REV = 360.0; // degrees in one revolution + public static final double NORMALIZED_REVOLUTION = 1.0; // one full revolution in normalized units + + public static final double ENCODER_COMBINED_TEETH = PINION_ENCODER_TEETH * FOLLOWER_ENCODER_TEETH; + public static final double ENCODER_COMBINED_PERIOD_REV = ENCODER_COMBINED_TEETH / PINION_ENCODER_TEETH; + + public static final double PINION_ENCODER_OFFSET = 0.0; // offset for pinion encoder in degrees, to be determined by calibration + public static final double FOLLOWER_ENCODER_OFFSET = 0.0; // offset for follower encoder in degrees, to be determined by calibration + private TurretIO turretIO; - private double targetAngleDeg = 0.0; public Turret() { - super("Turret", new TurretIO.TurretIOInputs()); - System.out.println("Turret subsystem initialized"); + super("Turret", new TurretIOInputsAutoLogged()); } - - /** - * Continuously sets the turret angle to point at the target position - */ + @Override public void onPeriodic() { - Logger.recordOutput("Turret/Degrees", io.turretAngleDegrees); - - // compensate for robot movement ShotCompensation.AdjustedShot shotData = Targeting.getShotData(); if (shotData != null) { double newTargetAngleDeg = shotData.turretAngleDeg(); - this.targetAngleDeg = newTargetAngleDeg; - this.turretIO.setTurretAngle(newTargetAngleDeg); + this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg); } } - - /** - * Sets up the data refresher for - */ + @Override protected Runnable setupDataRefresher() { - this.turretIO = new TurretIOTalonFX(RobotContainer.getDrive()); - return useAsyncDataRefresher(this.turretIO); + turretIO = new TurretIOTalonFX(RobotContainer.getDrive()); + return useAsyncDataRefresher(turretIO); } /** - * Determines if turret is at target angle within error bounds - * @return if turret is at angle within bounds + * Checks if the turret is at the target angle, within the tolerance defined by TURRET_AIMING_TOLERANCE_DEGREES. + * @return true if the turret is at the target angle, false otherwise */ public boolean isAtTargetAngle() { - double angleError = Math.abs(super.io.turretAngleDegrees - targetAngleDeg); - return angleError < TURRET_ANGLE_THRESHOLD_DEG; + double minAngle = io.turretAngleDegrees - TURRET_AIMING_TOLERANCE_DEGREES; + double maxAngle = io.turretAngleDegrees + TURRET_AIMING_TOLERANCE_DEGREES; + + return io.targetTurretMotorRotations >= minAngle && io.targetTurretMotorRotations <= maxAngle; } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index 3074ba8..49ac69a 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -1,22 +1,27 @@ package frc.robot.subsystems.turret; -import edu.wpi.first.math.geometry.Pose2d; import frc.lib.subsystem.BaseIO; import frc.lib.subsystem.BaseInputClass; import frc.lib.subsystem.IORefresher; import org.littletonrobotics.junction.AutoLog; -public interface TurretIO extends BaseIO, IORefresher { +public interface TurretIO extends IORefresher, BaseIO { @AutoLog public static class TurretIOInputs extends BaseInputClass { - public double turretAngleDegrees = 0.0; - public double pinionEncoder = 0.0; - public double followerEncoder = 0.0; - public double turretSetPointDegrees = 0.0; - public Pose2d turretPosition = new Pose2d(); - public double robotOmegaDegPerSec = 0.0; + public double turretAngleDegrees = 0.0; // current angle of the turret, field-relative, in degrees + public double targetTurretMotorRotations = 0.0; // target position for the turret motor, in rotations + public double normalizedTurretMotorRotations = 0.0; // calculated turret motor rotations, updated when recalculation is called } - void setTurretAngle(double angle); + /** + * Set the angle of the turret, field-relative, in degrees. 0 degrees is facing straight forward, positive angles are clockwise, and negative angles are counterclockwise. [-180, 180] + * @param fieldRelativeAngleDegrees field-relative angle to set the turret to, in degrees. 0 degrees is facing straight forward, positive angles are clockwise, and negative angles are counterclockwise. [-180, 180] + */ + void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees); + + /** + * Recalculates the turret motor zero position to fix any encoder drift. + */ + void recalculateTurretMotorZeroPosition(); } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 8a636a0..3a14c05 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -1,191 +1,348 @@ package frc.robot.subsystems.turret; -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.units.measure.Angle; import frc.robot.CanID; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; -import frc.robot.subsystems.turret.calc.TurretMath; +import org.graalvm.collections.Pair; public class TurretIOTalonFX implements TurretIO { - private final TalonFX turretMotor; + // KS KV CONSTANTS + private static final double kS = 0.25; // volts needed to overcome static friction + private static final double kV = 0.20; // volts per (rotation per second) to maintain motion + + // SUBSYSTEMS private final CommandSwerveDrivetrain drive; - private static final double TURRET_LIMIT_DEG = 160.0; + // HARDWARE + private final TalonFX turretMotor; // kraken x44 + private final CANcoder pinionEncoder; // wcp throughbore + private final CANcoder followerEncoder; // wcp throughbore - private final MotionMagicVoltage mmVoltage = new MotionMagicVoltage(0); - private final StatusSignal encoderSignal; - private final BaseStatusSignal pinionEncoderSignal; - private final BaseStatusSignal followerEncoderSignal; + // SIGNALS + private final StatusSignal turretMotorPosition; + private final StatusSignal pinionEncoderSignal; + private final StatusSignal followerEncoderSignal; - private static final double kTurretGearRatio = 84.0/10.0; // turret ring: 84 teeth, motor pinion: 10 teeth + // CALIBRATION STATES (CRT) + private boolean isInitialized = false; // whether the turret has been initialized with a known position yet + private double lastPositionRevs = 0.0; // last calculated position of the turret in revolutions - private final double turretDegreesOffset = 180.0; // offset in degrees to align turret with front of robot + // VALUES + private double targetTurretAngleMotorRevs; // target angle of the turret in motor rotations + private double calculatedMotorOffsetRevs; // calculated offset in motor rotations based on the current position of the turret and the pinion encoder reading - private double targetAngleDeg = 0.0; // target angle of the turret in degrees + // COMMANDS + private final MotionMagicVoltage mmRequest = new MotionMagicVoltage(0.0); + private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(kS, kV); // ks, kv public TurretIOTalonFX(CommandSwerveDrivetrain drive) { + // SUBSYSTEMS this.drive = drive; + + // HARDWARE this.turretMotor = new TalonFX(CanID.TURRET_MOTOR.getID()); + this.pinionEncoder = new CANcoder(CanID.TURRET_PINION_CANCODER.getID()); + this.followerEncoder = new CANcoder(CanID.TURRET_FOLLOWER_CANCODER.getID()); + + // CONFIGURATIONS + var turretMotorConfig = getTurretMotionConfigs(); + var pinionEncoderConfig = getPinionEncoderConfigs(); + var followerEncoderConfig = getFollowerEncoderConfigs(); + var turretMotorFeedbackConfig = getTurretMotorFeedbackConfigs(); + var turretSoftwareLimitConfig = getTurretSoftwareLimitConfigs(); + + this.turretMotor.getConfigurator().apply(turretMotorConfig.getLeft()); + this.turretMotor.getConfigurator().apply(turretMotorConfig.getRight()); + this.turretMotor.getConfigurator().apply(turretMotorFeedbackConfig); + this.turretMotor.getConfigurator().apply(turretSoftwareLimitConfig); + + this.pinionEncoder.getConfigurator().apply(pinionEncoderConfig); + this.followerEncoder.getConfigurator().apply(followerEncoderConfig); + + // SIGNALS + this.turretMotorPosition = this.turretMotor.getPosition(); + this.pinionEncoderSignal = this.pinionEncoder.getAbsolutePosition(); + this.followerEncoderSignal = this.followerEncoder.getAbsolutePosition(); + + // ZEROING POSITION + recalculateTurretMotorZeroPosition(); + } - MotionMagicConfigs mm = new MotionMagicConfigs(); - mm.MotionMagicAcceleration = 10; // rot/sec^2 - mm.MotionMagicCruiseVelocity = 10; // rot/sec + @Override + public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) { + double currentRobotHeading = this.drive.getPose().getRotation().getDegrees(); - // Motor configuration - Slot0Configs config = getTurretMotorConfig(); + // absolute robot-relative target, in motor rotations + double targetRobotRelativeDeg = wrap180(fieldRelativeAngleDegrees - currentRobotHeading); + double targetMotorRotations = targetRobotRelativeDeg / 180.0; - this.turretMotor.getConfigurator().apply(config); - this.turretMotor.getConfigurator().apply(mm); + this.targetTurretAngleMotorRevs = targetMotorRotations; - CANcoder pinionEncoder = new CANcoder(CanID.TURRET_PINION_CANCODER.getID()); - CANcoder followerEncoder = new CANcoder(CanID.TURRET_FOLLOWER_CANCODER.getID()); + this.turretMotor.setControl( + mmRequest + .withPosition(targetMotorRotations) + .withFeedForward(calculateFeedforward()) + ); + } + + /** + * @inheritDoc + */ + @Override + public void recalculateTurretMotorZeroPosition() { + double currentTurretAngle = getTurretAngleRobotRelative(); // get the current angle of the turret in robot frame - this.pinionEncoderSignal = pinionEncoder.getAbsolutePosition(); - this.followerEncoderSignal = followerEncoder.getAbsolutePosition(); + double normalizedPosition = currentTurretAngle / 180.0; // convert to normalized position [-1, 1] + this.calculatedMotorOffsetRevs = normalizedPosition; // calculate the offset in motor rotations based on the current turret angle - this.encoderSignal = this.turretMotor.getPosition(); + this.turretMotor.setPosition(normalizedPosition); + } - double pinionEncoderValue = this.pinionEncoderSignal.getValueAsDouble(); - double followerEncoderValue = this.followerEncoderSignal.getValueAsDouble(); + /** + * Calculates the feedforward voltage to apply to the turret motor to counteract the rotation of the robot, based on the current angular velocity of the robot. + * @return the feedforward value to apply to the turret rotation + */ + private double calculateFeedforward() { + // get the current angular velocity of the robot in radians per second + double gyroOmegaRadPerSecond = drive.getState().Speeds.omegaRadiansPerSecond; - // set offset of the turret on startup - double turretRotations = TurretMath.getTurretAngleRevs(pinionEncoderValue, followerEncoderValue); - - double turretDegrees = TurretMath.normalizeTurretHeading( - TurretMath.toDegreesWrapped(turretRotations), - this.turretDegreesOffset - ); + double gyroOmegaDegPerSecond = gyroOmegaRadPerSecond * (180.0 / Math.PI); // convert to degrees per second - double currentMotorRevs = this.turretMotor.getPosition().getValueAsDouble(); - double toZeroRevs = TurretMath.degreesToMotorPosition(turretDegrees); + return feedforward.calculate(gyroOmegaDegPerSecond); + } - turretMotor.setPosition(-toZeroRevs); + @Override + public void refreshData() { + StatusSignal.refreshAll(this.turretMotorPosition, this.pinionEncoderSignal, this.followerEncoderSignal); } + @Override + public void updateInputs(TurretIOInputs inputs) { + System.out.println("Updating inputs: turret angle (field-relative) = " + getTurretAngleFieldRelative()); + inputs.turretAngleDegrees = getTurretAngleFieldRelative(); + inputs.targetTurretMotorRotations = this.targetTurretAngleMotorRevs; + inputs.normalizedTurretMotorRotations = this.calculatedMotorOffsetRevs; + } + + // CONFIGURATIONS + /** - * Set the turret angle to a target field angle - * @param fieldTargetHeadingDeg field relative angle to set turret to + * Get the motor configurations for the turret motor. + * @return the motor configurations for the turret motor */ - @Override - public void setTurretAngle(double fieldTargetHeadingDeg) { - Logger.recordOutput("Turret/TargetAngle", fieldTargetHeadingDeg); - this.targetAngleDeg = fieldTargetHeadingDeg; - fieldTargetHeadingDeg = MathUtil.inputModulus(fieldTargetHeadingDeg, 0.0, 360.0); - - // convert robot heading to [0, 360) range - double robotHeadingDeg = - MathUtil.inputModulus( - drive.getPose().getRotation().getDegrees(), - 0.0, 360.0 - ); - - // dt = time we expect turret to reach commanded angle, tunable - double dt = 0.025; - double predictedHeadingDeg = - robotHeadingDeg + this.drive.getRobotOmegaDegPerSec() * dt; - - // turret angle in (-180, 180] range, where positive is counterclockwise relative to the robot's forward direction, and negative is clockwise - double turretAngleDeg = - MathUtil.inputModulus( - fieldTargetHeadingDeg - predictedHeadingDeg, - -180.0, 180.0 - ); - - if (turretAngleDeg > TURRET_LIMIT_DEG) { - turretAngleDeg -= 360.0; - } else if (turretAngleDeg < -TURRET_LIMIT_DEG) { - turretAngleDeg += 360.0; - } + private Pair getTurretMotionConfigs() { + Slot0Configs configs = new Slot0Configs(); + + configs.kP = 1; + configs.kI = 0.0; + configs.kD = 0.0; + + configs.kS = kS; + configs.kV = kV; + + MotionMagicConfigs mmConfigs = new MotionMagicConfigs(); + + mmConfigs.MotionMagicAcceleration = 20; // rotations per second^2 + mmConfigs.MotionMagicCruiseVelocity = 10; // rotations per second + + return Pair.create(configs, mmConfigs); + } + + /** + * Get the feedback configurations for the turret motor, which define the relationship between the motor rotations, the pinion encoder rotations, and the follower encoder rotations. + * @return the feedback configurations for the turret motor + */ + private FeedbackConfigs getTurretMotorFeedbackConfigs() { + FeedbackConfigs configs = new FeedbackConfigs(); + + // SensorToMechanismRatio = GR/2 means the motor completes 2 mechanism rotations per + // full turret revolution, so 1 mechanism rotation = 180 degrees of turret travel. + // Soft limits at +-1 mechanism rotation enforce the +-180 degrees physical range of the turret. + configs.SensorToMechanismRatio = Turret.TURRET_GEAR_RATIO / 2.0; + configs.RotorToSensorRatio = 1; + + return configs; + } + + /** + * Get the software limit switch configurations for the turret motor, which define the forward and reverse limits of the turret based on the motor position. + * @return the software limit switch configurations for the turret motor + */ + private SoftwareLimitSwitchConfigs getTurretSoftwareLimitConfigs() { + SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs(); + + configs.ForwardSoftLimitEnable = true; + configs.ForwardSoftLimitThreshold = 1; // 1 rotation of the motor past the zero point + + configs.ReverseSoftLimitEnable = true; + configs.ReverseSoftLimitThreshold = -1; // 1 rotation of the motor in the opposite direction past the zero point - // convert turret angle to motor rotations, accounting for gear ratio and offset - double turretAngleForMotor = turretAngleDeg; - if (turretAngleForMotor > 180.0) { - turretAngleForMotor -= 360.0; + return configs; + } + + /** + * Get the encoder configurations for the turret encoders. + * @return the encoder configurations for the turret encoders + */ + public CANcoderConfiguration getEncoderConfigs() { + CANcoderConfiguration configs = new CANcoderConfiguration(); + // constrain reading between [0, 1) + configs.MagnetSensor.withAbsoluteSensorDiscontinuityPoint(1.0); + return configs; + } + + public CANcoderConfiguration getPinionEncoderConfigs() { + CANcoderConfiguration configs = getEncoderConfigs(); + configs.MagnetSensor.MagnetOffset = Turret.FOLLOWER_ENCODER_OFFSET; + + return configs; + } + + public CANcoderConfiguration getFollowerEncoderConfigs() { + CANcoderConfiguration configs = getEncoderConfigs(); + configs.MagnetSensor.MagnetOffset = Turret.PINION_ENCODER_OFFSET; + + return configs; + } + + // CRT METHODS + + /** + * Calculates the continuous position of the pinion (driving) encoder in revolutions + * @return the continuous position of the pinion encoder in revolutions + */ + private double getPinionEncoderRevs() { + double pinionEncoderReading = positiveMod(this.pinionEncoderSignal.getValueAsDouble(), Turret.NORMALIZED_REVOLUTION); + double followerEncoderReading = positiveMod(this.followerEncoderSignal.getValueAsDouble(), Turret.NORMALIZED_REVOLUTION); + + double bestError = Double.MAX_VALUE; + double bestPosition = 0.0; + + int searchCount = (int) Turret.FOLLOWER_ENCODER_TEETH; // number of distinct branches to check (follower encoder teeth) + for (int k = 0; k < searchCount; k++) { + double assumedPinionRevs = pinionEncoderReading + k; + double predictedFollowerReading = positiveMod(assumedPinionRevs * (Turret.PINION_ENCODER_TEETH / Turret.FOLLOWER_ENCODER_TEETH), Turret.NORMALIZED_REVOLUTION); + double predictionError = Math.abs(predictedFollowerReading - followerEncoderReading); + + // check wrap-around error + if (predictionError > Turret.NORMALIZED_REVOLUTION / 2.0) { + predictionError = Turret.NORMALIZED_REVOLUTION - predictionError; + } + + // if this branch has a better prediction error than the best one so far, update the best guess for the pinion encoder position + if (predictionError < bestError) { + bestError = predictionError; + bestPosition = assumedPinionRevs; + } } - Logger.recordOutput("Turret/TargetAngleForMotor", turretAngleForMotor); - double rotations = -(turretAngleForMotor / 360.0) * kTurretGearRatio; - Logger.recordOutput("Turret/TargetRotations", rotations); - // set the motor to the desired position with feedforward to counteract robot rotation - this.turretMotor.setControl( - this.mmVoltage - .withPosition(rotations) - // 0.1167 is an empirically determined gain to convert from motor RPS to voltage needed to hold position against rotation - // .withFeedForward(motorRps * 0.1167) - ); + return bestPosition; } - - + /** - * Periodically called to update the Turret information for logging - * @param inputs TurretIOInputs object to update + * Calculates the continuous position of the turret in revolutions of the entire mechanism. + * @return the continuous position of the turret in revolutions. */ - @Override - public void updateInputs(TurretIOInputs inputs) { - inputs.pinionEncoder = this.pinionEncoderSignal.getValueAsDouble(); - inputs.followerEncoder = this.followerEncoderSignal.getValueAsDouble(); - inputs.turretSetPointDegrees = this.targetAngleDeg; + private double getTurretPositionRevs() { + double rawPinionRevs = getPinionEncoderRevs(); + double rawTurretRevs = rawPinionRevs * (Turret.PINION_ENCODER_TEETH / Turret.TURRET_GEAR_TEETH); // convert pinion revolutions to turret revolutions - double turretRotations = TurretMath.getTurretAngleRevs(inputs.pinionEncoder, inputs.followerEncoder); + double wrapped = positiveMod(rawTurretRevs, Turret.ENCODER_COMBINED_PERIOD_REV); - inputs.robotOmegaDegPerSec = this.drive.getRobotOmegaDegPerSec(); + if (!isInitialized) { + this.lastPositionRevs = wrapped; + isInitialized = true; + return wrapped; + } - // convert raw encoder readings to turret angle in degrees, accounting for gear ratio and offset - double turretAngleDegreesNonNormalized = TurretMath.normalizeTurretHeading( - TurretMath.toDegreesWrapped(turretRotations), - turretDegreesOffset - ); + double delta = wrapped - this.lastPositionRevs; - // normalize to -180 to 180 range - if (turretAngleDegreesNonNormalized > 180.0) { - inputs.turretAngleDegrees = turretAngleDegreesNonNormalized - 360.0; - } else if (turretAngleDegreesNonNormalized < -180.0) { - inputs.turretAngleDegrees = turretAngleDegreesNonNormalized + 360.0; - } else { - inputs.turretAngleDegrees = turretAngleDegreesNonNormalized; + // if the change in position is greater than half the combined period, we have wrapped around the encoder, so we need to adjust the delta accordingly + if (delta > Turret.ENCODER_COMBINED_PERIOD_REV / 2.0) { + delta -= Turret.ENCODER_COMBINED_PERIOD_REV; + } else if (delta < -Turret.ENCODER_COMBINED_PERIOD_REV / 2.0) { + delta += Turret.ENCODER_COMBINED_PERIOD_REV; } - // creates a position for the turret based on robot position, rotated by turret angle for logging - inputs.turretPosition = new Pose2d(drive.getPose().getTranslation(), new Rotation2d(TurretMath.toRad(inputs.turretAngleDegrees))); + lastPositionRevs += delta; + return lastPositionRevs; } /** - * Periodically refreshes encoder signal - * @note This is called automatically + * Converts rotations of the turret mechanism to degrees (heading) of the turret. + * @param turretRevs continuous revolutions of the turret + * @return degrees of the turret from revolutions, continuous and unwrapped */ - @Override - public void refreshData() { - BaseStatusSignal.refreshAll(encoderSignal, pinionEncoderSignal, followerEncoderSignal); + private double revsToDegreesContinuous(double turretRevs) { + return turretRevs * Turret.DEGREES_PER_REV; } - public static Slot0Configs getTurretMotorConfig() { - Slot0Configs config = new Slot0Configs(); - - // config.kP = 1; - // config.kI = 0.0; - // config.kD = 0.0; + /** + * Gets the angle of the turret in robot space wrapped from [-180, 180) + * @return the angle of the turret in robot space, wrapped + */ + private double getTurretAngleRobotRelative() { + double continuousRevs = getTurretPositionRevs(); + double turretAngleDegrees = revsToDegreesContinuous(continuousRevs); - // config.kS = 0.25; - // config.kV = 0.20; + // apply offset and wrap to [-180, 180) + return wrap180(turretAngleDegrees); + } - config.kP = 0.0; - config.kI = 0.0; - config.kD = 0.0; + /** + * Gets the angle of the turret in field space, wrapped from [-180, 180) + * @return the angle of the turret in field space, wrapped + */ + private double getTurretAngleFieldRelative() { + double robotRelativeAngle = getTurretAngleRobotRelative(); + double currentRobotHeading = this.drive.getPose().getRotation().getDegrees(); + + double fieldCentricContinuous = robotRelativeAngle - currentRobotHeading; - config.kS = 0.0; - config.kV = 0.0; + return wrap180(fieldCentricContinuous); + } - return config; + // UTILITY METHODS + + /** + * Wraps the input angle to be within the range [min, max). + * @param input the angle to wrap + * @param min the minimum angle of the range (inclusive) + * @param max the maximum angle of the range (exclusive) + * @return the wrapped angle within the range [min, max) + */ + private double wrap(double input, double min, double max) { + // input modulo the range size + return MathUtil.inputModulus( + input, + min, + max + ); + } + + /** + * Wraps the input angle to be within the range [-180, 180). + * @param input the angle to wrap + * @return the wrapped angle within the range [-180, 180) + */ + private double wrap180(double input) { + return wrap(input, -180.0, 180.0); + } + + /** + * A positive modulus function that wraps x into the range [0, m). + * @param x the value to wrap + * @param m the modulus + * @return the wrapped value in the range [0, m) + */ + private double positiveMod(double x, double m) { + return ((x % m) + m) % m; } } diff --git a/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java b/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java deleted file mode 100644 index 7887d70..0000000 --- a/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java +++ /dev/null @@ -1,192 +0,0 @@ -package frc.robot.subsystems.turret.calc; - -/** - * Utility class for computing absolute turret position using two geared absolute encoders - * with a CRT-like search approach. - * - * Hardware setup (example): - * - Turret gear: 140 teeth - * - Encoder A: e.g. 13-tooth gear driving an absolute encoder (reads 0-1 rev) - * - Encoder B: e.g. 11-tooth gear driving an absolute encoder (reads 0-1 rev) - * - * The combination gives unique positions over (encoderA_teeth x encoderB_teeth) teeth. - * Beyond that range, continuity tracking (unwrapping) is used. - */ -public class TurretMath { - - // Hardware tooth counts - private static final double TURRET_GEAR_TEETH = 84.0; - private static final double ENCODER_A_TEETH = 10.0; - private static final double ENCODER_B_TEETH = 13.0; - - // Derived encoder combination values - private static final double ENCODER_COMBINED_TEETH = ENCODER_A_TEETH * ENCODER_B_TEETH; - private static final double ENCODER_COMBINED_PERIOD_REV = ENCODER_COMBINED_TEETH / TURRET_GEAR_TEETH; - private static final double HALF_ENCODER_COMBINED_PERIOD_REV = ENCODER_COMBINED_PERIOD_REV / 2.0; - - private static final double NORMALIZED_REV = 1.0; // encoder reading range (0..1) - private static final double HALF_NORMALIZED_REV = NORMALIZED_REV / 2.0; - private static final double DEGREES_PER_REV = 360.0; - private static final double RAD_PER_REV = 2.0 * Math.PI; - private static final double MOTOR_UNITS_PER_REV = TURRET_GEAR_TEETH/ENCODER_A_TEETH; // scale used in degreesToMotorPosition - - // Defaults / initial values - private static final double DEFAULT_OFFSET_DEGREES = 0.0; - private static final double DEFAULT_LAST_POSITION_REVS = 0.0; - - // State - private static double offsetDegrees = DEFAULT_OFFSET_DEGREES; // Calibration offset (degrees) - private static double lastPositionRevs = DEFAULT_LAST_POSITION_REVS; // Unwrapped continuous position - private static boolean initialized = false; - - /** - * Helper method to perform modulus that always returns a positive result, wrapping x into [0, m). - * @param x value to wrap - * @param m modulus (e.g. 1.0 for normalized encoder readings) - * @return wrapped value in [0, m) - */ - private static double mod(double x, double m) { - return ((x % m) + m) % m; - } - - /** - * Computes the raw absolute turret position in revolutions of the encoder-A gear - * (i.e. position in "encoder-A-gear-equivalent revolutions"). - * Range is approximately 0 to (combined_teeth / encoderA_teeth) revolutions of the encoder-A gear. - * - * This uses a search-based approach across the possible integer wraps of the encoder-A reading - * to find the branch that best matches the encoder-B reading. - * - * @param encoderAReading Absolute encoder A reading (normalized [0, 1)) - * @param encoderBReading Absolute encoder B reading (normalized [0, 1)) - * @return Raw turret position in encoder-A-gear revolutions - */ - public static double getRawPositionPrimaryRevs(double encoderAReading, double encoderBReading) { - double offsetRevs = offsetDegrees / DEGREES_PER_REV; - - // Apply offset and wrap to [0,1) - encoderAReading = mod(encoderAReading - offsetRevs, NORMALIZED_REV); - encoderBReading = mod(encoderBReading - offsetRevs, NORMALIZED_REV); - - double bestError = Double.POSITIVE_INFINITY; - double bestPosition = 0.0; - - // Search over the possible integer wraps of the secondary/primary relationship - int searchCount = (int) ENCODER_B_TEETH; // number of distinct branches to check (encoder B teeth) - for (int k = 0; k < searchCount; k++) { - double assumedPrimaryRevs = encoderAReading + k; - // Predict what the encoder-B reading should be if this is the correct branch - double predictedEncoderB = mod(assumedPrimaryRevs * (ENCODER_A_TEETH / ENCODER_B_TEETH), NORMALIZED_REV); - - double error = Math.abs(predictedEncoderB - encoderBReading); - // Handle wrap-around distance - if (error > HALF_NORMALIZED_REV) { - error = NORMALIZED_REV - error; - } - - if (error < bestError) { - bestError = error; - bestPosition = assumedPrimaryRevs; - } - } - - return bestPosition; - } - - /** - * Returns the turret angle in revolutions, unwrapped for continuous multi-turn motion. - * Applies offset and continuity tracking. - * - * @param encoderAReading Absolute encoder A reading [0,1) - * @param encoderBReading Absolute encoder B reading [0,1) - * @return Continuous turret position in revolutions (can be >1 or <0) - */ - public static double getTurretAngleRevs(double encoderAReading, double encoderBReading) { - double rawPrimaryRevs = getRawPositionPrimaryRevs(encoderAReading, encoderBReading); - double rawTurretRevs = rawPrimaryRevs * (ENCODER_A_TEETH / TURRET_GEAR_TEETH); - - // Apply offset again (in turret space) - rawTurretRevs -= offsetDegrees / DEGREES_PER_REV; - - // Wrap raw reading into one encoded period for comparison - double wrapped = mod(rawTurretRevs, ENCODER_COMBINED_PERIOD_REV); - - if (!initialized) { - lastPositionRevs = wrapped; - initialized = true; - return wrapped; - } - - // Compute shortest path delta (assuming small motion between calls) - double delta = wrapped - lastPositionRevs; - - // Unwrap using the known encoder combined period - if (delta > HALF_ENCODER_COMBINED_PERIOD_REV) { - delta -= ENCODER_COMBINED_PERIOD_REV; - } else if (delta < -HALF_ENCODER_COMBINED_PERIOD_REV) { - delta += ENCODER_COMBINED_PERIOD_REV; - } - - lastPositionRevs += delta; - return lastPositionRevs; - } - - /** - * Convert turret revolutions to degrees, wrapped to [0, 360). - * Use this when you only care about single-turn angle. - */ - public static double toDegreesWrapped(double turretRevs) { - return mod(turretRevs, NORMALIZED_REV) * DEGREES_PER_REV; - } - - /** - * Convert turret revolutions to radians, wrapped to [0, 2pi). - */ - public static double toRadiansWrapped(double turretRevs) { - return mod(turretRevs, NORMALIZED_REV) * RAD_PER_REV; - } - - public static double degreesToMotorPosition(double turretDegrees) { - if (turretDegrees > 180.0) { - turretDegrees -= 360.0; - } - return (turretDegrees * MOTOR_UNITS_PER_REV) / DEGREES_PER_REV; - } - - public static double normalizeTurretHeading(double turretHeading, double zeroDegrees) { - double newHeading = turretHeading - zeroDegrees; - - if (newHeading < 0) { - newHeading = DEGREES_PER_REV - Math.abs(newHeading); - } - - return newHeading; - } - - /** - * Convert turret revolutions to total accumulated degrees (can be >360 or <0). - * Use this when you want continuous angle for PID or motion profiling. - */ - public static double toDegreesContinuous(double turretRevs) { - return turretRevs * DEGREES_PER_REV; - } - - // Calibration / zeroing - public static void setOffsetDegrees(double degrees) { - offsetDegrees = degrees; - } - - public static double getOffsetDegrees() { - return offsetDegrees; - } - - // Reset continuity tracker - public static void resetContinuity() { - initialized = false; - lastPositionRevs = DEFAULT_LAST_POSITION_REVS; - } - - public static double toRad(double angle) { - return angle * (Math.PI / DEGREES_PER_REV); - } -} From 1936f7c83df45cdb53ae2abfd37a72e5ce85be24 Mon Sep 17 00:00:00 2001 From: nathan Date: Sun, 8 Mar 2026 20:09:30 -0400 Subject: [PATCH 06/29] Add region utilities and set hood angle when within trench bounds. Co-authored-by: PillageDev --- src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../calc => targeting}/ShotCompensation.java | 27 +- .../{turret/calc => targeting}/ShotData.java | 2 +- .../robot/subsystems/targeting/Targeting.java | 1 - .../frc/robot/subsystems/turret/Turret.java | 59 +-- .../frc/robot/subsystems/turret/TurretIO.java | 23 +- .../subsystems/turret/TurretIOTalonFX.java | 419 ++++++++++++------ .../subsystems/turret/calc/TurretMath.java | 192 -------- 9 files changed, 336 insertions(+), 390 deletions(-) rename src/main/java/frc/robot/subsystems/{turret/calc => targeting}/ShotCompensation.java (79%) rename src/main/java/frc/robot/subsystems/{turret/calc => targeting}/ShotData.java (92%) delete mode 100644 src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2231cbf..1e3e39b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d0aa767..ddf79eb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,7 +2,7 @@ import frc.lib.subsystem.SpikeSystem; import frc.robot.subsystems.targeting.Targeting; -import frc.robot.subsystems.turret.calc.ShotCompensation; +import frc.robot.subsystems.targeting.ShotCompensation; public class Shooter extends SpikeSystem { private static final double SHOOTER_READY_THRESHOLD_RPS = 0.5; // RPS threshold to consider the shooter ready diff --git a/src/main/java/frc/robot/subsystems/turret/calc/ShotCompensation.java b/src/main/java/frc/robot/subsystems/targeting/ShotCompensation.java similarity index 79% rename from src/main/java/frc/robot/subsystems/turret/calc/ShotCompensation.java rename to src/main/java/frc/robot/subsystems/targeting/ShotCompensation.java index f19f80e..140d63b 100644 --- a/src/main/java/frc/robot/subsystems/turret/calc/ShotCompensation.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotCompensation.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.turret.calc; +package frc.robot.subsystems.targeting; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -87,29 +87,4 @@ public static AdjustedShot compensateForMovement( return new AdjustedShot(baseRPM, hoodDeg, effectiveTurretDeg, turretFF_deg_s, rangeFF_m_s); } - - /** - * Calculate a feedforward term to compensate for the robot's velocity toward the target, which reduces effective range and thus shot time. - * @param velocity current robot velocity in field-relative coordinates - * @param directionToTarget unit vector pointing from robot to target in field-relative coordinates - * @return feedforward term to add to shot speed (in whatever units the caller expects, legacy scaling applies) - */ - private static double calculateVelocityCompensation( - ChassisSpeeds velocity, - Translation2d directionToTarget) { - - // project robot velocity onto the direction to target to get speed toward target - double robotSpeedTowardTarget = - velocity.vxMetersPerSecond * Math.cos(directionToTarget.getAngle().getRadians()) + - velocity.vyMetersPerSecond * Math.sin(directionToTarget.getAngle().getRadians()); - - // multiply by 100 to convert to whatever unit the caller expects (legacy scaling) - return robotSpeedTowardTarget * 100.0; - } - - private static double rpmToVelocity(double rpm) { - // convert wheel rpm to linear m/s - // wheel diameter 4 inches => 0.1016 m - return rpm * (Math.PI * 0.1016) / 60.0; - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java similarity index 92% rename from src/main/java/frc/robot/subsystems/turret/calc/ShotData.java rename to src/main/java/frc/robot/subsystems/targeting/ShotData.java index b5fb97a..4730a7c 100644 --- a/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.turret.calc; +package frc.robot.subsystems.targeting; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index e30efdb..c791192 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -11,7 +11,6 @@ import frc.lib.Elastic.NotificationLevel; import frc.lib.FieldConstants; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; -import frc.robot.subsystems.turret.calc.ShotCompensation; public class Targeting extends SubsystemBase { private static final double NOMINAL_SHOT_TIME_S = 0.3; // see github issue #23 (https://github.com/Team293/Rebuilt/issues/23) diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index de99f8a..dc31519 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,57 +1,60 @@ package frc.robot.subsystems.turret; -import org.littletonrobotics.junction.Logger; - import frc.lib.subsystem.SpikeSystem; import frc.robot.RobotContainer; - import frc.robot.subsystems.targeting.Targeting; -import frc.robot.subsystems.turret.calc.ShotCompensation; +import frc.robot.subsystems.targeting.ShotCompensation; public class Turret extends SpikeSystem { - private static final double TURRET_ANGLE_THRESHOLD_DEG = 3.0; // degrees within target angle to be considered "at target" - + public static final double TURRET_AIMING_TOLERANCE_DEGREES = 2.0; // degrees within which we consider the turret to be aimed at the target (+-) + + // HARDWARE CONSTANTS + // gearing + public static final double TURRET_GEAR_TEETH = 84.0; // number of teeth on fixed turret gear + public static final double PINION_ENCODER_TEETH = 10.0; // number of teeth on pinion gear (driving turret) + public static final double FOLLOWER_ENCODER_TEETH = 13.0; // number of teeth on follower gear + public static final double TURRET_GEAR_RATIO = TURRET_GEAR_TEETH / PINION_ENCODER_TEETH; // gear ratio from motor to turret + + public static final double DEGREES_PER_REV = 360.0; // degrees in one revolution + public static final double NORMALIZED_REVOLUTION = 1.0; // one full revolution in normalized units + + public static final double ENCODER_COMBINED_TEETH = PINION_ENCODER_TEETH * FOLLOWER_ENCODER_TEETH; + public static final double ENCODER_COMBINED_PERIOD_REV = ENCODER_COMBINED_TEETH / PINION_ENCODER_TEETH; + + public static final double PINION_ENCODER_OFFSET = 0.0; // offset for pinion encoder in degrees, to be determined by calibration + public static final double FOLLOWER_ENCODER_OFFSET = 0.0; // offset for follower encoder in degrees, to be determined by calibration + private TurretIO turretIO; - private double targetAngleDeg = 0.0; public Turret() { - super("Turret", new TurretIO.TurretIOInputs()); - System.out.println("Turret subsystem initialized"); + super("Turret", new TurretIOInputsAutoLogged()); } - - /** - * Continuously sets the turret angle to point at the target position - */ + @Override public void onPeriodic() { - Logger.recordOutput("Turret/Degrees", io.turretAngleDegrees); - - // compensate for robot movement ShotCompensation.AdjustedShot shotData = Targeting.getShotData(); if (shotData != null) { double newTargetAngleDeg = shotData.turretAngleDeg(); - this.targetAngleDeg = newTargetAngleDeg; - this.turretIO.setTurretAngle(newTargetAngleDeg); + this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg); } } - - /** - * Sets up the data refresher for - */ + @Override protected Runnable setupDataRefresher() { - this.turretIO = new TurretIOTalonFX(RobotContainer.getDrive()); - return useAsyncDataRefresher(this.turretIO); + turretIO = new TurretIOTalonFX(RobotContainer.getDrive()); + return useAsyncDataRefresher(turretIO); } /** - * Determines if turret is at target angle within error bounds - * @return if turret is at angle within bounds + * Checks if the turret is at the target angle, within the tolerance defined by TURRET_AIMING_TOLERANCE_DEGREES. + * @return true if the turret is at the target angle, false otherwise */ public boolean isAtTargetAngle() { - double angleError = Math.abs(super.io.turretAngleDegrees - targetAngleDeg); - return angleError < TURRET_ANGLE_THRESHOLD_DEG; + double minAngle = io.turretAngleDegrees - TURRET_AIMING_TOLERANCE_DEGREES; + double maxAngle = io.turretAngleDegrees + TURRET_AIMING_TOLERANCE_DEGREES; + + return io.targetTurretMotorRotations >= minAngle && io.targetTurretMotorRotations <= maxAngle; } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index 3074ba8..49ac69a 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -1,22 +1,27 @@ package frc.robot.subsystems.turret; -import edu.wpi.first.math.geometry.Pose2d; import frc.lib.subsystem.BaseIO; import frc.lib.subsystem.BaseInputClass; import frc.lib.subsystem.IORefresher; import org.littletonrobotics.junction.AutoLog; -public interface TurretIO extends BaseIO, IORefresher { +public interface TurretIO extends IORefresher, BaseIO { @AutoLog public static class TurretIOInputs extends BaseInputClass { - public double turretAngleDegrees = 0.0; - public double pinionEncoder = 0.0; - public double followerEncoder = 0.0; - public double turretSetPointDegrees = 0.0; - public Pose2d turretPosition = new Pose2d(); - public double robotOmegaDegPerSec = 0.0; + public double turretAngleDegrees = 0.0; // current angle of the turret, field-relative, in degrees + public double targetTurretMotorRotations = 0.0; // target position for the turret motor, in rotations + public double normalizedTurretMotorRotations = 0.0; // calculated turret motor rotations, updated when recalculation is called } - void setTurretAngle(double angle); + /** + * Set the angle of the turret, field-relative, in degrees. 0 degrees is facing straight forward, positive angles are clockwise, and negative angles are counterclockwise. [-180, 180] + * @param fieldRelativeAngleDegrees field-relative angle to set the turret to, in degrees. 0 degrees is facing straight forward, positive angles are clockwise, and negative angles are counterclockwise. [-180, 180] + */ + void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees); + + /** + * Recalculates the turret motor zero position to fix any encoder drift. + */ + void recalculateTurretMotorZeroPosition(); } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 8a636a0..3a14c05 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -1,191 +1,348 @@ package frc.robot.subsystems.turret; -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.units.measure.Angle; import frc.robot.CanID; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; -import frc.robot.subsystems.turret.calc.TurretMath; +import org.graalvm.collections.Pair; public class TurretIOTalonFX implements TurretIO { - private final TalonFX turretMotor; + // KS KV CONSTANTS + private static final double kS = 0.25; // volts needed to overcome static friction + private static final double kV = 0.20; // volts per (rotation per second) to maintain motion + + // SUBSYSTEMS private final CommandSwerveDrivetrain drive; - private static final double TURRET_LIMIT_DEG = 160.0; + // HARDWARE + private final TalonFX turretMotor; // kraken x44 + private final CANcoder pinionEncoder; // wcp throughbore + private final CANcoder followerEncoder; // wcp throughbore - private final MotionMagicVoltage mmVoltage = new MotionMagicVoltage(0); - private final StatusSignal encoderSignal; - private final BaseStatusSignal pinionEncoderSignal; - private final BaseStatusSignal followerEncoderSignal; + // SIGNALS + private final StatusSignal turretMotorPosition; + private final StatusSignal pinionEncoderSignal; + private final StatusSignal followerEncoderSignal; - private static final double kTurretGearRatio = 84.0/10.0; // turret ring: 84 teeth, motor pinion: 10 teeth + // CALIBRATION STATES (CRT) + private boolean isInitialized = false; // whether the turret has been initialized with a known position yet + private double lastPositionRevs = 0.0; // last calculated position of the turret in revolutions - private final double turretDegreesOffset = 180.0; // offset in degrees to align turret with front of robot + // VALUES + private double targetTurretAngleMotorRevs; // target angle of the turret in motor rotations + private double calculatedMotorOffsetRevs; // calculated offset in motor rotations based on the current position of the turret and the pinion encoder reading - private double targetAngleDeg = 0.0; // target angle of the turret in degrees + // COMMANDS + private final MotionMagicVoltage mmRequest = new MotionMagicVoltage(0.0); + private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(kS, kV); // ks, kv public TurretIOTalonFX(CommandSwerveDrivetrain drive) { + // SUBSYSTEMS this.drive = drive; + + // HARDWARE this.turretMotor = new TalonFX(CanID.TURRET_MOTOR.getID()); + this.pinionEncoder = new CANcoder(CanID.TURRET_PINION_CANCODER.getID()); + this.followerEncoder = new CANcoder(CanID.TURRET_FOLLOWER_CANCODER.getID()); + + // CONFIGURATIONS + var turretMotorConfig = getTurretMotionConfigs(); + var pinionEncoderConfig = getPinionEncoderConfigs(); + var followerEncoderConfig = getFollowerEncoderConfigs(); + var turretMotorFeedbackConfig = getTurretMotorFeedbackConfigs(); + var turretSoftwareLimitConfig = getTurretSoftwareLimitConfigs(); + + this.turretMotor.getConfigurator().apply(turretMotorConfig.getLeft()); + this.turretMotor.getConfigurator().apply(turretMotorConfig.getRight()); + this.turretMotor.getConfigurator().apply(turretMotorFeedbackConfig); + this.turretMotor.getConfigurator().apply(turretSoftwareLimitConfig); + + this.pinionEncoder.getConfigurator().apply(pinionEncoderConfig); + this.followerEncoder.getConfigurator().apply(followerEncoderConfig); + + // SIGNALS + this.turretMotorPosition = this.turretMotor.getPosition(); + this.pinionEncoderSignal = this.pinionEncoder.getAbsolutePosition(); + this.followerEncoderSignal = this.followerEncoder.getAbsolutePosition(); + + // ZEROING POSITION + recalculateTurretMotorZeroPosition(); + } - MotionMagicConfigs mm = new MotionMagicConfigs(); - mm.MotionMagicAcceleration = 10; // rot/sec^2 - mm.MotionMagicCruiseVelocity = 10; // rot/sec + @Override + public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) { + double currentRobotHeading = this.drive.getPose().getRotation().getDegrees(); - // Motor configuration - Slot0Configs config = getTurretMotorConfig(); + // absolute robot-relative target, in motor rotations + double targetRobotRelativeDeg = wrap180(fieldRelativeAngleDegrees - currentRobotHeading); + double targetMotorRotations = targetRobotRelativeDeg / 180.0; - this.turretMotor.getConfigurator().apply(config); - this.turretMotor.getConfigurator().apply(mm); + this.targetTurretAngleMotorRevs = targetMotorRotations; - CANcoder pinionEncoder = new CANcoder(CanID.TURRET_PINION_CANCODER.getID()); - CANcoder followerEncoder = new CANcoder(CanID.TURRET_FOLLOWER_CANCODER.getID()); + this.turretMotor.setControl( + mmRequest + .withPosition(targetMotorRotations) + .withFeedForward(calculateFeedforward()) + ); + } + + /** + * @inheritDoc + */ + @Override + public void recalculateTurretMotorZeroPosition() { + double currentTurretAngle = getTurretAngleRobotRelative(); // get the current angle of the turret in robot frame - this.pinionEncoderSignal = pinionEncoder.getAbsolutePosition(); - this.followerEncoderSignal = followerEncoder.getAbsolutePosition(); + double normalizedPosition = currentTurretAngle / 180.0; // convert to normalized position [-1, 1] + this.calculatedMotorOffsetRevs = normalizedPosition; // calculate the offset in motor rotations based on the current turret angle - this.encoderSignal = this.turretMotor.getPosition(); + this.turretMotor.setPosition(normalizedPosition); + } - double pinionEncoderValue = this.pinionEncoderSignal.getValueAsDouble(); - double followerEncoderValue = this.followerEncoderSignal.getValueAsDouble(); + /** + * Calculates the feedforward voltage to apply to the turret motor to counteract the rotation of the robot, based on the current angular velocity of the robot. + * @return the feedforward value to apply to the turret rotation + */ + private double calculateFeedforward() { + // get the current angular velocity of the robot in radians per second + double gyroOmegaRadPerSecond = drive.getState().Speeds.omegaRadiansPerSecond; - // set offset of the turret on startup - double turretRotations = TurretMath.getTurretAngleRevs(pinionEncoderValue, followerEncoderValue); - - double turretDegrees = TurretMath.normalizeTurretHeading( - TurretMath.toDegreesWrapped(turretRotations), - this.turretDegreesOffset - ); + double gyroOmegaDegPerSecond = gyroOmegaRadPerSecond * (180.0 / Math.PI); // convert to degrees per second - double currentMotorRevs = this.turretMotor.getPosition().getValueAsDouble(); - double toZeroRevs = TurretMath.degreesToMotorPosition(turretDegrees); + return feedforward.calculate(gyroOmegaDegPerSecond); + } - turretMotor.setPosition(-toZeroRevs); + @Override + public void refreshData() { + StatusSignal.refreshAll(this.turretMotorPosition, this.pinionEncoderSignal, this.followerEncoderSignal); } + @Override + public void updateInputs(TurretIOInputs inputs) { + System.out.println("Updating inputs: turret angle (field-relative) = " + getTurretAngleFieldRelative()); + inputs.turretAngleDegrees = getTurretAngleFieldRelative(); + inputs.targetTurretMotorRotations = this.targetTurretAngleMotorRevs; + inputs.normalizedTurretMotorRotations = this.calculatedMotorOffsetRevs; + } + + // CONFIGURATIONS + /** - * Set the turret angle to a target field angle - * @param fieldTargetHeadingDeg field relative angle to set turret to + * Get the motor configurations for the turret motor. + * @return the motor configurations for the turret motor */ - @Override - public void setTurretAngle(double fieldTargetHeadingDeg) { - Logger.recordOutput("Turret/TargetAngle", fieldTargetHeadingDeg); - this.targetAngleDeg = fieldTargetHeadingDeg; - fieldTargetHeadingDeg = MathUtil.inputModulus(fieldTargetHeadingDeg, 0.0, 360.0); - - // convert robot heading to [0, 360) range - double robotHeadingDeg = - MathUtil.inputModulus( - drive.getPose().getRotation().getDegrees(), - 0.0, 360.0 - ); - - // dt = time we expect turret to reach commanded angle, tunable - double dt = 0.025; - double predictedHeadingDeg = - robotHeadingDeg + this.drive.getRobotOmegaDegPerSec() * dt; - - // turret angle in (-180, 180] range, where positive is counterclockwise relative to the robot's forward direction, and negative is clockwise - double turretAngleDeg = - MathUtil.inputModulus( - fieldTargetHeadingDeg - predictedHeadingDeg, - -180.0, 180.0 - ); - - if (turretAngleDeg > TURRET_LIMIT_DEG) { - turretAngleDeg -= 360.0; - } else if (turretAngleDeg < -TURRET_LIMIT_DEG) { - turretAngleDeg += 360.0; - } + private Pair getTurretMotionConfigs() { + Slot0Configs configs = new Slot0Configs(); + + configs.kP = 1; + configs.kI = 0.0; + configs.kD = 0.0; + + configs.kS = kS; + configs.kV = kV; + + MotionMagicConfigs mmConfigs = new MotionMagicConfigs(); + + mmConfigs.MotionMagicAcceleration = 20; // rotations per second^2 + mmConfigs.MotionMagicCruiseVelocity = 10; // rotations per second + + return Pair.create(configs, mmConfigs); + } + + /** + * Get the feedback configurations for the turret motor, which define the relationship between the motor rotations, the pinion encoder rotations, and the follower encoder rotations. + * @return the feedback configurations for the turret motor + */ + private FeedbackConfigs getTurretMotorFeedbackConfigs() { + FeedbackConfigs configs = new FeedbackConfigs(); + + // SensorToMechanismRatio = GR/2 means the motor completes 2 mechanism rotations per + // full turret revolution, so 1 mechanism rotation = 180 degrees of turret travel. + // Soft limits at +-1 mechanism rotation enforce the +-180 degrees physical range of the turret. + configs.SensorToMechanismRatio = Turret.TURRET_GEAR_RATIO / 2.0; + configs.RotorToSensorRatio = 1; + + return configs; + } + + /** + * Get the software limit switch configurations for the turret motor, which define the forward and reverse limits of the turret based on the motor position. + * @return the software limit switch configurations for the turret motor + */ + private SoftwareLimitSwitchConfigs getTurretSoftwareLimitConfigs() { + SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs(); + + configs.ForwardSoftLimitEnable = true; + configs.ForwardSoftLimitThreshold = 1; // 1 rotation of the motor past the zero point + + configs.ReverseSoftLimitEnable = true; + configs.ReverseSoftLimitThreshold = -1; // 1 rotation of the motor in the opposite direction past the zero point - // convert turret angle to motor rotations, accounting for gear ratio and offset - double turretAngleForMotor = turretAngleDeg; - if (turretAngleForMotor > 180.0) { - turretAngleForMotor -= 360.0; + return configs; + } + + /** + * Get the encoder configurations for the turret encoders. + * @return the encoder configurations for the turret encoders + */ + public CANcoderConfiguration getEncoderConfigs() { + CANcoderConfiguration configs = new CANcoderConfiguration(); + // constrain reading between [0, 1) + configs.MagnetSensor.withAbsoluteSensorDiscontinuityPoint(1.0); + return configs; + } + + public CANcoderConfiguration getPinionEncoderConfigs() { + CANcoderConfiguration configs = getEncoderConfigs(); + configs.MagnetSensor.MagnetOffset = Turret.FOLLOWER_ENCODER_OFFSET; + + return configs; + } + + public CANcoderConfiguration getFollowerEncoderConfigs() { + CANcoderConfiguration configs = getEncoderConfigs(); + configs.MagnetSensor.MagnetOffset = Turret.PINION_ENCODER_OFFSET; + + return configs; + } + + // CRT METHODS + + /** + * Calculates the continuous position of the pinion (driving) encoder in revolutions + * @return the continuous position of the pinion encoder in revolutions + */ + private double getPinionEncoderRevs() { + double pinionEncoderReading = positiveMod(this.pinionEncoderSignal.getValueAsDouble(), Turret.NORMALIZED_REVOLUTION); + double followerEncoderReading = positiveMod(this.followerEncoderSignal.getValueAsDouble(), Turret.NORMALIZED_REVOLUTION); + + double bestError = Double.MAX_VALUE; + double bestPosition = 0.0; + + int searchCount = (int) Turret.FOLLOWER_ENCODER_TEETH; // number of distinct branches to check (follower encoder teeth) + for (int k = 0; k < searchCount; k++) { + double assumedPinionRevs = pinionEncoderReading + k; + double predictedFollowerReading = positiveMod(assumedPinionRevs * (Turret.PINION_ENCODER_TEETH / Turret.FOLLOWER_ENCODER_TEETH), Turret.NORMALIZED_REVOLUTION); + double predictionError = Math.abs(predictedFollowerReading - followerEncoderReading); + + // check wrap-around error + if (predictionError > Turret.NORMALIZED_REVOLUTION / 2.0) { + predictionError = Turret.NORMALIZED_REVOLUTION - predictionError; + } + + // if this branch has a better prediction error than the best one so far, update the best guess for the pinion encoder position + if (predictionError < bestError) { + bestError = predictionError; + bestPosition = assumedPinionRevs; + } } - Logger.recordOutput("Turret/TargetAngleForMotor", turretAngleForMotor); - double rotations = -(turretAngleForMotor / 360.0) * kTurretGearRatio; - Logger.recordOutput("Turret/TargetRotations", rotations); - // set the motor to the desired position with feedforward to counteract robot rotation - this.turretMotor.setControl( - this.mmVoltage - .withPosition(rotations) - // 0.1167 is an empirically determined gain to convert from motor RPS to voltage needed to hold position against rotation - // .withFeedForward(motorRps * 0.1167) - ); + return bestPosition; } - - + /** - * Periodically called to update the Turret information for logging - * @param inputs TurretIOInputs object to update + * Calculates the continuous position of the turret in revolutions of the entire mechanism. + * @return the continuous position of the turret in revolutions. */ - @Override - public void updateInputs(TurretIOInputs inputs) { - inputs.pinionEncoder = this.pinionEncoderSignal.getValueAsDouble(); - inputs.followerEncoder = this.followerEncoderSignal.getValueAsDouble(); - inputs.turretSetPointDegrees = this.targetAngleDeg; + private double getTurretPositionRevs() { + double rawPinionRevs = getPinionEncoderRevs(); + double rawTurretRevs = rawPinionRevs * (Turret.PINION_ENCODER_TEETH / Turret.TURRET_GEAR_TEETH); // convert pinion revolutions to turret revolutions - double turretRotations = TurretMath.getTurretAngleRevs(inputs.pinionEncoder, inputs.followerEncoder); + double wrapped = positiveMod(rawTurretRevs, Turret.ENCODER_COMBINED_PERIOD_REV); - inputs.robotOmegaDegPerSec = this.drive.getRobotOmegaDegPerSec(); + if (!isInitialized) { + this.lastPositionRevs = wrapped; + isInitialized = true; + return wrapped; + } - // convert raw encoder readings to turret angle in degrees, accounting for gear ratio and offset - double turretAngleDegreesNonNormalized = TurretMath.normalizeTurretHeading( - TurretMath.toDegreesWrapped(turretRotations), - turretDegreesOffset - ); + double delta = wrapped - this.lastPositionRevs; - // normalize to -180 to 180 range - if (turretAngleDegreesNonNormalized > 180.0) { - inputs.turretAngleDegrees = turretAngleDegreesNonNormalized - 360.0; - } else if (turretAngleDegreesNonNormalized < -180.0) { - inputs.turretAngleDegrees = turretAngleDegreesNonNormalized + 360.0; - } else { - inputs.turretAngleDegrees = turretAngleDegreesNonNormalized; + // if the change in position is greater than half the combined period, we have wrapped around the encoder, so we need to adjust the delta accordingly + if (delta > Turret.ENCODER_COMBINED_PERIOD_REV / 2.0) { + delta -= Turret.ENCODER_COMBINED_PERIOD_REV; + } else if (delta < -Turret.ENCODER_COMBINED_PERIOD_REV / 2.0) { + delta += Turret.ENCODER_COMBINED_PERIOD_REV; } - // creates a position for the turret based on robot position, rotated by turret angle for logging - inputs.turretPosition = new Pose2d(drive.getPose().getTranslation(), new Rotation2d(TurretMath.toRad(inputs.turretAngleDegrees))); + lastPositionRevs += delta; + return lastPositionRevs; } /** - * Periodically refreshes encoder signal - * @note This is called automatically + * Converts rotations of the turret mechanism to degrees (heading) of the turret. + * @param turretRevs continuous revolutions of the turret + * @return degrees of the turret from revolutions, continuous and unwrapped */ - @Override - public void refreshData() { - BaseStatusSignal.refreshAll(encoderSignal, pinionEncoderSignal, followerEncoderSignal); + private double revsToDegreesContinuous(double turretRevs) { + return turretRevs * Turret.DEGREES_PER_REV; } - public static Slot0Configs getTurretMotorConfig() { - Slot0Configs config = new Slot0Configs(); - - // config.kP = 1; - // config.kI = 0.0; - // config.kD = 0.0; + /** + * Gets the angle of the turret in robot space wrapped from [-180, 180) + * @return the angle of the turret in robot space, wrapped + */ + private double getTurretAngleRobotRelative() { + double continuousRevs = getTurretPositionRevs(); + double turretAngleDegrees = revsToDegreesContinuous(continuousRevs); - // config.kS = 0.25; - // config.kV = 0.20; + // apply offset and wrap to [-180, 180) + return wrap180(turretAngleDegrees); + } - config.kP = 0.0; - config.kI = 0.0; - config.kD = 0.0; + /** + * Gets the angle of the turret in field space, wrapped from [-180, 180) + * @return the angle of the turret in field space, wrapped + */ + private double getTurretAngleFieldRelative() { + double robotRelativeAngle = getTurretAngleRobotRelative(); + double currentRobotHeading = this.drive.getPose().getRotation().getDegrees(); + + double fieldCentricContinuous = robotRelativeAngle - currentRobotHeading; - config.kS = 0.0; - config.kV = 0.0; + return wrap180(fieldCentricContinuous); + } - return config; + // UTILITY METHODS + + /** + * Wraps the input angle to be within the range [min, max). + * @param input the angle to wrap + * @param min the minimum angle of the range (inclusive) + * @param max the maximum angle of the range (exclusive) + * @return the wrapped angle within the range [min, max) + */ + private double wrap(double input, double min, double max) { + // input modulo the range size + return MathUtil.inputModulus( + input, + min, + max + ); + } + + /** + * Wraps the input angle to be within the range [-180, 180). + * @param input the angle to wrap + * @return the wrapped angle within the range [-180, 180) + */ + private double wrap180(double input) { + return wrap(input, -180.0, 180.0); + } + + /** + * A positive modulus function that wraps x into the range [0, m). + * @param x the value to wrap + * @param m the modulus + * @return the wrapped value in the range [0, m) + */ + private double positiveMod(double x, double m) { + return ((x % m) + m) % m; } } diff --git a/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java b/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java deleted file mode 100644 index 7887d70..0000000 --- a/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java +++ /dev/null @@ -1,192 +0,0 @@ -package frc.robot.subsystems.turret.calc; - -/** - * Utility class for computing absolute turret position using two geared absolute encoders - * with a CRT-like search approach. - * - * Hardware setup (example): - * - Turret gear: 140 teeth - * - Encoder A: e.g. 13-tooth gear driving an absolute encoder (reads 0-1 rev) - * - Encoder B: e.g. 11-tooth gear driving an absolute encoder (reads 0-1 rev) - * - * The combination gives unique positions over (encoderA_teeth x encoderB_teeth) teeth. - * Beyond that range, continuity tracking (unwrapping) is used. - */ -public class TurretMath { - - // Hardware tooth counts - private static final double TURRET_GEAR_TEETH = 84.0; - private static final double ENCODER_A_TEETH = 10.0; - private static final double ENCODER_B_TEETH = 13.0; - - // Derived encoder combination values - private static final double ENCODER_COMBINED_TEETH = ENCODER_A_TEETH * ENCODER_B_TEETH; - private static final double ENCODER_COMBINED_PERIOD_REV = ENCODER_COMBINED_TEETH / TURRET_GEAR_TEETH; - private static final double HALF_ENCODER_COMBINED_PERIOD_REV = ENCODER_COMBINED_PERIOD_REV / 2.0; - - private static final double NORMALIZED_REV = 1.0; // encoder reading range (0..1) - private static final double HALF_NORMALIZED_REV = NORMALIZED_REV / 2.0; - private static final double DEGREES_PER_REV = 360.0; - private static final double RAD_PER_REV = 2.0 * Math.PI; - private static final double MOTOR_UNITS_PER_REV = TURRET_GEAR_TEETH/ENCODER_A_TEETH; // scale used in degreesToMotorPosition - - // Defaults / initial values - private static final double DEFAULT_OFFSET_DEGREES = 0.0; - private static final double DEFAULT_LAST_POSITION_REVS = 0.0; - - // State - private static double offsetDegrees = DEFAULT_OFFSET_DEGREES; // Calibration offset (degrees) - private static double lastPositionRevs = DEFAULT_LAST_POSITION_REVS; // Unwrapped continuous position - private static boolean initialized = false; - - /** - * Helper method to perform modulus that always returns a positive result, wrapping x into [0, m). - * @param x value to wrap - * @param m modulus (e.g. 1.0 for normalized encoder readings) - * @return wrapped value in [0, m) - */ - private static double mod(double x, double m) { - return ((x % m) + m) % m; - } - - /** - * Computes the raw absolute turret position in revolutions of the encoder-A gear - * (i.e. position in "encoder-A-gear-equivalent revolutions"). - * Range is approximately 0 to (combined_teeth / encoderA_teeth) revolutions of the encoder-A gear. - * - * This uses a search-based approach across the possible integer wraps of the encoder-A reading - * to find the branch that best matches the encoder-B reading. - * - * @param encoderAReading Absolute encoder A reading (normalized [0, 1)) - * @param encoderBReading Absolute encoder B reading (normalized [0, 1)) - * @return Raw turret position in encoder-A-gear revolutions - */ - public static double getRawPositionPrimaryRevs(double encoderAReading, double encoderBReading) { - double offsetRevs = offsetDegrees / DEGREES_PER_REV; - - // Apply offset and wrap to [0,1) - encoderAReading = mod(encoderAReading - offsetRevs, NORMALIZED_REV); - encoderBReading = mod(encoderBReading - offsetRevs, NORMALIZED_REV); - - double bestError = Double.POSITIVE_INFINITY; - double bestPosition = 0.0; - - // Search over the possible integer wraps of the secondary/primary relationship - int searchCount = (int) ENCODER_B_TEETH; // number of distinct branches to check (encoder B teeth) - for (int k = 0; k < searchCount; k++) { - double assumedPrimaryRevs = encoderAReading + k; - // Predict what the encoder-B reading should be if this is the correct branch - double predictedEncoderB = mod(assumedPrimaryRevs * (ENCODER_A_TEETH / ENCODER_B_TEETH), NORMALIZED_REV); - - double error = Math.abs(predictedEncoderB - encoderBReading); - // Handle wrap-around distance - if (error > HALF_NORMALIZED_REV) { - error = NORMALIZED_REV - error; - } - - if (error < bestError) { - bestError = error; - bestPosition = assumedPrimaryRevs; - } - } - - return bestPosition; - } - - /** - * Returns the turret angle in revolutions, unwrapped for continuous multi-turn motion. - * Applies offset and continuity tracking. - * - * @param encoderAReading Absolute encoder A reading [0,1) - * @param encoderBReading Absolute encoder B reading [0,1) - * @return Continuous turret position in revolutions (can be >1 or <0) - */ - public static double getTurretAngleRevs(double encoderAReading, double encoderBReading) { - double rawPrimaryRevs = getRawPositionPrimaryRevs(encoderAReading, encoderBReading); - double rawTurretRevs = rawPrimaryRevs * (ENCODER_A_TEETH / TURRET_GEAR_TEETH); - - // Apply offset again (in turret space) - rawTurretRevs -= offsetDegrees / DEGREES_PER_REV; - - // Wrap raw reading into one encoded period for comparison - double wrapped = mod(rawTurretRevs, ENCODER_COMBINED_PERIOD_REV); - - if (!initialized) { - lastPositionRevs = wrapped; - initialized = true; - return wrapped; - } - - // Compute shortest path delta (assuming small motion between calls) - double delta = wrapped - lastPositionRevs; - - // Unwrap using the known encoder combined period - if (delta > HALF_ENCODER_COMBINED_PERIOD_REV) { - delta -= ENCODER_COMBINED_PERIOD_REV; - } else if (delta < -HALF_ENCODER_COMBINED_PERIOD_REV) { - delta += ENCODER_COMBINED_PERIOD_REV; - } - - lastPositionRevs += delta; - return lastPositionRevs; - } - - /** - * Convert turret revolutions to degrees, wrapped to [0, 360). - * Use this when you only care about single-turn angle. - */ - public static double toDegreesWrapped(double turretRevs) { - return mod(turretRevs, NORMALIZED_REV) * DEGREES_PER_REV; - } - - /** - * Convert turret revolutions to radians, wrapped to [0, 2pi). - */ - public static double toRadiansWrapped(double turretRevs) { - return mod(turretRevs, NORMALIZED_REV) * RAD_PER_REV; - } - - public static double degreesToMotorPosition(double turretDegrees) { - if (turretDegrees > 180.0) { - turretDegrees -= 360.0; - } - return (turretDegrees * MOTOR_UNITS_PER_REV) / DEGREES_PER_REV; - } - - public static double normalizeTurretHeading(double turretHeading, double zeroDegrees) { - double newHeading = turretHeading - zeroDegrees; - - if (newHeading < 0) { - newHeading = DEGREES_PER_REV - Math.abs(newHeading); - } - - return newHeading; - } - - /** - * Convert turret revolutions to total accumulated degrees (can be >360 or <0). - * Use this when you want continuous angle for PID or motion profiling. - */ - public static double toDegreesContinuous(double turretRevs) { - return turretRevs * DEGREES_PER_REV; - } - - // Calibration / zeroing - public static void setOffsetDegrees(double degrees) { - offsetDegrees = degrees; - } - - public static double getOffsetDegrees() { - return offsetDegrees; - } - - // Reset continuity tracker - public static void resetContinuity() { - initialized = false; - lastPositionRevs = DEFAULT_LAST_POSITION_REVS; - } - - public static double toRad(double angle) { - return angle * (Math.PI / DEGREES_PER_REV); - } -} From 75d6b11a7a461ebd87b0f52362cb5101fe41dae4 Mon Sep 17 00:00:00 2001 From: nathan Date: Sun, 8 Mar 2026 20:47:44 -0400 Subject: [PATCH 07/29] Switch graalvm to wpilib Co-authored-by: PillageDev --- .../java/frc/robot/subsystems/turret/TurretIOTalonFX.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 3a14c05..28a9903 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -6,11 +6,11 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.units.measure.Angle; import frc.robot.CanID; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; -import org.graalvm.collections.Pair; public class TurretIOTalonFX implements TurretIO { // KS KV CONSTANTS @@ -58,8 +58,8 @@ public TurretIOTalonFX(CommandSwerveDrivetrain drive) { var turretMotorFeedbackConfig = getTurretMotorFeedbackConfigs(); var turretSoftwareLimitConfig = getTurretSoftwareLimitConfigs(); - this.turretMotor.getConfigurator().apply(turretMotorConfig.getLeft()); - this.turretMotor.getConfigurator().apply(turretMotorConfig.getRight()); + this.turretMotor.getConfigurator().apply(turretMotorConfig.getFirst()); + this.turretMotor.getConfigurator().apply(turretMotorConfig.getSecond()); this.turretMotor.getConfigurator().apply(turretMotorFeedbackConfig); this.turretMotor.getConfigurator().apply(turretSoftwareLimitConfig); @@ -152,7 +152,7 @@ private Pair getTurretMotionConfigs() { mmConfigs.MotionMagicAcceleration = 20; // rotations per second^2 mmConfigs.MotionMagicCruiseVelocity = 10; // rotations per second - return Pair.create(configs, mmConfigs); + return new Pair<>(configs, mmConfigs); } /** From b9aff84935cb26d287418d9e2d9b3246fce50baf Mon Sep 17 00:00:00 2001 From: Justin E Date: Mon, 9 Mar 2026 08:27:05 -0400 Subject: [PATCH 08/29] Fix auto logging and logged tracer outputs --- src/main/java/frc/lib/LoggedTracer.java | 4 +-- .../java/frc/lib/subsystem/SpikeSystem.java | 18 +++++++++++-- src/main/java/frc/robot/Robot.java | 25 +++++++++---------- .../robot/subsystems/findexer/Findexer.java | 2 +- .../frc/robot/subsystems/intake/Intake.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../frc/robot/subsystems/trigger/Trigger.java | 2 +- .../frc/robot/subsystems/turret/Turret.java | 1 + .../subsystems/turret/TurretIOTalonFX.java | 1 - .../frc/robot/subsystems/vision/Vision.java | 2 +- 10 files changed, 36 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/lib/LoggedTracer.java b/src/main/java/frc/lib/LoggedTracer.java index f0b20d4..8754954 100644 --- a/src/main/java/frc/lib/LoggedTracer.java +++ b/src/main/java/frc/lib/LoggedTracer.java @@ -9,7 +9,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.littletonrobotics.junction.Logger; /** Utility class for logging code execution times. */ public class LoggedTracer { @@ -25,7 +25,7 @@ public static void reset() { /** Save the time elapsed since the last reset or record. */ public static void record(String epochName) { double now = Timer.getFPGATimestamp(); - SmartDashboard.putNumber( + Logger.recordOutput( "Logged Tracer/" + epochName + " Milliseconds", Units.secondsToMilliseconds(now - startTime)); startTime = now; } diff --git a/src/main/java/frc/lib/subsystem/SpikeSystem.java b/src/main/java/frc/lib/subsystem/SpikeSystem.java index fc456d4..c921977 100644 --- a/src/main/java/frc/lib/subsystem/SpikeSystem.java +++ b/src/main/java/frc/lib/subsystem/SpikeSystem.java @@ -3,6 +3,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.DataUtils; import frc.lib.LoggedTracer; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; public abstract class SpikeSystem extends SubsystemBase { private final Runnable dataRefreshTask; @@ -20,19 +22,31 @@ protected & IORefresher> Runnable useAsyncDataRefresher( T baseIO ) { DataUtils.createDataLogger(io, baseIO); - return () -> {}; + return () -> { + if (io instanceof LoggableInputs loggable) { + synchronized (io) { + Logger.processInputs(getName(), loggable); + } + } + }; } protected > Runnable useDataRefresher( T baseIO ) { - return () -> baseIO.updateInputs(io); + return () -> { + baseIO.updateInputs(io); + if (io instanceof LoggableInputs loggable) { + Logger.processInputs(getName(), loggable); + } + }; } public void onPeriodic() {} @Override public final void periodic() { + LoggedTracer.reset(); dataRefreshTask.run(); onPeriodic(); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a916faa..21b3fb2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -36,9 +36,6 @@ * project. */ public class Robot extends LoggedRobot { - private static final double LOOP_PERIOD_S = 0.02; // 20 ms loop period - private static double tickStart = 0; // start time of current tick, updated for each tick, in seconds - private Command autonomousCommand; private RobotContainer robotContainer; @@ -49,6 +46,9 @@ public class Robot extends LoggedRobot { ? 100000000 : // 100 MB 1000000000; // 1 GB + + private double lastPeriodicTime = Timer.getFPGATimestamp(); + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -154,13 +154,21 @@ void SetupLog() { /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { - tickStart = Timer.getFPGATimestamp(); + double startTime = Timer.getFPGATimestamp(); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled commands, running already-scheduled commands, removing // finished or interrupted commands, and running subsystem periodic() methods. // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); + double endTime = Timer.getFPGATimestamp(); + + double execMs = (endTime - startTime) * 1000.0; + double periodMs = (startTime - lastPeriodicTime) * 1000.0; + lastPeriodicTime = startTime; + + Logger.recordOutput("Logged Tracer/RobotPeriodic Execution Milliseconds", execMs); + Logger.recordOutput("Logged Tracer/RobotPeriodic Period Milliseconds", periodMs); } /** This function is called once when the robot is disabled. */ @@ -220,13 +228,4 @@ public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() {} - - /** - * Utility function to check if a given timestamp is within the current tick. - * @param timestamp timestamp to check, in seconds - * @return true if the timestamp is within the current tick, false otherwise - */ - public static boolean isTimestampInCurrentTick(double timestamp) { - return timestamp >= tickStart && timestamp < tickStart + LOOP_PERIOD_S; - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/findexer/Findexer.java b/src/main/java/frc/robot/subsystems/findexer/Findexer.java index 87404eb..8f8cbfa 100644 --- a/src/main/java/frc/robot/subsystems/findexer/Findexer.java +++ b/src/main/java/frc/robot/subsystems/findexer/Findexer.java @@ -11,7 +11,7 @@ public class Findexer extends SpikeSystem { private FindexerIO findexerIO; public Findexer(Trigger trigger) { - super("Findexer", new FindexerIO.FindexerIOInputs()); + super("Findexer", new FindexerIOInputsAutoLogged()); this.trigger = trigger; } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 77b800a..3bd4cb1 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -24,7 +24,7 @@ public enum IntakeState { // Intake constructor public Intake(CommandSwerveDrivetrain drivetrain) { - super("Intake", new IntakeIO.IntakeIOInputs()); + super("Intake", new IntakeIOInputsAutoLogged()); this.drivetrain = drivetrain; enable(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ddf79eb..fb92821 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -12,7 +12,7 @@ public class Shooter extends SpikeSystem { private boolean driverRequestingShooting = false; // Whether the driver is currently requesting to shoot public Shooter() { - super("Shooter", new ShooterIO.ShooterIOInputs()); + super("Shooter", new ShooterIOInputsAutoLogged()); } /** diff --git a/src/main/java/frc/robot/subsystems/trigger/Trigger.java b/src/main/java/frc/robot/subsystems/trigger/Trigger.java index 37bf23b..11eee77 100644 --- a/src/main/java/frc/robot/subsystems/trigger/Trigger.java +++ b/src/main/java/frc/robot/subsystems/trigger/Trigger.java @@ -15,7 +15,7 @@ public class Trigger extends SpikeSystem { private TriggerIO triggerIO; public Trigger(Shooter shooter, Turret turret) { - super("Trigger", new TriggerIO.TriggerIOInputs()); + super("Trigger", new TriggerIOInputsAutoLogged()); this.shooter = shooter; this.turret = turret; diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index dc31519..d31adc4 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -4,6 +4,7 @@ import frc.robot.RobotContainer; import frc.robot.subsystems.targeting.Targeting; import frc.robot.subsystems.targeting.ShotCompensation; +import org.littletonrobotics.junction.Logger; public class Turret extends SpikeSystem { public static final double TURRET_AIMING_TOLERANCE_DEGREES = 2.0; // degrees within which we consider the turret to be aimed at the target (+-) diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 28a9903..8dfa898 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -125,7 +125,6 @@ public void refreshData() { @Override public void updateInputs(TurretIOInputs inputs) { - System.out.println("Updating inputs: turret angle (field-relative) = " + getTurretAngleFieldRelative()); inputs.turretAngleDegrees = getTurretAngleFieldRelative(); inputs.targetTurretMotorRotations = this.targetTurretAngleMotorRevs; inputs.normalizedTurretMotorRotations = this.calculatedMotorOffsetRevs; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index d5414b5..2c7d93a 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -15,7 +15,7 @@ public class Vision extends SpikeSystem { private final CommandSwerveDrivetrain drive; public Vision(CommandSwerveDrivetrain drive) { - super("Vision", new VisionIO.VisionIOInputs()); + super("Vision", new VisionIOInputsAutoLogged()); this.drive = drive; } From 175e039a30da891578b8e03f3913393b3225300d Mon Sep 17 00:00:00 2001 From: Justin E Date: Mon, 9 Mar 2026 14:42:05 -0400 Subject: [PATCH 09/29] Add more logging inputs for turret --- src/main/java/frc/robot/subsystems/turret/TurretIO.java | 5 +++++ .../frc/robot/subsystems/turret/TurretIOTalonFX.java | 9 +++++++++ 2 files changed, 14 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index 49ac69a..a8b15d9 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -10,8 +10,13 @@ public interface TurretIO extends IORefresher, BaseIO { @AutoLog public static class TurretIOInputs extends BaseInputClass { public double turretAngleDegrees = 0.0; // current angle of the turret, field-relative, in degrees + public double targetTurretDegrees = 0.0; // target angle for the turret, field-relative, in degrees + public double processedTargetTurretDegrees = 0.0; // processed target angle for the turret, field-relative, in degrees, updated when recalculation is called public double targetTurretMotorRotations = 0.0; // target position for the turret motor, in rotations public double normalizedTurretMotorRotations = 0.0; // calculated turret motor rotations, updated when recalculation is called + public double turretMotorPositionRotations = 0.0; // current position of the turret motor, in rotations + public double pinionEncoderRotations = 0.0; // current rotations of the pinion encoder + public double followerEncoderRotations = 0.0; // current rotations of the follower } /** diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 8dfa898..36dfee4 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -35,6 +35,8 @@ public class TurretIOTalonFX implements TurretIO { private double lastPositionRevs = 0.0; // last calculated position of the turret in revolutions // VALUES + private double targetTurretDegreesFieldRelative; // target angle of the turret in degrees, relative to the field + private double processedTargetTurretDegreesFieldRelative; // processed target angle of the turret in degrees, relative to the field private double targetTurretAngleMotorRevs; // target angle of the turret in motor rotations private double calculatedMotorOffsetRevs; // calculated offset in motor rotations based on the current position of the turret and the pinion encoder reading @@ -77,12 +79,14 @@ public TurretIOTalonFX(CommandSwerveDrivetrain drive) { @Override public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) { + this.targetTurretDegreesFieldRelative = fieldRelativeAngleDegrees; double currentRobotHeading = this.drive.getPose().getRotation().getDegrees(); // absolute robot-relative target, in motor rotations double targetRobotRelativeDeg = wrap180(fieldRelativeAngleDegrees - currentRobotHeading); double targetMotorRotations = targetRobotRelativeDeg / 180.0; + this.processedTargetTurretDegreesFieldRelative = targetRobotRelativeDeg; this.targetTurretAngleMotorRevs = targetMotorRotations; this.turretMotor.setControl( @@ -128,6 +132,11 @@ public void updateInputs(TurretIOInputs inputs) { inputs.turretAngleDegrees = getTurretAngleFieldRelative(); inputs.targetTurretMotorRotations = this.targetTurretAngleMotorRevs; inputs.normalizedTurretMotorRotations = this.calculatedMotorOffsetRevs; + inputs.targetTurretDegrees = this.targetTurretDegreesFieldRelative; + inputs.processedTargetTurretDegrees = this.processedTargetTurretDegreesFieldRelative; + inputs.turretMotorPositionRotations = this.turretMotorPosition.getValueAsDouble(); + inputs.pinionEncoderRotations = this.pinionEncoderSignal.getValueAsDouble(); + inputs.followerEncoderRotations = this.followerEncoderSignal.getValueAsDouble(); } // CONFIGURATIONS From 5c8f2ad9a226d70c54965c84e8b6954dcecda129 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Tue, 10 Mar 2026 18:18:10 -0400 Subject: [PATCH 10/29] More changes --- .../frc/robot/generated/TunerConstants.java | 48 +-- .../robot/generated/TunerConstantsMain.java | 285 ++++++++++++++++++ .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../robot/subsystems/targeting/ShotData.java | 2 +- .../frc/robot/subsystems/trigger/Trigger.java | 2 +- .../frc/robot/subsystems/turret/Turret.java | 18 +- .../frc/robot/subsystems/turret/TurretIO.java | 6 +- .../subsystems/turret/TurretIOTalonFX.java | 81 +++-- .../vision/photon/CameraManager.java | 59 ++-- 9 files changed, 417 insertions(+), 86 deletions(-) create mode 100644 src/main/java/frc/robot/generated/TunerConstantsMain.java diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 949bab5..1564bbe 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -23,7 +23,7 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) + .withKP(15).withKI(0).withKD(0.5) .withKS(0.1).withKV(1.59).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control @@ -86,7 +86,7 @@ public class TunerConstants { private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 62; + private static final int kPigeonId = 0; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -125,48 +125,48 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 41; - private static final int kFrontLeftSteerMotorId = 27; - private static final int kFrontLeftEncoderId = 25; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.32666015625); + private static final int kFrontLeftDriveMotorId = 0; + private static final int kFrontLeftSteerMotorId = 50; + private static final int kFrontLeftEncoderId = 31; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.463134765625); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(10.25); - private static final Distance kFrontLeftYPos = Inches.of(10.25); + private static final Distance kFrontLeftXPos = Inches.of(11.5); + private static final Distance kFrontLeftYPos = Inches.of(11.5); // Front Right - private static final int kFrontRightDriveMotorId = 42; - private static final int kFrontRightSteerMotorId = 7; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.48828125); + private static final int kFrontRightDriveMotorId = 61; + private static final int kFrontRightSteerMotorId = 27; + private static final int kFrontRightEncoderId = 25; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.306396484375); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(10.25); - private static final Distance kFrontRightYPos = Inches.of(-10.25); + private static final Distance kFrontRightXPos = Inches.of(11.5); + private static final Distance kFrontRightYPos = Inches.of(-11.5); // Back Left - private static final int kBackLeftDriveMotorId = 43; + private static final int kBackLeftDriveMotorId = 1; private static final int kBackLeftSteerMotorId = 30; private static final int kBackLeftEncoderId = 22; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.04833984375); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.20166015625); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-10.25); - private static final Distance kBackLeftYPos = Inches.of(10.25); + private static final Distance kBackLeftXPos = Inches.of(-11.5); + private static final Distance kBackLeftYPos = Inches.of(11.5); // Back Right - private static final int kBackRightDriveMotorId = 46; - private static final int kBackRightSteerMotorId = 50; - private static final int kBackRightEncoderId = 31; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.49365234375); + private static final int kBackRightDriveMotorId = 3; + private static final int kBackRightSteerMotorId = 24; + private static final int kBackRightEncoderId = 5; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.344482421875); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-10.25); - private static final Distance kBackRightYPos = Inches.of(-10.25); + private static final Distance kBackRightXPos = Inches.of(-11.5); + private static final Distance kBackRightYPos = Inches.of(-11.5); public static final SwerveModuleConstants FrontLeft = diff --git a/src/main/java/frc/robot/generated/TunerConstantsMain.java b/src/main/java/frc/robot/generated/TunerConstantsMain.java new file mode 100644 index 0000000..dd5da93 --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstantsMain.java @@ -0,0 +1,285 @@ +package frc.robot.generated; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; +import frc.robot.subsystems.drive.CommandSwerveDrivetrain; + +// Generated by the 2026 Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstantsMain { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.59).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("Canivore_Drivetrain", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(3.79); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.5714285714285716; + + private static final double kDriveGearRatio = 8.142857142857142; + private static final double kSteerGearRatio = 12.8; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 62; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 41; + private static final int kFrontLeftSteerMotorId = 27; + private static final int kFrontLeftEncoderId = 25; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.32666015625); + private static final boolean kFrontLeftSteerMotorInverted = false; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10.25); + private static final Distance kFrontLeftYPos = Inches.of(10.25); + + // Front Right + private static final int kFrontRightDriveMotorId = 42; + private static final int kFrontRightSteerMotorId = 7; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.48828125); + private static final boolean kFrontRightSteerMotorInverted = false; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10.25); + private static final Distance kFrontRightYPos = Inches.of(-10.25); + + // Back Left + private static final int kBackLeftDriveMotorId = 43; + private static final int kBackLeftSteerMotorId = 30; + private static final int kBackLeftEncoderId = 22; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.04833984375); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10.25); + private static final Distance kBackLeftYPos = Inches.of(10.25); + + // Back Right + private static final int kBackRightDriveMotorId = 46; + private static final int kBackRightSteerMotorId = 50; + private static final int kBackRightEncoderId = 31; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.49365234375); + private static final boolean kBackRightSteerMotorInverted = false; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10.25); + private static final Distance kBackRightYPos = Inches.of(-10.25); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); + } + + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fb92821..148e5cd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -27,7 +27,7 @@ public void onPeriodic() { this.targetRPS = newTargetRPS; shooterIO.setHoodAngle(shotData.hoodAngleDeg()); - shooterIO.setFlywheelVelocity(newTargetRPS); + // shooterIO.setFlywheelVelocity(newTargetRPS); } } diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java index 4730a7c..6857a9b 100644 --- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -9,7 +9,7 @@ public class ShotData { static { // TODO: get data and fill these in // distanceToRPM.put(20.0, 3000.0); - distanceToRPM.put(10.0, 2500.0); + distanceToRPM.put(10.0, 3500.0); distanceToHoodAngle.put(20.0, 15.0); } } diff --git a/src/main/java/frc/robot/subsystems/trigger/Trigger.java b/src/main/java/frc/robot/subsystems/trigger/Trigger.java index 11eee77..b58e163 100644 --- a/src/main/java/frc/robot/subsystems/trigger/Trigger.java +++ b/src/main/java/frc/robot/subsystems/trigger/Trigger.java @@ -7,7 +7,7 @@ public class Trigger extends SpikeSystem { private static final int PROXIMITY_SENSOR_CHANNEL = 2; // DIO channel for the proximity sensor - private final static double TRIGGER_SPEED = 10.0; // Rotations per second + private final static double TRIGGER_SPEED = 0; // Rotations per second private final Shooter shooter; private final Turret turret; diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index d31adc4..40d6b1a 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -21,9 +21,14 @@ public class Turret extends SpikeSystem { public static final double ENCODER_COMBINED_TEETH = PINION_ENCODER_TEETH * FOLLOWER_ENCODER_TEETH; public static final double ENCODER_COMBINED_PERIOD_REV = ENCODER_COMBINED_TEETH / PINION_ENCODER_TEETH; + public static final double ENCODER_COMBINED_PERIOD_TURRET_REV = + ENCODER_COMBINED_PERIOD_REV * (PINION_ENCODER_TEETH / TURRET_GEAR_TEETH); + + public static final double PINION_ENCODER_OFFSET = 0; // offset for pinion encoder, to be determined by calibration + public static final double FOLLOWER_ENCODER_OFFSET = 0; // offset for follower encoder, to be determined by calibration + + public static final double TURRET_FORWARD_OFFSET_DEG = 83.7; - public static final double PINION_ENCODER_OFFSET = 0.0; // offset for pinion encoder in degrees, to be determined by calibration - public static final double FOLLOWER_ENCODER_OFFSET = 0.0; // offset for follower encoder in degrees, to be determined by calibration private TurretIO turretIO; @@ -34,11 +39,12 @@ public Turret() { @Override public void onPeriodic() { ShotCompensation.AdjustedShot shotData = Targeting.getShotData(); + turretIO.setTurretAngleFieldRelativeDegrees(0); if (shotData != null) { double newTargetAngleDeg = shotData.turretAngleDeg(); - this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg); + // this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg); } } @@ -53,9 +59,9 @@ protected Runnable setupDataRefresher() { * @return true if the turret is at the target angle, false otherwise */ public boolean isAtTargetAngle() { - double minAngle = io.turretAngleDegrees - TURRET_AIMING_TOLERANCE_DEGREES; - double maxAngle = io.turretAngleDegrees + TURRET_AIMING_TOLERANCE_DEGREES; + double error = + Math.abs(io.turretAngleDegreesFieldRelative - io.targetTurretDegrees); - return io.targetTurretMotorRotations >= minAngle && io.targetTurretMotorRotations <= maxAngle; + return error <= TURRET_AIMING_TOLERANCE_DEGREES; } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index a8b15d9..ed37bd9 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -9,14 +9,16 @@ public interface TurretIO extends IORefresher, BaseIO { @AutoLog public static class TurretIOInputs extends BaseInputClass { - public double turretAngleDegrees = 0.0; // current angle of the turret, field-relative, in degrees + public double turretAngleDegreesFieldRelative = 0.0; // current angle of the turret, field-relative, in degrees + public double turretAngleDegreesRobotRelative = 0.0; // current angle of the turret, robot-relative, in degrees public double targetTurretDegrees = 0.0; // target angle for the turret, field-relative, in degrees public double processedTargetTurretDegrees = 0.0; // processed target angle for the turret, field-relative, in degrees, updated when recalculation is called public double targetTurretMotorRotations = 0.0; // target position for the turret motor, in rotations - public double normalizedTurretMotorRotations = 0.0; // calculated turret motor rotations, updated when recalculation is called + public double turretOffsetRotations = 0.0; // calculated turret motor rotations, updated when recalculation is called public double turretMotorPositionRotations = 0.0; // current position of the turret motor, in rotations public double pinionEncoderRotations = 0.0; // current rotations of the pinion encoder public double followerEncoderRotations = 0.0; // current rotations of the follower + public double rawTurretMechanismRotations = 0.0; // raw rotations of the entire turret mechanism } /** diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 36dfee4..340a18c 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.turret; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.controls.MotionMagicVoltage; @@ -33,6 +34,7 @@ public class TurretIOTalonFX implements TurretIO { // CALIBRATION STATES (CRT) private boolean isInitialized = false; // whether the turret has been initialized with a known position yet private double lastPositionRevs = 0.0; // last calculated position of the turret in revolutions + private double lastPinionRevs = 0.0; // last calculated position of the pinion encoder in revolutions // VALUES private double targetTurretDegreesFieldRelative; // target angle of the turret in degrees, relative to the field @@ -73,6 +75,12 @@ public TurretIOTalonFX(CommandSwerveDrivetrain drive) { this.pinionEncoderSignal = this.pinionEncoder.getAbsolutePosition(); this.followerEncoderSignal = this.followerEncoder.getAbsolutePosition(); + BaseStatusSignal.refreshAll( + this.turretMotorPosition, + this.pinionEncoderSignal, + this.followerEncoderSignal + ); + // ZEROING POSITION recalculateTurretMotorZeroPosition(); } @@ -92,7 +100,7 @@ public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) this.turretMotor.setControl( mmRequest .withPosition(targetMotorRotations) - .withFeedForward(calculateFeedforward()) + // .withFeedForward(calculateFeedforward()) ); } @@ -101,12 +109,16 @@ public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) */ @Override public void recalculateTurretMotorZeroPosition() { - double currentTurretAngle = getTurretAngleRobotRelative(); // get the current angle of the turret in robot frame - double normalizedPosition = currentTurretAngle / 180.0; // convert to normalized position [-1, 1] - this.calculatedMotorOffsetRevs = normalizedPosition; // calculate the offset in motor rotations based on the current turret angle + // absolute turret position from CRT + double turretRevs = getTurretPositionRevs(); + + // mechanism rotations where 1 rotation = 180 degrees + double mechanismRotations = turretRevs * 2.0; + + this.calculatedMotorOffsetRevs = mechanismRotations; - this.turretMotor.setPosition(normalizedPosition); + turretMotor.setPosition(mechanismRotations); } /** @@ -117,9 +129,8 @@ private double calculateFeedforward() { // get the current angular velocity of the robot in radians per second double gyroOmegaRadPerSecond = drive.getState().Speeds.omegaRadiansPerSecond; - double gyroOmegaDegPerSecond = gyroOmegaRadPerSecond * (180.0 / Math.PI); // convert to degrees per second - - return feedforward.calculate(gyroOmegaDegPerSecond); + double mechanismRotationsPerSecond = gyroOmegaRadPerSecond / Math.PI; + return feedforward.calculate(-mechanismRotationsPerSecond); } @Override @@ -129,14 +140,16 @@ public void refreshData() { @Override public void updateInputs(TurretIOInputs inputs) { - inputs.turretAngleDegrees = getTurretAngleFieldRelative(); + inputs.turretAngleDegreesFieldRelative = getTurretAngleFieldRelative(); + inputs.turretAngleDegreesRobotRelative = getTurretAngleRobotRelative(); inputs.targetTurretMotorRotations = this.targetTurretAngleMotorRevs; - inputs.normalizedTurretMotorRotations = this.calculatedMotorOffsetRevs; + inputs.turretOffsetRotations = this.calculatedMotorOffsetRevs; inputs.targetTurretDegrees = this.targetTurretDegreesFieldRelative; inputs.processedTargetTurretDegrees = this.processedTargetTurretDegreesFieldRelative; inputs.turretMotorPositionRotations = this.turretMotorPosition.getValueAsDouble(); inputs.pinionEncoderRotations = this.pinionEncoderSignal.getValueAsDouble(); inputs.followerEncoderRotations = this.followerEncoderSignal.getValueAsDouble(); + inputs.rawTurretMechanismRotations = this.getTurretPositionRevs(); } // CONFIGURATIONS @@ -208,14 +221,14 @@ public CANcoderConfiguration getEncoderConfigs() { public CANcoderConfiguration getPinionEncoderConfigs() { CANcoderConfiguration configs = getEncoderConfigs(); - configs.MagnetSensor.MagnetOffset = Turret.FOLLOWER_ENCODER_OFFSET; + configs.MagnetSensor.MagnetOffset = Turret.PINION_ENCODER_OFFSET; return configs; } public CANcoderConfiguration getFollowerEncoderConfigs() { CANcoderConfiguration configs = getEncoderConfigs(); - configs.MagnetSensor.MagnetOffset = Turret.PINION_ENCODER_OFFSET; + configs.MagnetSensor.MagnetOffset = Turret.FOLLOWER_ENCODER_OFFSET; return configs; } @@ -227,30 +240,38 @@ public CANcoderConfiguration getFollowerEncoderConfigs() { * @return the continuous position of the pinion encoder in revolutions */ private double getPinionEncoderRevs() { - double pinionEncoderReading = positiveMod(this.pinionEncoderSignal.getValueAsDouble(), Turret.NORMALIZED_REVOLUTION); - double followerEncoderReading = positiveMod(this.followerEncoderSignal.getValueAsDouble(), Turret.NORMALIZED_REVOLUTION); + double pinionEncoderReading = positiveMod(this.pinionEncoderSignal.getValueAsDouble(), 1.0); + double followerEncoderReading = positiveMod(this.followerEncoderSignal.getValueAsDouble(), 1.0); double bestError = Double.MAX_VALUE; - double bestPosition = 0.0; + double bestPosition = lastPinionRevs; + + int searchCount = (int) Turret.FOLLOWER_ENCODER_TEETH; - int searchCount = (int) Turret.FOLLOWER_ENCODER_TEETH; // number of distinct branches to check (follower encoder teeth) for (int k = 0; k < searchCount; k++) { double assumedPinionRevs = pinionEncoderReading + k; - double predictedFollowerReading = positiveMod(assumedPinionRevs * (Turret.PINION_ENCODER_TEETH / Turret.FOLLOWER_ENCODER_TEETH), Turret.NORMALIZED_REVOLUTION); + + double predictedFollowerReading = + positiveMod(assumedPinionRevs * (Turret.PINION_ENCODER_TEETH / Turret.FOLLOWER_ENCODER_TEETH), 1.0); + double predictionError = Math.abs(predictedFollowerReading - followerEncoderReading); - // check wrap-around error - if (predictionError > Turret.NORMALIZED_REVOLUTION / 2.0) { - predictionError = Turret.NORMALIZED_REVOLUTION - predictionError; + if (predictionError > 0.5) { + predictionError = 1.0 - predictionError; } - // if this branch has a better prediction error than the best one so far, update the best guess for the pinion encoder position - if (predictionError < bestError) { - bestError = predictionError; + // continuity penalty + double continuityError = Math.abs(assumedPinionRevs - lastPinionRevs); + + double score = predictionError + continuityError * 0.1; + + if (score < bestError) { + bestError = score; bestPosition = assumedPinionRevs; } } + lastPinionRevs = bestPosition; return bestPosition; } @@ -262,7 +283,7 @@ private double getTurretPositionRevs() { double rawPinionRevs = getPinionEncoderRevs(); double rawTurretRevs = rawPinionRevs * (Turret.PINION_ENCODER_TEETH / Turret.TURRET_GEAR_TEETH); // convert pinion revolutions to turret revolutions - double wrapped = positiveMod(rawTurretRevs, Turret.ENCODER_COMBINED_PERIOD_REV); + double wrapped = positiveMod(rawTurretRevs, Turret.ENCODER_COMBINED_PERIOD_TURRET_REV); if (!isInitialized) { this.lastPositionRevs = wrapped; @@ -273,10 +294,10 @@ private double getTurretPositionRevs() { double delta = wrapped - this.lastPositionRevs; // if the change in position is greater than half the combined period, we have wrapped around the encoder, so we need to adjust the delta accordingly - if (delta > Turret.ENCODER_COMBINED_PERIOD_REV / 2.0) { - delta -= Turret.ENCODER_COMBINED_PERIOD_REV; - } else if (delta < -Turret.ENCODER_COMBINED_PERIOD_REV / 2.0) { - delta += Turret.ENCODER_COMBINED_PERIOD_REV; + if (delta > Turret.ENCODER_COMBINED_PERIOD_TURRET_REV / 2.0) { + delta -= Turret.ENCODER_COMBINED_PERIOD_TURRET_REV ; + } else if (delta < -Turret.ENCODER_COMBINED_PERIOD_TURRET_REV / 2.0) { + delta += Turret.ENCODER_COMBINED_PERIOD_TURRET_REV ; } lastPositionRevs += delta; @@ -299,9 +320,11 @@ private double revsToDegreesContinuous(double turretRevs) { private double getTurretAngleRobotRelative() { double continuousRevs = getTurretPositionRevs(); double turretAngleDegrees = revsToDegreesContinuous(continuousRevs); + + double turretAngleWithOffset = turretAngleDegrees - Turret.TURRET_FORWARD_OFFSET_DEG; // apply offset and wrap to [-180, 180) - return wrap180(turretAngleDegrees); + return wrap180(turretAngleWithOffset); } /** diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java index 9dc5534..45f3d07 100644 --- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java +++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java @@ -22,36 +22,51 @@ public static List getCameras() { static { // register cameras + // registerCamera( + // new Camera( + // "left", + // new Transform3d( + // Inches.of(-13.8), + // Inches.of(7.5), + // Inches.of(22.3/4), + // new Rotation3d( + // Degrees.of(0), + // Degrees.of(60), + // Degrees.of(90) + // ) + // ) + // ) + // ); + + // registerCamera( + // new Camera( + // "right", + // new Transform3d( + // Inches.of(13.8), + // Inches.of(5), + // Inches.of(6.3/4), + // new Rotation3d( + // Degrees.of(0), + // Degrees.of(60), + // Degrees.of(-90) + // ) + // ) + // ) + // ); + registerCamera( new Camera( "left", new Transform3d( - Inches.of(-13.8), - Inches.of(7.5), - Inches.of(22.3/4), - new Rotation3d( - Degrees.of(0), - Degrees.of(60), - Degrees.of(90) - ) - ) - ) - ); - - registerCamera( - new Camera( - "right", - new Transform3d( - Inches.of(13.8), - Inches.of(5), - Inches.of(6.3/4), + Inches.of(0), + Inches.of(29/2.0), + Inches.of(6.75), new Rotation3d( Degrees.of(0), Degrees.of(60), - Degrees.of(-90) + Degrees.of(180) ) - ) - ) + )) ); } } From 8428cb36a7708e08296072dd522a80e2a368a2ed Mon Sep 17 00:00:00 2001 From: Justin E Date: Wed, 11 Mar 2026 11:21:22 -0400 Subject: [PATCH 11/29] Correct turret motor position calculations and adjust field-centric angle logic --- .../frc/robot/subsystems/turret/TurretIOTalonFX.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 340a18c..8a960cf 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -109,12 +109,12 @@ public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) */ @Override public void recalculateTurretMotorZeroPosition() { - // absolute turret position from CRT double turretRevs = getTurretPositionRevs(); + double correctedRevs = turretRevs - (Turret.TURRET_FORWARD_OFFSET_DEG / 360.0); // mechanism rotations where 1 rotation = 180 degrees - double mechanismRotations = turretRevs * 2.0; + double mechanismRotations = correctedRevs * 2.0; this.calculatedMotorOffsetRevs = mechanismRotations; @@ -246,9 +246,9 @@ private double getPinionEncoderRevs() { double bestError = Double.MAX_VALUE; double bestPosition = lastPinionRevs; - int searchCount = (int) Turret.FOLLOWER_ENCODER_TEETH; +// int searchCount = (int) Turret.FOLLOWER_ENCODER_TEETH; - for (int k = 0; k < searchCount; k++) { + for (int k = -20; k < 20; k++) { double assumedPinionRevs = pinionEncoderReading + k; double predictedFollowerReading = @@ -335,7 +335,7 @@ private double getTurretAngleFieldRelative() { double robotRelativeAngle = getTurretAngleRobotRelative(); double currentRobotHeading = this.drive.getPose().getRotation().getDegrees(); - double fieldCentricContinuous = robotRelativeAngle - currentRobotHeading; + double fieldCentricContinuous = robotRelativeAngle + currentRobotHeading; return wrap180(fieldCentricContinuous); } From d04caa2a485a9dcbe3ca3f75bbb9261ce45b58a2 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Wed, 11 Mar 2026 17:16:25 -0400 Subject: [PATCH 12/29] Tune turret PIDs --- .../frc/robot/subsystems/turret/Turret.java | 14 +++- .../frc/robot/subsystems/turret/TurretIO.java | 3 +- .../subsystems/turret/TurretIOTalonFX.java | 75 ++++++++++++------- 3 files changed, 59 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 40d6b1a..e750e7c 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -6,6 +6,9 @@ import frc.robot.subsystems.targeting.ShotCompensation; import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + public class Turret extends SpikeSystem { public static final double TURRET_AIMING_TOLERANCE_DEGREES = 2.0; // degrees within which we consider the turret to be aimed at the target (+-) @@ -24,10 +27,8 @@ public class Turret extends SpikeSystem { public static final double ENCODER_COMBINED_PERIOD_TURRET_REV = ENCODER_COMBINED_PERIOD_REV * (PINION_ENCODER_TEETH / TURRET_GEAR_TEETH); - public static final double PINION_ENCODER_OFFSET = 0; // offset for pinion encoder, to be determined by calibration - public static final double FOLLOWER_ENCODER_OFFSET = 0; // offset for follower encoder, to be determined by calibration - - public static final double TURRET_FORWARD_OFFSET_DEG = 83.7; + public static final double TURRET_CENTER_OFFSET_DEG = 144; // subtracted from robot relative heading + public static final double TURRET_ROBOT_OFFSET_DEG = 120; private TurretIO turretIO; @@ -46,6 +47,11 @@ public void onPeriodic() { // this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg); } + + + Pose2d robotPose = RobotContainer.getDrive().getPose(); + Pose2d turretTranslatedPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Math.toRadians(io.turretAngleDegreesFieldRelative))); + Logger.recordOutput("Turret/RobotTurretPose", turretTranslatedPose); } @Override diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index ed37bd9..fae6535 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -10,7 +10,8 @@ public interface TurretIO extends IORefresher, BaseIO { @AutoLog public static class TurretIOInputs extends BaseInputClass { public double turretAngleDegreesFieldRelative = 0.0; // current angle of the turret, field-relative, in degrees - public double turretAngleDegreesRobotRelative = 0.0; // current angle of the turret, robot-relative, in degrees + public double turretAngleDegreesTurretRelative = 0.0; // current angle of the turret, turret-relative, in degrees, after setting a turret center offset + public double turretAngleDegreesRobotRelative = 0.0; // current angle of the turret, robot-relative, in degrees, after setting a robot center offset public double targetTurretDegrees = 0.0; // target angle for the turret, field-relative, in degrees public double processedTargetTurretDegrees = 0.0; // processed target angle for the turret, field-relative, in degrees, updated when recalculation is called public double targetTurretMotorRotations = 0.0; // target position for the turret motor, in rotations diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 8a960cf..6c70f6a 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -1,21 +1,28 @@ package frc.robot.subsystems.turret; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import frc.robot.CanID; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; public class TurretIOTalonFX implements TurretIO { // KS KV CONSTANTS - private static final double kS = 0.25; // volts needed to overcome static friction + private static final double kS = 0.35; // volts needed to overcome static friction private static final double kV = 0.20; // volts per (rotation per second) to maintain motion // SUBSYSTEMS @@ -66,6 +73,8 @@ public TurretIOTalonFX(CommandSwerveDrivetrain drive) { this.turretMotor.getConfigurator().apply(turretMotorConfig.getSecond()); this.turretMotor.getConfigurator().apply(turretMotorFeedbackConfig); this.turretMotor.getConfigurator().apply(turretSoftwareLimitConfig); + // invert motor + this.turretMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); this.pinionEncoder.getConfigurator().apply(pinionEncoderConfig); this.followerEncoder.getConfigurator().apply(followerEncoderConfig); @@ -91,16 +100,22 @@ public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) double currentRobotHeading = this.drive.getPose().getRotation().getDegrees(); // absolute robot-relative target, in motor rotations - double targetRobotRelativeDeg = wrap180(fieldRelativeAngleDegrees - currentRobotHeading); - double targetMotorRotations = targetRobotRelativeDeg / 180.0; + double targetRobotRelativeDeg = fieldRelativeAngleDegrees - currentRobotHeading; + this.processedTargetTurretDegreesFieldRelative = wrap180(targetRobotRelativeDeg); + + setTurretAngleRobotRelativeDegrees(targetRobotRelativeDeg); + } - this.processedTargetTurretDegreesFieldRelative = targetRobotRelativeDeg; - this.targetTurretAngleMotorRevs = targetMotorRotations; + private void setTurretAngleRobotRelativeDegrees(double robotRelativeAngleDegrees) { + setTurretAngleTurretRelativeDegrees(robotRelativeAngleDegrees + Turret.TURRET_ROBOT_OFFSET_DEG); + } + private void setTurretAngleTurretRelativeDegrees(double angleDegrees) { + angleDegrees = wrap180(angleDegrees); + double targetMotorRotations = angleDegrees / 180.0; + Logger.recordOutput("Turret/TargetTurretRelativePosition", targetMotorRotations); this.turretMotor.setControl( - mmRequest - .withPosition(targetMotorRotations) - // .withFeedForward(calculateFeedforward()) + mmRequest.withPosition(targetMotorRotations) ); } @@ -110,15 +125,9 @@ public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) @Override public void recalculateTurretMotorZeroPosition() { // absolute turret position from CRT - double turretRevs = getTurretPositionRevs(); - double correctedRevs = turretRevs - (Turret.TURRET_FORWARD_OFFSET_DEG / 360.0); - - // mechanism rotations where 1 rotation = 180 degrees - double mechanismRotations = correctedRevs * 2.0; + this.calculatedMotorOffsetRevs = getTurretAngle() / 180.0; - this.calculatedMotorOffsetRevs = mechanismRotations; - - turretMotor.setPosition(mechanismRotations); + turretMotor.setPosition(this.calculatedMotorOffsetRevs); } /** @@ -141,6 +150,7 @@ public void refreshData() { @Override public void updateInputs(TurretIOInputs inputs) { inputs.turretAngleDegreesFieldRelative = getTurretAngleFieldRelative(); + inputs.turretAngleDegreesTurretRelative = getTurretAngle(); inputs.turretAngleDegreesRobotRelative = getTurretAngleRobotRelative(); inputs.targetTurretMotorRotations = this.targetTurretAngleMotorRevs; inputs.turretOffsetRotations = this.calculatedMotorOffsetRevs; @@ -161,17 +171,17 @@ public void updateInputs(TurretIOInputs inputs) { private Pair getTurretMotionConfigs() { Slot0Configs configs = new Slot0Configs(); - configs.kP = 1; + configs.kP = 30; configs.kI = 0.0; - configs.kD = 0.0; + configs.kD = 1; - configs.kS = kS; - configs.kV = kV; + configs.kS = 0.4; //kS; + configs.kV = 0.2; //kV; MotionMagicConfigs mmConfigs = new MotionMagicConfigs(); - mmConfigs.MotionMagicAcceleration = 20; // rotations per second^2 - mmConfigs.MotionMagicCruiseVelocity = 10; // rotations per second + mmConfigs.MotionMagicAcceleration = 5; // rotations per second^2 + mmConfigs.MotionMagicCruiseVelocity = 3; // rotations per second return new Pair<>(configs, mmConfigs); } @@ -221,14 +231,14 @@ public CANcoderConfiguration getEncoderConfigs() { public CANcoderConfiguration getPinionEncoderConfigs() { CANcoderConfiguration configs = getEncoderConfigs(); - configs.MagnetSensor.MagnetOffset = Turret.PINION_ENCODER_OFFSET; + // configs.MagnetSensor.MagnetOffset = Turret.PINION_ENCODER_OFFSET; return configs; } public CANcoderConfiguration getFollowerEncoderConfigs() { CANcoderConfiguration configs = getEncoderConfigs(); - configs.MagnetSensor.MagnetOffset = Turret.FOLLOWER_ENCODER_OFFSET; + // configs.MagnetSensor.MagnetOffset = Turret.FOLLOWER_ENCODER_OFFSET; return configs; } @@ -239,6 +249,7 @@ public CANcoderConfiguration getFollowerEncoderConfigs() { * Calculates the continuous position of the pinion (driving) encoder in revolutions * @return the continuous position of the pinion encoder in revolutions */ + @AutoLogOutput(key = "Turret/PinionEncoderRevsCalculated") private double getPinionEncoderRevs() { double pinionEncoderReading = positiveMod(this.pinionEncoderSignal.getValueAsDouble(), 1.0); double followerEncoderReading = positiveMod(this.followerEncoderSignal.getValueAsDouble(), 1.0); @@ -261,9 +272,9 @@ private double getPinionEncoderRevs() { } // continuity penalty - double continuityError = Math.abs(assumedPinionRevs - lastPinionRevs); + // double continuityError = Math.abs(assumedPinionRevs - lastPinionRevs); - double score = predictionError + continuityError * 0.1; + double score = predictionError * 0.1; if (score < bestError) { bestError = score; @@ -317,16 +328,24 @@ private double revsToDegreesContinuous(double turretRevs) { * Gets the angle of the turret in robot space wrapped from [-180, 180) * @return the angle of the turret in robot space, wrapped */ - private double getTurretAngleRobotRelative() { + private double getTurretAngle() { double continuousRevs = getTurretPositionRevs(); double turretAngleDegrees = revsToDegreesContinuous(continuousRevs); - double turretAngleWithOffset = turretAngleDegrees - Turret.TURRET_FORWARD_OFFSET_DEG; + double turretAngleWithOffset = turretAngleDegrees - Turret.TURRET_CENTER_OFFSET_DEG; // apply offset and wrap to [-180, 180) return wrap180(turretAngleWithOffset); } + /** + * Gets the angle of the turret in robot space, without wrapping, so it can be used for continuous calculations. + * @return the angle of the turret in robot space, without wrapping + */ + public double getTurretAngleRobotRelative() { + return wrap180(getTurretAngle() - Turret.TURRET_ROBOT_OFFSET_DEG); + } + /** * Gets the angle of the turret in field space, wrapped from [-180, 180) * @return the angle of the turret in field space, wrapped From 9df8b847c6e8769f94864b8f600da73d052745d7 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Wed, 11 Mar 2026 18:55:45 -0400 Subject: [PATCH 13/29] more work done --- src/main/java/frc/robot/RobotContainer.java | 1 + .../drive/CommandSwerveDrivetrain.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../robot/subsystems/targeting/ShotData.java | 2 +- .../frc/robot/subsystems/trigger/Trigger.java | 2 +- .../frc/robot/subsystems/turret/Turret.java | 37 ++++++++++++++++--- .../subsystems/turret/TurretIOTalonFX.java | 2 +- .../vision/photon/CameraManager.java | 2 +- 8 files changed, 38 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1e3e39b..b9f765a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -116,6 +116,7 @@ private void setupSwerveBindings() { driverController.start().and(driverController.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); // reset the field-centric heading on left bumper press + driverController.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); operatorController.y().onTrue(vision.runOnce(() -> { diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java index b2185a4..77519a3 100644 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java @@ -44,7 +44,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { // default standard deviations // x, y, heading; trust x and y translation and reject yaw - public static final Vector kDefaultVisionStdDevs = VecBuilder.fill(0.3, 0.3, 99999.0); + public static final Vector kDefaultVisionStdDevs = VecBuilder.fill(0.3, 0.3, 1.0); private static final double kSimLoopPeriod = 0.005; // 5 ms private double m_lastSimTime; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 148e5cd..fb92821 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -27,7 +27,7 @@ public void onPeriodic() { this.targetRPS = newTargetRPS; shooterIO.setHoodAngle(shotData.hoodAngleDeg()); - // shooterIO.setFlywheelVelocity(newTargetRPS); + shooterIO.setFlywheelVelocity(newTargetRPS); } } diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java index 6857a9b..4ad098e 100644 --- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -9,7 +9,7 @@ public class ShotData { static { // TODO: get data and fill these in // distanceToRPM.put(20.0, 3000.0); - distanceToRPM.put(10.0, 3500.0); + distanceToRPM.put(10.0, 2000.0); distanceToHoodAngle.put(20.0, 15.0); } } diff --git a/src/main/java/frc/robot/subsystems/trigger/Trigger.java b/src/main/java/frc/robot/subsystems/trigger/Trigger.java index b58e163..e5b2694 100644 --- a/src/main/java/frc/robot/subsystems/trigger/Trigger.java +++ b/src/main/java/frc/robot/subsystems/trigger/Trigger.java @@ -7,7 +7,7 @@ public class Trigger extends SpikeSystem { private static final int PROXIMITY_SENSOR_CHANNEL = 2; // DIO channel for the proximity sensor - private final static double TRIGGER_SPEED = 0; // Rotations per second + private final static double TRIGGER_SPEED = 20.0; // Rotations per second private final Shooter shooter; private final Turret turret; diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index e750e7c..3c54775 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.turret; +import frc.lib.FieldConstants; import frc.lib.subsystem.SpikeSystem; import frc.robot.RobotContainer; import frc.robot.subsystems.targeting.Targeting; @@ -8,6 +9,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; public class Turret extends SpikeSystem { public static final double TURRET_AIMING_TOLERANCE_DEGREES = 2.0; // degrees within which we consider the turret to be aimed at the target (+-) @@ -28,8 +31,9 @@ public class Turret extends SpikeSystem { ENCODER_COMBINED_PERIOD_REV * (PINION_ENCODER_TEETH / TURRET_GEAR_TEETH); public static final double TURRET_CENTER_OFFSET_DEG = 144; // subtracted from robot relative heading - public static final double TURRET_ROBOT_OFFSET_DEG = 120; + public static final double TURRET_ROBOT_OFFSET_DEG = -61; // subtracted from robot relative heading to get turret relative heading + public static final Translation2d TURRET_OFFSET_FROM_CENTER = new Translation2d(-0.3, 0); // distance from the center of the robot to the center of the turret, in meters private TurretIO turretIO; @@ -40,13 +44,15 @@ public Turret() { @Override public void onPeriodic() { ShotCompensation.AdjustedShot shotData = Targeting.getShotData(); - turretIO.setTurretAngleFieldRelativeDegrees(0); + // turretIO.setTurretAngleFieldRelativeDegrees(0); - if (shotData != null) { - double newTargetAngleDeg = shotData.turretAngleDeg(); + // if (shotData != null) { + // double newTargetAngleDeg = shotData.turretAngleDeg(); - // this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg); - } + // this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg); + // } + + this.turretIO.setTurretAngleFieldRelativeDegrees(getTurretAngleDegreesFieldRelative()); Pose2d robotPose = RobotContainer.getDrive().getPose(); @@ -54,6 +60,25 @@ public void onPeriodic() { Logger.recordOutput("Turret/RobotTurretPose", turretTranslatedPose); } + public double getTurretAngleDegreesFieldRelative() { + // calculate field-relative angle of the turret based on the turret motor position and the robot's heading + // get pose of robot + + Pose2d robotPose = RobotContainer.getDrive().getPose(); + Translation2d turretPose = TURRET_OFFSET_FROM_CENTER.rotateBy(robotPose.getRotation()).plus(robotPose.getTranslation()); + // get pose of the target + Translation2d goalPose = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + // calculate the angle from the robot to the target + Translation2d difference = goalPose.minus(turretPose); + // add turret offset from center to get the angle from the turret to the target + difference = TURRET_OFFSET_FROM_CENTER.rotateBy(robotPose.getRotation()).plus(difference); + double angleToTarget = difference.getAngle().getDegrees(); + Pose2d targetTurretPose = new Pose2d(turretPose, difference.getAngle()); + Logger.recordOutput("Turret/TargetPose", targetTurretPose); + Logger.recordOutput("Turret/AngleToTargetDeg", angleToTarget); + return angleToTarget; + } + @Override protected Runnable setupDataRefresher() { turretIO = new TurretIOTalonFX(RobotContainer.getDrive()); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 6c70f6a..66c6fbc 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -113,7 +113,7 @@ private void setTurretAngleRobotRelativeDegrees(double robotRelativeAngleDegrees private void setTurretAngleTurretRelativeDegrees(double angleDegrees) { angleDegrees = wrap180(angleDegrees); double targetMotorRotations = angleDegrees / 180.0; - Logger.recordOutput("Turret/TargetTurretRelativePosition", targetMotorRotations); + this.targetTurretAngleMotorRevs = targetMotorRotations; this.turretMotor.setControl( mmRequest.withPosition(targetMotorRotations) ); diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java index 45f3d07..0c655e8 100644 --- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java +++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java @@ -59,7 +59,7 @@ public static List getCameras() { "left", new Transform3d( Inches.of(0), - Inches.of(29/2.0), + Inches.of(-29/2.0), Inches.of(6.75), new Rotation3d( Degrees.of(0), From e85f68a9163660db13f20366d266779121657315 Mon Sep 17 00:00:00 2001 From: Justin E Date: Thu, 12 Mar 2026 14:32:33 -0400 Subject: [PATCH 14/29] Correct camera logic and turret aiming code --- .../robot/subsystems/targeting/Targeting.java | 14 +++++- .../frc/robot/subsystems/turret/Turret.java | 43 +++++++++++++------ .../frc/robot/subsystems/vision/Vision.java | 6 +-- .../vision/VisionIOPhotonCamera.java | 17 ++++++-- .../subsystems/vision/photon/Camera.java | 24 ++++++++--- .../vision/photon/CameraManager.java | 9 ++-- 6 files changed, 81 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index c791192..2e09ace 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -11,6 +11,7 @@ import frc.lib.Elastic.NotificationLevel; import frc.lib.FieldConstants; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; +import frc.robot.subsystems.turret.Turret; public class Targeting extends SubsystemBase { private static final double NOMINAL_SHOT_TIME_S = 0.3; // see github issue #23 (https://github.com/Team293/Rebuilt/issues/23) @@ -31,13 +32,22 @@ public Targeting(CommandSwerveDrivetrain drive) { */ @Override public void periodic() { - // calculate the adjusted shot parameters based on the current robot movement and the turret's target position + Pose2d robotPose = drive.getPose(); + + // compute the turret pivot location in field coordinates so ShotCompensation + Translation2d turretPivot = Turret.TURRET_OFFSET_FROM_CENTER + .rotateBy(robotPose.getRotation()) + .plus(robotPose.getTranslation()); + Pose2d turretPivotPose = new Pose2d(turretPivot, robotPose.getRotation()); + shotData = ShotCompensation.compensateForMovement( - drive.getPose(), + turretPivotPose, drive.getState().Speeds, new Pose2d(targetPos, new Rotation2d()), NOMINAL_SHOT_TIME_S ); + + Logger.recordOutput("Targeting/TurretPivot", turretPivotPose); } /** diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 3c54775..97d3032 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; public class Turret extends SpikeSystem { public static final double TURRET_AIMING_TOLERANCE_DEGREES = 2.0; // degrees within which we consider the turret to be aimed at the target (+-) @@ -54,27 +53,45 @@ public void onPeriodic() { this.turretIO.setTurretAngleFieldRelativeDegrees(getTurretAngleDegreesFieldRelative()); - + Pose2d robotPose = RobotContainer.getDrive().getPose(); Pose2d turretTranslatedPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Math.toRadians(io.turretAngleDegreesFieldRelative))); Logger.recordOutput("Turret/RobotTurretPose", turretTranslatedPose); + + // Aiming ray: Pose2d[] from turret pivot to hub — renders as a path line in AdvantageScope + Translation2d turretPivot = TURRET_OFFSET_FROM_CENTER + .rotateBy(robotPose.getRotation()) + .plus(robotPose.getTranslation()); + Translation2d hub = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + Translation2d aimVec = hub.minus(turretPivot); + Rotation2d aimAngle = aimVec.getAngle(); + + final int NUM_POINTS = 6; + Pose2d[] aimingRay = new Pose2d[NUM_POINTS]; + for (int i = 0; i < NUM_POINTS; i++) { + double t = (double) i / (NUM_POINTS - 1); + aimingRay[i] = new Pose2d(turretPivot.plus(aimVec.times(t)), aimAngle); + } + Logger.recordOutput("Turret/AimingRay", aimingRay); } public double getTurretAngleDegreesFieldRelative() { // calculate field-relative angle of the turret based on the turret motor position and the robot's heading - // get pose of robot - + // get pose of robo Pose2d robotPose = RobotContainer.getDrive().getPose(); - Translation2d turretPose = TURRET_OFFSET_FROM_CENTER.rotateBy(robotPose.getRotation()).plus(robotPose.getTranslation()); - // get pose of the target + + // translate robot-center pose to the turret pivot location on the field + Translation2d turretPivot = TURRET_OFFSET_FROM_CENTER + .rotateBy(robotPose.getRotation()) + .plus(robotPose.getTranslation()); + + // vector from the turret pivot directly to the goal Translation2d goalPose = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); - // calculate the angle from the robot to the target - Translation2d difference = goalPose.minus(turretPose); - // add turret offset from center to get the angle from the turret to the target - difference = TURRET_OFFSET_FROM_CENTER.rotateBy(robotPose.getRotation()).plus(difference); - double angleToTarget = difference.getAngle().getDegrees(); - Pose2d targetTurretPose = new Pose2d(turretPose, difference.getAngle()); - Logger.recordOutput("Turret/TargetPose", targetTurretPose); + Translation2d toGoal = goalPose.minus(turretPivot); + + double angleToTarget = toGoal.getAngle().getDegrees(); + + Logger.recordOutput("Turret/TurretPivot", new Pose2d(turretPivot, toGoal.getAngle())); Logger.recordOutput("Turret/AngleToTargetDeg", angleToTarget); return angleToTarget; } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 2c7d93a..3a669f1 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.vision; import frc.lib.subsystem.SpikeSystem; +import frc.robot.RobotContainer; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; @@ -46,8 +47,7 @@ public void onPeriodic() { drive.addVisionMeasurement( pose.estimatedPose.toPose2d(), pose.timestampSeconds, - // CommandSwerveDrivetrain.kDefaultVisionStdDevs - CommandSwerveDrivetrain.kDefaultVisionStdDevs.times(1 + ((avgDist * avgDist) / 30)) // scale the std devs based on the average distance to the targets (farther targets are less accurate) + CommandSwerveDrivetrain.kDefaultVisionStdDevs.times(1 + ((avgDist * avgDist) / 30)) ); index++; } @@ -55,7 +55,7 @@ public void onPeriodic() { @Override protected Runnable setupDataRefresher() { - this.visionIO = new VisionIOPhotonCamera(); + this.visionIO = new VisionIOPhotonCamera(() -> RobotContainer.getDrive().getPose()); return useAsyncDataRefresher(visionIO); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java index 6d1fc1b..b0c36b2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import frc.lib.subsystem.IORefresher; import frc.robot.subsystems.vision.photon.Camera; @@ -9,19 +10,28 @@ import java.util.ArrayList; import java.util.List; import java.util.Objects; +import java.util.function.Supplier; public class VisionIOPhotonCamera implements VisionIO, IORefresher { private final List estimatedRobotPoses; + private final Supplier odometryPoseSupplier; - public VisionIOPhotonCamera() { + public VisionIOPhotonCamera(Supplier odometryPoseSupplier) { this.estimatedRobotPoses = new ArrayList<>(); + this.odometryPoseSupplier = odometryPoseSupplier; } @Override public void refreshData() { + Pose2d currentOdometryPose = odometryPoseSupplier.get(); + var newPoses = CameraManager.getCameras().stream() - .map(Camera::getEstimatedRobotPose) + .map(camera -> { + // give the estimator the current odometry pose so single-tag fallback is accurate + camera.setReferencePose(currentOdometryPose); + return camera.getEstimatedRobotPose(); + }) .filter(Objects::nonNull) .toList(); estimatedRobotPoses.clear(); @@ -35,9 +45,10 @@ public void updateInputs(VisionIOInputs inputs) { .toArray(Pose3d[]::new); } - @Override public List getEstimatedRobotPoses() { return new ArrayList<>(estimatedRobotPoses); } } + + diff --git a/src/main/java/frc/robot/subsystems/vision/photon/Camera.java b/src/main/java/frc/robot/subsystems/vision/photon/Camera.java index f8d8514..09679f3 100644 --- a/src/main/java/frc/robot/subsystems/vision/photon/Camera.java +++ b/src/main/java/frc/robot/subsystems/vision/photon/Camera.java @@ -4,7 +4,6 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Transform3d; -import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -20,17 +19,19 @@ public class Camera { private transient final PhotonCamera photonCamera; private final String id; private transient final PhotonPoseEstimator poseEstimator; - private final Transform3d cameraToRobot; + private final Transform3d robotToCamera; - public Camera(String id, Transform3d cameraToRobot) { - this.cameraToRobot = cameraToRobot; + public Camera(String id, Transform3d robotToCamera) { + this.robotToCamera = robotToCamera; this.id = id; this.photonCamera = new PhotonCamera(id); this.poseEstimator = new PhotonPoseEstimator( AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField), PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - cameraToRobot + robotToCamera ); + // fall back to lowest-ambiguity single-tag strategy when multi-tag isn't available + this.poseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY); } /** @@ -85,7 +86,16 @@ public String getId() { return id; } - public Transform3d getCameraToRobot() { - return cameraToRobot; + public Transform3d getRobotToCamera() { + return robotToCamera; + } + + /** + * Sets the reference pose used by the pose estimator for single-tag fallback estimation. + * Should be called with the current odometry pose before each call to getEstimatedRobotPose(). + * @param referencePose the current best-estimate robot pose from odometry + */ + public void setReferencePose(edu.wpi.first.math.geometry.Pose2d referencePose) { + poseEstimator.setReferencePose(referencePose); } } diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java index 0c655e8..97650e8 100644 --- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java +++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java @@ -56,17 +56,18 @@ public static List getCameras() { registerCamera( new Camera( - "left", + "left", new Transform3d( + Inches.of(-29.5/2), Inches.of(0), - Inches.of(-29/2.0), Inches.of(6.75), new Rotation3d( Degrees.of(0), - Degrees.of(60), + Degrees.of(-60), // negative pitch = tilted upward Degrees.of(180) ) - )) + ) + ) ); } } From 99a8799103e781ad0b2c8a2438a61f5ac02f0014 Mon Sep 17 00:00:00 2001 From: Justin E Date: Thu, 12 Mar 2026 14:33:09 -0400 Subject: [PATCH 15/29] clean up formatting --- src/main/java/frc/robot/subsystems/turret/Turret.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 97d3032..d88db9d 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -53,12 +53,10 @@ public void onPeriodic() { this.turretIO.setTurretAngleFieldRelativeDegrees(getTurretAngleDegreesFieldRelative()); - Pose2d robotPose = RobotContainer.getDrive().getPose(); Pose2d turretTranslatedPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Math.toRadians(io.turretAngleDegreesFieldRelative))); Logger.recordOutput("Turret/RobotTurretPose", turretTranslatedPose); - // Aiming ray: Pose2d[] from turret pivot to hub — renders as a path line in AdvantageScope Translation2d turretPivot = TURRET_OFFSET_FROM_CENTER .rotateBy(robotPose.getRotation()) .plus(robotPose.getTranslation()); From 084aa8b44a099c2c3078190582f17c47e724cb2c Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Thu, 12 Mar 2026 18:36:02 -0400 Subject: [PATCH 16/29] Tune turret and shooter --- src/main/java/frc/robot/RobotContainer.java | 44 +++---- .../frc/robot/generated/TunerConstants.java | 56 ++++---- .../frc/robot/subsystems/shooter/Shooter.java | 38 ++++-- .../subsystems/shooter/ShooterIOTalonFX.java | 122 ++++++++++++------ .../robot/subsystems/targeting/ShotData.java | 6 +- .../robot/subsystems/targeting/Targeting.java | 19 +++ .../frc/robot/subsystems/trigger/Trigger.java | 6 +- .../frc/robot/subsystems/turret/Turret.java | 29 ++--- .../subsystems/turret/TurretIOTalonFX.java | 6 +- 9 files changed, 197 insertions(+), 129 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b9f765a..93d7edc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -51,21 +51,21 @@ public class RobotContainer { public static CommandSwerveDrivetrain drive; private final Vision vision; private final Turret turret; - private final Intake intake; + // private final Intake intake; private final Trigger trigger; private final Shooter shooter; - private final Targeting targeting; - private final Findexer findexer; + // private final Targeting targeting; + // private final Findexer findexer; public RobotContainer() { drive = TunerConstants.createDrivetrain(); this.turret = new Turret(); this.vision = new Vision(drive); - this.intake = new Intake(drive); + // this.intake = new Intake(drive); this.shooter = new Shooter(); - this.targeting = new Targeting(drive); + // this.targeting = new Targeting(drive); this.trigger = new Trigger(shooter, turret); - this.findexer = new Findexer(trigger); + // this.findexer = new Findexer(trigger); autoChooser = drive.getAutoChooser(); SmartDashboard.putData("Auto Path", autoChooser); @@ -81,7 +81,7 @@ private void configureBindings() { setupSwerveBindings(); setupIntakeBindings(); setupTargetingBindings(); - // setupShooterBindings(); + setupShooterBindings(); } private void setupSwerveBindings() { @@ -119,13 +119,13 @@ private void setupSwerveBindings() { driverController.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); - operatorController.y().onTrue(vision.runOnce(() -> { - var estimatedPose = vision.getEstimatedPositionFromCameras(); - if (estimatedPose != null) { - Logger.recordOutput("Vision/SnapshotEstimate", estimatedPose); - drive.resetPose(estimatedPose); - } - })); + // operatorController.y().onTrue(vision.runOnce(() -> { + // var estimatedPose = vision.getEstimatedPositionFromCameras(); + // if (estimatedPose != null) { + // Logger.recordOutput("Vision/SnapshotEstimate", estimatedPose); + // drive.resetPose(estimatedPose); + // } + // })); } private void setupIntakeBindings() { @@ -134,16 +134,16 @@ private void setupIntakeBindings() { } private void setupTargetingBindings() { - operatorController.rightBumper().onTrue(targeting.run(targeting::setTargetingHub)); - operatorController.leftBumper().onTrue(targeting.run(targeting::setTargetingShuttle)); + // operatorController.rightBumper().onTrue(targeting.run(targeting::setTargetingHub)); + // operatorController.leftBumper().onTrue(targeting.run(targeting::setTargetingShuttle)); } - // private void setupShooterBindings() { - // // toggle shooter on right trigger hold - // driverController.rightTrigger() - // .whileTrue(shooter.run(() -> shooter.setDriverRequestingShooting(true))) - // .whileFalse(shooter.run(() -> shooter.setDriverRequestingShooting(false))); - // } + private void setupShooterBindings() { + // toggle shooter on right trigger hold + driverController.rightBumper() + .onTrue(shooter.run(() -> shooter.setDriverRequestingShooting(true))) + .onFalse(shooter.run(() -> shooter.setDriverRequestingShooting(false))); + } public Command getAutonomousCommand() { return autoChooser.getSelected(); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 1564bbe..1693682 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -124,49 +124,49 @@ public class TunerConstants { .withDriveFrictionVoltage(kDriveFrictionVoltage); - // Front Left - private static final int kFrontLeftDriveMotorId = 0; - private static final int kFrontLeftSteerMotorId = 50; - private static final int kFrontLeftEncoderId = 31; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.463134765625); + // Front Left (was Back Right) + private static final int kFrontLeftDriveMotorId = 3; + private static final int kFrontLeftSteerMotorId = 24; + private static final int kFrontLeftEncoderId = 5; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.344482421875); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(11.5); - private static final Distance kFrontLeftYPos = Inches.of(11.5); + private static final Distance kFrontLeftXPos = Inches.of(-11.5); + private static final Distance kFrontLeftYPos = Inches.of(-11.5); - // Front Right - private static final int kFrontRightDriveMotorId = 61; - private static final int kFrontRightSteerMotorId = 27; - private static final int kFrontRightEncoderId = 25; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.306396484375); + // Front Right (was Back Left) + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 30; + private static final int kFrontRightEncoderId = 22; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.20166015625); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(11.5); - private static final Distance kFrontRightYPos = Inches.of(-11.5); + private static final Distance kFrontRightXPos = Inches.of(-11.5); + private static final Distance kFrontRightYPos = Inches.of(11.5); - // Back Left - private static final int kBackLeftDriveMotorId = 1; - private static final int kBackLeftSteerMotorId = 30; - private static final int kBackLeftEncoderId = 22; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.20166015625); + // Back Left (was Front Right) + private static final int kBackLeftDriveMotorId = 61; + private static final int kBackLeftSteerMotorId = 27; + private static final int kBackLeftEncoderId = 25; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.306396484375); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-11.5); - private static final Distance kBackLeftYPos = Inches.of(11.5); + private static final Distance kBackLeftXPos = Inches.of(11.5); + private static final Distance kBackLeftYPos = Inches.of(-11.5); - // Back Right - private static final int kBackRightDriveMotorId = 3; - private static final int kBackRightSteerMotorId = 24; - private static final int kBackRightEncoderId = 5; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.344482421875); + // Back Right (was Front Left) + private static final int kBackRightDriveMotorId = 0; + private static final int kBackRightSteerMotorId = 50; + private static final int kBackRightEncoderId = 31; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.463134765625); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-11.5); - private static final Distance kBackRightYPos = Inches.of(-11.5); + private static final Distance kBackRightXPos = Inches.of(11.5); + private static final Distance kBackRightYPos = Inches.of(11.5); public static final SwerveModuleConstants FrontLeft = diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fb92821..e79f069 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,18 +1,25 @@ package frc.robot.subsystems.shooter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.subsystem.SpikeSystem; import frc.robot.subsystems.targeting.Targeting; import frc.robot.subsystems.targeting.ShotCompensation; +import frc.robot.subsystems.targeting.ShotData; public class Shooter extends SpikeSystem { - private static final double SHOOTER_READY_THRESHOLD_RPS = 0.5; // RPS threshold to consider the shooter ready + private static final double SHOOTER_READY_THRESHOLD_RPS = 3.0; // RPS threshold to consider the shooter ready private ShooterIO shooterIO; - private double targetRPS = 0.0; // Target rotations per second private boolean driverRequestingShooting = false; // Whether the driver is currently requesting to shoot public Shooter() { super("Shooter", new ShooterIOInputsAutoLogged()); + SmartDashboard.putNumber("TargetRPM", 0); + SmartDashboard.putNumber("TargetHoodAngle", 0); } /** @@ -20,15 +27,15 @@ public Shooter() { */ @Override public void onPeriodic() { - ShotCompensation.AdjustedShot shotData = Targeting.getShotData(); + double distToTarget = getDistanceToTarget(); // distance in meters + // double targetRPM = ShotData.distanceToRPM.get(distToTarget); + // double hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget); - if (shotData != null) { - double newTargetRPS = shotData.rpm() / 60.0; - this.targetRPS = newTargetRPS; + double targetRPM = SmartDashboard.getNumber("TargetRPM", 0); + double hoodAngle = SmartDashboard.getNumber("TargetHoodAngle", 0); - shooterIO.setHoodAngle(shotData.hoodAngleDeg()); - shooterIO.setFlywheelVelocity(newTargetRPS); - } + shooterIO.setHoodAngle(hoodAngle); + shooterIO.setFlywheelVelocity(targetRPM / 60.0); // convert RPM to RPS } /** @@ -44,8 +51,19 @@ protected Runnable setupDataRefresher() { * Checks if current rps of the motor is within the allowed error bounds * @return if the target is within error bounds */ + @AutoLogOutput(key="Shooter/IsAtTargetRPS") public boolean isAtTargetRPS() { - return Math.abs(super.io.motorRPS - targetRPS) < SHOOTER_READY_THRESHOLD_RPS; + return Math.abs(super.io.motorRPS - io.flywheelSetPointRPS) < SHOOTER_READY_THRESHOLD_RPS; + } + + /** + * Returns the distance from the center of the turret to the target in meters + * @return + */ + public double getDistanceToTarget() { + Translation2d toGoal = Targeting.differenceBetweenRobotAndTarget(); + Logger.recordOutput("Targeting/DistanceToTarget", toGoal.getNorm()); + return toGoal.getNorm(); } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index afd9de6..8af537a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -3,29 +3,39 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CommutationConfigs; +import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFXS; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; import frc.lib.subsystem.IORefresher; import frc.robot.CanID; public class ShooterIOTalonFX implements IORefresher, ShooterIO { - private static final double HOOD_GEAR_RATIO = 1.0/1.0; // hood pulley teeth / motor pulley teeth (motor revs per hood rev) + private static final double HOOD_GEAR_RATIO = 1.0 / 1.0; // hood pulley teeth / motor pulley teeth (motor revs per + // hood rev) private final TalonFX flywheelMotor; private final TalonFXS hoodMotor; private final CANcoder hoodEncoder; // using wcp throughbore; you interface through 'CANcoder' class - private final double hoodMotorPositionOffset; // offset in motor rotations, calculated at startup, units in motor rotations - private final VelocityVoltage flywheelVelocityControl = new VelocityVoltage(0); - private final MotionMagicVelocityVoltage hoodVelocityControl = new MotionMagicVelocityVoltage(0); + private final PositionVoltage hoodPositionControl = new PositionVoltage(0); + private final StatusSignal hoodMotorVoltage; // volts private final StatusSignal motorVelocity; // rps private final StatusSignal hoodMotorPosition; // motor rotations private final StatusSignal hoodAngle; // degrees @@ -36,21 +46,49 @@ public class ShooterIOTalonFX implements IORefresher, ShooterIO { private double hoodTargetEncoder = 0.0; public ShooterIOTalonFX() { - // hood configs this.hoodMotor = new TalonFXS(CanID.HOOD_MOTOR.getID()); + this.hoodEncoder = new CANcoder(CanID.HOOD_ENCODER.getID()); + this.flywheelMotor = new TalonFX(CanID.FLYWHEEL_MOTOR.getID()); + + // hood encoder configs + CANcoderConfiguration hoodEncoderConfig = new CANcoderConfiguration(); + hoodEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + this.hoodEncoder.getConfigurator().apply(hoodEncoderConfig); // apply default configs to encoder before + // using it for feedback + // constrains the range to [0, 1) + hoodEncoderConfig.MagnetSensor.withAbsoluteSensorDiscontinuityPoint(1.0); + this.hoodEncoder.getConfigurator().apply(hoodEncoderConfig); + // hood configs var hoodSlot0 = new Slot0Configs(); - hoodSlot0.kP = 0.1; + hoodSlot0.kP = 100.0; hoodSlot0.kI = 0.0; hoodSlot0.kD = 0.0; - hoodSlot0.kS = 0.0; + hoodSlot0.kS = 20.0; hoodSlot0.kV = 0.0; + SoftwareLimitSwitchConfigs hoodSoftLimits = new SoftwareLimitSwitchConfigs(); + hoodSoftLimits.ForwardSoftLimitEnable = true; + hoodSoftLimits.ForwardSoftLimitThreshold = 1.1; + hoodSoftLimits.ReverseSoftLimitEnable = true; + hoodSoftLimits.ReverseSoftLimitThreshold = -0.1; + + MotorOutputConfigs hoodMotorOutputConfigs = new MotorOutputConfigs(); + hoodMotorOutputConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + + ExternalFeedbackConfigs hoodExternalFeedback = new ExternalFeedbackConfigs(); + hoodExternalFeedback.withRemoteCANcoder(this.hoodEncoder); + + CommutationConfigs hoodCommutation = new CommutationConfigs(); + hoodCommutation.MotorArrangement = MotorArrangementValue.Brushed_DC; + this.hoodMotor.getConfigurator().apply(hoodSlot0); + this.hoodMotor.getConfigurator().apply(hoodExternalFeedback); + this.hoodMotor.getConfigurator().apply(hoodSoftLimits); + this.hoodMotor.getConfigurator().apply(hoodCommutation); + this.hoodMotor.getConfigurator().apply(hoodMotorOutputConfigs); // flywheel configs - this.flywheelMotor = new TalonFX(CanID.FLYWHEEL_MOTOR.getID()); - var flywheelSlot0 = new Slot0Configs(); flywheelSlot0.kP = 0.15; flywheelSlot0.kI = 0.0; @@ -60,52 +98,46 @@ public ShooterIOTalonFX() { this.flywheelMotor.getConfigurator().apply(flywheelSlot0); - this.hoodEncoder = new CANcoder(CanID.HOOD_ENCODER.getID()); - - CANcoderConfiguration hoodEncoderConfig = new CANcoderConfiguration(); - // constrains the range to [0, 1) - hoodEncoderConfig.MagnetSensor.withAbsoluteSensorDiscontinuityPoint(1.0); - - this.hoodEncoder.getConfigurator().apply(hoodEncoderConfig); - this.motorVelocity = flywheelMotor.getVelocity(); this.hoodAngle = hoodEncoder.getAbsolutePosition(); this.hoodMotorPosition = hoodMotor.getPosition(); + this.hoodMotorVoltage = hoodMotor.getMotorVoltage(); // force refresh before zero calculations - BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition); + BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition, hoodMotorVoltage); - // convert hood angle to motor rotations and calculate offset - this.hoodMotorPositionOffset = this.hoodMotorPosition.getValueAsDouble() - angleToMotorRotations(this.hoodAngle.getValueAsDouble()); + this.hoodEncoder.setPosition(-0.005); flywheelMotor.optimizeBusUtilization(); hoodMotor.optimizeBusUtilization(); } - + /** * Periodically refreshes encoder signal + * * @note This is called automatically */ @Override public void refreshData() { BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition); - double current = hoodAngle.getValueAsDouble(); + // double current = hoodAngle.getValueAsDouble(); - if (Math.abs(current - hoodTargetEncoder) < 0.005) { - hoodMotor.stopMotor(); - } + // // if (Math.abs(current - hoodTargetEncoder) < 0.005) { + // // hoodMotor.stopMotor(); + // // } } /** * Periodically called to update the shooter information for logging + * * @param inputs ShooterIOInputs object to update */ @Override public void updateInputs(ShooterIOInputs inputs) { inputs.motorRPS = this.motorVelocity.getValueAsDouble(); - inputs.hoodAngle = this.hoodAngle.getValueAsDouble() * 360; // convert rotations to degrees - inputs.hoodMotorPosition = this.hoodMotorPosition.getValueAsDouble(); + inputs.hoodAngle = this.hoodAngle.getValueAsDouble() * 30.0 + 15.0; // convert rotations to degrees + inputs.hoodMotorPosition = this.hoodAngle.getValueAsDouble(); inputs.flywheelSetPointRPS = this.flywheelRPSSetPoint; inputs.hoodSetPointAngle = this.hoodAngleSetPoint; @@ -113,6 +145,7 @@ public void updateInputs(ShooterIOInputs inputs) { /** * Set the target flywheel velocity + * * @param rps - target rotations per second */ @Override @@ -124,10 +157,11 @@ public void setFlywheelVelocity(double rps) { } /** - * Set the target hood angle - * @param angle target angle TODO: relative to what + * Set the target hood angle + * + * @param angle target angle TODO: relative to what */ - @Override + @Override public void setHoodAngle(double angle) { angle = Math.max(15, Math.min(45, angle)); this.hoodAngleSetPoint = angle; @@ -135,19 +169,12 @@ public void setHoodAngle(double angle) { // convert target angle -> encoder rotations this.hoodTargetEncoder = angleToEncoder(angle); - double current = hoodAngle.getValueAsDouble(); - - double velocity = 0.5; // rps, tune later - - if (current > hoodTargetEncoder) { - velocity = -velocity; - } - - hoodMotor.setControl(hoodVelocityControl.withVelocity(velocity)); + hoodMotor.setControl(hoodPositionControl.withPosition(this.hoodTargetEncoder).withSlot(0)); } /** * Converts angle in degrees to motor rotations per second + * * @param angle Input angle in degrees * @return double motor rotations per second */ @@ -156,7 +183,22 @@ private static double angleToMotorRotations(double angle) { return angle * HOOD_GEAR_RATIO / 360.0; } + /** + * Returns a position from zero to 1 representing the position of the hood, + * where 0 is 15 degrees and 1 is 45 degrees + * + * @param angle + * @return + */ private static double angleToEncoder(double angle) { - return (45.0 - angle) / 30.0; + angle -= 14.0; // shift so that 0 is at 15 degrees + angle /= 30.0; // scale so that 1 is at 45 degrees + if (angle < 0.0) { + return 0.0; + } else if (angle > 1.0) { + return 1.0; + } else { + return angle; + } } } diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java index 4ad098e..e94ec08 100644 --- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -7,9 +7,7 @@ public class ShotData { public static final InterpolatingDoubleTreeMap distanceToHoodAngle = new InterpolatingDoubleTreeMap(); static { - // TODO: get data and fill these in - // distanceToRPM.put(20.0, 3000.0); - distanceToRPM.put(10.0, 2000.0); - distanceToHoodAngle.put(20.0, 15.0); + distanceToRPM.put(2.7, 1900.0); + distanceToHoodAngle.put(2.7, 30.0); } } diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index 2e09ace..6650684 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -10,6 +10,7 @@ import frc.lib.Elastic.Notification; import frc.lib.Elastic.NotificationLevel; import frc.lib.FieldConstants; +import frc.robot.RobotContainer; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; import frc.robot.subsystems.turret.Turret; @@ -26,6 +27,24 @@ public Targeting(CommandSwerveDrivetrain drive) { Logger.recordOutput("HubTarget", FieldConstants.Hub.oppTopCenterPoint); Logger.recordOutput("ShuttleTarget", new Pose2d(0, 0, new Rotation2d())); } + + public static Translation2d differenceBetweenRobotAndTarget() { + // calculate field-relative angle of the turret based on the turret motor position and the robot's heading + // get pose of robo + Pose2d robotPose = RobotContainer.getDrive().getPose(); + + // translate robot-center pose to the turret pivot location on the field + Translation2d turretPivot = Turret.TURRET_OFFSET_FROM_CENTER + .rotateBy(robotPose.getRotation()) + .plus(robotPose.getTranslation()); + + // vector from the turret pivot directly to the goal + Translation2d goalPose = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + Translation2d toGoal = goalPose.minus(turretPivot); + + Logger.recordOutput("Turret/TurretPivot", new Pose2d(turretPivot, toGoal.getAngle())); + return toGoal; + } /** * Periodically calculate the shot data given the target position and robot movement diff --git a/src/main/java/frc/robot/subsystems/trigger/Trigger.java b/src/main/java/frc/robot/subsystems/trigger/Trigger.java index e5b2694..0f1b2ee 100644 --- a/src/main/java/frc/robot/subsystems/trigger/Trigger.java +++ b/src/main/java/frc/robot/subsystems/trigger/Trigger.java @@ -28,9 +28,9 @@ public void onPeriodic() { // run the indexer if the mechanisms are ready for balls // run it regardless of ball in indexer, so that it can feed a ball in if there is one queued up triggerIO.setSpeed(TRIGGER_SPEED); - } else if (needsFeeding()) { - // bring the ball to the indexer and stop once we see a ball - triggerIO.setSpeed(TRIGGER_SPEED); + // } else if (needsFeeding()) { + // // bring the ball to the indexer and stop once we see a ball + // triggerIO.setSpeed(TRIGGER_SPEED); } else { // stop the indexer if the mechanisms aren't ready and we have a ball queued triggerIO.setSpeed(0.0); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index d88db9d..8d3fcb0 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -5,6 +5,8 @@ import frc.robot.RobotContainer; import frc.robot.subsystems.targeting.Targeting; import frc.robot.subsystems.targeting.ShotCompensation; + +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; @@ -12,7 +14,7 @@ import edu.wpi.first.math.geometry.Translation2d; public class Turret extends SpikeSystem { - public static final double TURRET_AIMING_TOLERANCE_DEGREES = 2.0; // degrees within which we consider the turret to be aimed at the target (+-) + public static final double TURRET_AIMING_TOLERANCE_DEGREES = 5.0; // degrees within which we consider the turret to be aimed at the target (+-) // HARDWARE CONSTANTS // gearing @@ -29,10 +31,10 @@ public class Turret extends SpikeSystem { public static final double ENCODER_COMBINED_PERIOD_TURRET_REV = ENCODER_COMBINED_PERIOD_REV * (PINION_ENCODER_TEETH / TURRET_GEAR_TEETH); - public static final double TURRET_CENTER_OFFSET_DEG = 144; // subtracted from robot relative heading - public static final double TURRET_ROBOT_OFFSET_DEG = -61; // subtracted from robot relative heading to get turret relative heading + public static final double TURRET_CENTER_OFFSET_DEG = -17; // subtracted from robot relative heading + public static final double TURRET_ROBOT_OFFSET_DEG = 120; // subtracted from robot relative heading to get turret relative heading - public static final Translation2d TURRET_OFFSET_FROM_CENTER = new Translation2d(-0.3, 0); // distance from the center of the robot to the center of the turret, in meters + public static final Translation2d TURRET_OFFSET_FROM_CENTER = new Translation2d(0.3, 0); // distance from the center of the robot to the center of the turret, in meters private TurretIO turretIO; @@ -74,23 +76,10 @@ public void onPeriodic() { } public double getTurretAngleDegreesFieldRelative() { - // calculate field-relative angle of the turret based on the turret motor position and the robot's heading - // get pose of robo - Pose2d robotPose = RobotContainer.getDrive().getPose(); - - // translate robot-center pose to the turret pivot location on the field - Translation2d turretPivot = TURRET_OFFSET_FROM_CENTER - .rotateBy(robotPose.getRotation()) - .plus(robotPose.getTranslation()); - - // vector from the turret pivot directly to the goal - Translation2d goalPose = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); - Translation2d toGoal = goalPose.minus(turretPivot); + Translation2d toGoal = Targeting.differenceBetweenRobotAndTarget(); double angleToTarget = toGoal.getAngle().getDegrees(); - - Logger.recordOutput("Turret/TurretPivot", new Pose2d(turretPivot, toGoal.getAngle())); - Logger.recordOutput("Turret/AngleToTargetDeg", angleToTarget); + Logger.recordOutput("Targeting/AngleToTargetDeg", angleToTarget); return angleToTarget; } @@ -104,6 +93,8 @@ protected Runnable setupDataRefresher() { * Checks if the turret is at the target angle, within the tolerance defined by TURRET_AIMING_TOLERANCE_DEGREES. * @return true if the turret is at the target angle, false otherwise */ + + @AutoLogOutput(key="Turret/IsAtTargetAngle") public boolean isAtTargetAngle() { double error = Math.abs(io.turretAngleDegreesFieldRelative - io.targetTurretDegrees); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 66c6fbc..e464d47 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -171,7 +171,7 @@ public void updateInputs(TurretIOInputs inputs) { private Pair getTurretMotionConfigs() { Slot0Configs configs = new Slot0Configs(); - configs.kP = 30; + configs.kP = 50; configs.kI = 0.0; configs.kD = 1; @@ -180,8 +180,8 @@ private Pair getTurretMotionConfigs() { MotionMagicConfigs mmConfigs = new MotionMagicConfigs(); - mmConfigs.MotionMagicAcceleration = 5; // rotations per second^2 - mmConfigs.MotionMagicCruiseVelocity = 3; // rotations per second + mmConfigs.MotionMagicAcceleration = 10; // rotations per second^2 + mmConfigs.MotionMagicCruiseVelocity = 3.5; // rotations per second return new Pair<>(configs, mmConfigs); } From 110866cbde4f44d9b5c4d2ba6bb23f5551db2323 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Fri, 13 Mar 2026 17:23:10 -0400 Subject: [PATCH 17/29] Fixed bugs and tuned shot data, added trim --- src/main/java/frc/robot/RobotContainer.java | 14 ++++-- .../frc/robot/subsystems/shooter/Shooter.java | 26 +++++++--- .../robot/subsystems/shooter/ShooterIO.java | 6 +++ .../subsystems/shooter/ShooterIOTalonFX.java | 49 +++++++++++++++++-- .../robot/subsystems/targeting/ShotData.java | 6 +++ .../frc/robot/subsystems/turret/Turret.java | 6 ++- .../frc/robot/subsystems/turret/TurretIO.java | 9 +++- .../subsystems/turret/TurretIOTalonFX.java | 8 +++ 8 files changed, 107 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 93d7edc..1aae1e5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -92,12 +92,12 @@ private void setupSwerveBindings() { drive.applyRequest(() -> driveCmd.withVelocityX(-driverController.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) .withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + .withRotationalRate(driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) ) ); // Idle while the robot is disabled. This ensures the configured - // neutral mode is applied to the drive motors while disabled. + // neutral mode is applied to the drive motors while disabled. final var idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled().whileTrue( drive.applyRequest(() -> idle).ignoringDisable(true) @@ -140,9 +140,17 @@ private void setupTargetingBindings() { private void setupShooterBindings() { // toggle shooter on right trigger hold - driverController.rightBumper() + operatorController.rightTrigger() .onTrue(shooter.run(() -> shooter.setDriverRequestingShooting(true))) .onFalse(shooter.run(() -> shooter.setDriverRequestingShooting(false))); + + + operatorController.x().onTrue(shooter.runOnce(() -> shooter.zeroHood())); + + operatorController.povUp().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(0.5))); + operatorController.povDown().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(-0.5))); + operatorController.povLeft().onTrue(turret.runOnce(() -> turret.changeTrim(2))); + operatorController.povRight().onTrue(turret.runOnce(() -> turret.changeTrim(-2))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index e79f069..2dd3635 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -7,11 +7,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.subsystem.SpikeSystem; import frc.robot.subsystems.targeting.Targeting; -import frc.robot.subsystems.targeting.ShotCompensation; -import frc.robot.subsystems.targeting.ShotData; public class Shooter extends SpikeSystem { - private static final double SHOOTER_READY_THRESHOLD_RPS = 3.0; // RPS threshold to consider the shooter ready + private static final double SHOOTER_READY_THRESHOLD_RPS = 2.0; // RPS threshold to consider the shooter ready private ShooterIO shooterIO; private boolean driverRequestingShooting = false; // Whether the driver is currently requesting to shoot @@ -30,11 +28,17 @@ public void onPeriodic() { double distToTarget = getDistanceToTarget(); // distance in meters // double targetRPM = ShotData.distanceToRPM.get(distToTarget); // double hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget); - + double targetRPM = SmartDashboard.getNumber("TargetRPM", 0); double hoodAngle = SmartDashboard.getNumber("TargetHoodAngle", 0); - shooterIO.setHoodAngle(hoodAngle); + if (io.isZeroing) { + shooterIO.runZeroingHood(); + } else { + // compensate hood angle for rpm of flywheel lowering due to slowdowns + shooterIO.setHoodAngle(hoodAngle); + } + shooterIO.setFlywheelVelocity(targetRPM / 60.0); // convert RPM to RPS } @@ -53,7 +57,7 @@ protected Runnable setupDataRefresher() { */ @AutoLogOutput(key="Shooter/IsAtTargetRPS") public boolean isAtTargetRPS() { - return Math.abs(super.io.motorRPS - io.flywheelSetPointRPS) < SHOOTER_READY_THRESHOLD_RPS; + return Math.abs((super.io.motorRPS - 0.5) - io.flywheelSetPointRPS) < SHOOTER_READY_THRESHOLD_RPS; } /** @@ -63,7 +67,7 @@ public boolean isAtTargetRPS() { public double getDistanceToTarget() { Translation2d toGoal = Targeting.differenceBetweenRobotAndTarget(); Logger.recordOutput("Targeting/DistanceToTarget", toGoal.getNorm()); - return toGoal.getNorm(); + return toGoal.getNorm() + io.distanceTrim; // add distance trim to adjust the distance based on operator controller input } /** @@ -74,6 +78,10 @@ public void setDriverRequestingShooting(boolean isRequesting) { this.driverRequestingShooting = isRequesting; } + public void zeroHood() { + shooterIO.zeroHood(); + } + /** * Returns whether the driver is currently requesting to shoot. * @return true if the driver is requesting to shoot, false otherwise @@ -81,4 +89,8 @@ public void setDriverRequestingShooting(boolean isRequesting) { public boolean isDriverRequestingShooting() { return this.driverRequestingShooting; } + + public void changeDistanceTrim(double deltaDistance) { + shooterIO.changeDistanceTrim(deltaDistance); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 7adbc44..d6e4f7f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -13,8 +13,14 @@ public static class ShooterIOInputs extends BaseInputClass { public double hoodMotorPosition = 0.0; // TODO: position is in relation to what? public double flywheelSetPointRPS = 0.0; // Flywheel set point rotations per second public double hoodSetPointAngle = 0.0; // TODO: Hood setpoint in relation to what? + public double hoodMotorCurrent = 0.0; // Current being drawn by the hood motor + public double distanceTrim = 0.0; // minor adjustment to the returned distance based on operator controller input, in degrees + public boolean isZeroing = false; } void setFlywheelVelocity(double rps); void setHoodAngle(double angle); + void runZeroingHood(); + void zeroHood(); + void changeDistanceTrim(double deltaDistance); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 8af537a..279a925 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFXS; @@ -21,6 +22,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.lib.subsystem.IORefresher; import frc.robot.CanID; @@ -28,6 +30,7 @@ public class ShooterIOTalonFX implements IORefresher, ShooterIO { private static final double HOOD_GEAR_RATIO = 1.0 / 1.0; // hood pulley teeth / motor pulley teeth (motor revs per // hood rev) + private static final double SOFTWARE_LIMIT_SWITCH_CURRENT_THRESHOLD = 1.75; // amps at which we consider the hood to have hit a limit private final TalonFX flywheelMotor; private final TalonFXS hoodMotor; @@ -39,11 +42,15 @@ public class ShooterIOTalonFX implements IORefresher, ShooterIO { private final StatusSignal motorVelocity; // rps private final StatusSignal hoodMotorPosition; // motor rotations private final StatusSignal hoodAngle; // degrees + private final StatusSignal hoodMotorCurrent; // amps private double hoodAngleSetPoint = 0.0; private double flywheelRPSSetPoint = 0.0; private double hoodTargetEncoder = 0.0; + private boolean isZeroing = true; + + private double distanceTrim = 0.0; // minor adjustment to the returned distance based on operator controller input, in degrees public ShooterIOTalonFX() { this.hoodMotor = new TalonFXS(CanID.HOOD_MOTOR.getID()); @@ -68,9 +75,9 @@ public ShooterIOTalonFX() { hoodSlot0.kV = 0.0; SoftwareLimitSwitchConfigs hoodSoftLimits = new SoftwareLimitSwitchConfigs(); - hoodSoftLimits.ForwardSoftLimitEnable = true; + hoodSoftLimits.ForwardSoftLimitEnable = false; hoodSoftLimits.ForwardSoftLimitThreshold = 1.1; - hoodSoftLimits.ReverseSoftLimitEnable = true; + hoodSoftLimits.ReverseSoftLimitEnable = false; hoodSoftLimits.ReverseSoftLimitThreshold = -0.1; MotorOutputConfigs hoodMotorOutputConfigs = new MotorOutputConfigs(); @@ -96,22 +103,51 @@ public ShooterIOTalonFX() { flywheelSlot0.kS = 0.225; flywheelSlot0.kV = 0.125; + // current limits changed from + // 120, 70 to 80, 60 + this.flywheelMotor.getConfigurator().apply(flywheelSlot0); this.motorVelocity = flywheelMotor.getVelocity(); this.hoodAngle = hoodEncoder.getAbsolutePosition(); this.hoodMotorPosition = hoodMotor.getPosition(); this.hoodMotorVoltage = hoodMotor.getMotorVoltage(); + this.hoodMotorCurrent = hoodMotor.getSupplyCurrent(); // force refresh before zero calculations - BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition, hoodMotorVoltage); + BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition, hoodMotorVoltage, hoodMotorCurrent); - this.hoodEncoder.setPosition(-0.005); + this.hoodEncoder.setPosition(0); flywheelMotor.optimizeBusUtilization(); hoodMotor.optimizeBusUtilization(); } + @Override + public void runZeroingHood() { + hoodMotor.setControl(new VoltageOut(-4)); // move hood down at a slow speed + + if (hoodMotorCurrent.getValueAsDouble() > SOFTWARE_LIMIT_SWITCH_CURRENT_THRESHOLD) { // if we hit the floor, the current will spike up + hoodMotor.stopMotor(); + hoodEncoder.setPosition(0); // set encoder position to 0 when we hit the limit + isZeroing = false; + } + } + + @Override + public void zeroHood() { + isZeroing = true; + } + + /** + * Changes the distance trim by a certain amount of degrees. This is used to make minor adjustments to the distance based on operator controller input. + * @param deltaDistance the amount of meters to change the distance trim by. Positive values add to the distance, and negative values subtract from the distance. + */ + @Override + public void changeDistanceTrim(double deltaDistance) { + this.distanceTrim += deltaDistance; + } + /** * Periodically refreshes encoder signal * @@ -119,7 +155,7 @@ public ShooterIOTalonFX() { */ @Override public void refreshData() { - BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition); + BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition, hoodMotorCurrent, hoodMotorVoltage); // double current = hoodAngle.getValueAsDouble(); @@ -138,9 +174,12 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.motorRPS = this.motorVelocity.getValueAsDouble(); inputs.hoodAngle = this.hoodAngle.getValueAsDouble() * 30.0 + 15.0; // convert rotations to degrees inputs.hoodMotorPosition = this.hoodAngle.getValueAsDouble(); + inputs.hoodMotorCurrent = this.hoodMotorCurrent.getValueAsDouble(); inputs.flywheelSetPointRPS = this.flywheelRPSSetPoint; inputs.hoodSetPointAngle = this.hoodAngleSetPoint; + inputs.isZeroing = this.isZeroing; + inputs.distanceTrim = this.distanceTrim; } /** diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java index e94ec08..a356480 100644 --- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -9,5 +9,11 @@ public class ShotData { static { distanceToRPM.put(2.7, 1900.0); distanceToHoodAngle.put(2.7, 30.0); + + distanceToRPM.put(3.0, 2000.0); + distanceToHoodAngle.put(3.0, 30.0); + + distanceToRPM.put(4.0, 2400.0); + distanceToHoodAngle.put(4.0, 30.0); } } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 8d3fcb0..7052db2 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -97,8 +97,12 @@ protected Runnable setupDataRefresher() { @AutoLogOutput(key="Turret/IsAtTargetAngle") public boolean isAtTargetAngle() { double error = - Math.abs(io.turretAngleDegreesFieldRelative - io.targetTurretDegrees); + Math.abs(io.turretAngleDegreesRobotRelative - io.processedTargetTurretDegrees); return error <= TURRET_AIMING_TOLERANCE_DEGREES; } + + public void changeTrim(double deltaDegrees) { + turretIO.changeTurretTrim(deltaDegrees); + } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index fae6535..a2b232b 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -19,7 +19,8 @@ public static class TurretIOInputs extends BaseInputClass { public double turretMotorPositionRotations = 0.0; // current position of the turret motor, in rotations public double pinionEncoderRotations = 0.0; // current rotations of the pinion encoder public double followerEncoderRotations = 0.0; // current rotations of the follower - public double rawTurretMechanismRotations = 0.0; // raw rotations of the entire turret mechanism + public double rawTurretMechanismRotations = 0.0; // raw rotations of the entire turret mechanism + public double turretTrimDegrees = 0.0; // minor adjustment to the turret angle based on operator controller input, in degrees } /** @@ -32,4 +33,10 @@ public static class TurretIOInputs extends BaseInputClass { * Recalculates the turret motor zero position to fix any encoder drift. */ void recalculateTurretMotorZeroPosition(); + + /** + * Changes the turret trim by a certain amount of degrees. This is used to make minor adjustments to the turret angle based on operator controller input. + * @param deltaDegrees the amount of degrees to change the turret trim by, in degrees. Positive values will adjust the turret angle clockwise, and negative values will adjust the turret angle counterclockwise. + */ + void changeTurretTrim(double deltaDegrees); } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index e464d47..5ac1573 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -53,6 +53,8 @@ public class TurretIOTalonFX implements TurretIO { private final MotionMagicVoltage mmRequest = new MotionMagicVoltage(0.0); private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(kS, kV); // ks, kv + private double turretTrimDegrees = 0.0; + public TurretIOTalonFX(CommandSwerveDrivetrain drive) { // SUBSYSTEMS this.drive = drive; @@ -111,6 +113,7 @@ private void setTurretAngleRobotRelativeDegrees(double robotRelativeAngleDegrees } private void setTurretAngleTurretRelativeDegrees(double angleDegrees) { + angleDegrees += turretTrimDegrees; // add minor adjustment based on operator controller angleDegrees = wrap180(angleDegrees); double targetMotorRotations = angleDegrees / 180.0; this.targetTurretAngleMotorRevs = targetMotorRotations; @@ -160,6 +163,7 @@ public void updateInputs(TurretIOInputs inputs) { inputs.pinionEncoderRotations = this.pinionEncoderSignal.getValueAsDouble(); inputs.followerEncoderRotations = this.followerEncoderSignal.getValueAsDouble(); inputs.rawTurretMechanismRotations = this.getTurretPositionRevs(); + inputs.turretTrimDegrees = turretTrimDegrees; } // CONFIGURATIONS @@ -218,6 +222,10 @@ private SoftwareLimitSwitchConfigs getTurretSoftwareLimitConfigs() { return configs; } + public void changeTurretTrim(double deltaDegrees) { + this.turretTrimDegrees += deltaDegrees; + } + /** * Get the encoder configurations for the turret encoders. * @return the encoder configurations for the turret encoders From 0c225ad58cfac59d2141175c46a51133d6dd6ecf Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Sat, 14 Mar 2026 11:26:33 -0400 Subject: [PATCH 18/29] Assorted tuning --- src/main/java/frc/robot/RobotContainer.java | 50 +++++++++---------- .../frc/robot/subsystems/shooter/Shooter.java | 22 ++++++-- .../subsystems/shooter/ShooterIOTalonFX.java | 9 ++-- .../robot/subsystems/targeting/ShotData.java | 17 ++++++- .../subsystems/turret/TurretIOTalonFX.java | 4 +- 5 files changed, 64 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1aae1e5..baf3a5c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,8 +6,6 @@ import static edu.wpi.first.units.Units.*; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -32,7 +30,8 @@ public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second + // max angular velocity private final SendableChooser autoChooser; @@ -43,7 +42,7 @@ public class RobotContainer { private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(MaxSpeed); + // private final Telemetry logger = new Telemetry(MaxSpeed); private final CommandXboxController driverController = new SpikeController(0, 0.05); private final CommandXboxController operatorController = new SpikeController(1, 0.05); @@ -89,24 +88,24 @@ private void setupSwerveBindings() { // and Y is defined as to the left according to WPILib convention. drive.setDefaultCommand( // Drivetrain will execute this command periodically - drive.applyRequest(() -> - driveCmd.withVelocityX(-driverController.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) - ) - ); + drive.applyRequest(() -> driveCmd.withVelocityX(-driverController.getLeftY() * MaxSpeed) // Drive + // forward with + // negative Y + // (forward) + .withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(driverController.getRightX() * MaxAngularRate) // Drive counterclockwise + // with negative X (left) + )); // Idle while the robot is disabled. This ensures the configured - // neutral mode is applied to the drive motors while disabled. + // neutral mode is applied to the drive motors while disabled. final var idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled().whileTrue( - drive.applyRequest(() -> idle).ignoringDisable(true) - ); + drive.applyRequest(() -> idle).ignoringDisable(true)); driverController.a().whileTrue(drive.applyRequest(() -> brake)); - driverController.b().whileTrue(drive.applyRequest(() -> - point.withModuleDirection(new Rotation2d(-driverController.getLeftY(), -driverController.getLeftX())) - )); + driverController.b().whileTrue(drive.applyRequest(() -> point + .withModuleDirection(new Rotation2d(-driverController.getLeftY(), -driverController.getLeftX())))); // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. @@ -116,15 +115,15 @@ private void setupSwerveBindings() { driverController.start().and(driverController.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); // reset the field-centric heading on left bumper press - + driverController.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); // operatorController.y().onTrue(vision.runOnce(() -> { - // var estimatedPose = vision.getEstimatedPositionFromCameras(); - // if (estimatedPose != null) { - // Logger.recordOutput("Vision/SnapshotEstimate", estimatedPose); - // drive.resetPose(estimatedPose); - // } + // var estimatedPose = vision.getEstimatedPositionFromCameras(); + // if (estimatedPose != null) { + // Logger.recordOutput("Vision/SnapshotEstimate", estimatedPose); + // drive.resetPose(estimatedPose); + // } // })); } @@ -140,15 +139,14 @@ private void setupTargetingBindings() { private void setupShooterBindings() { // toggle shooter on right trigger hold - operatorController.rightTrigger() + driverController.rightTrigger() .onTrue(shooter.run(() -> shooter.setDriverRequestingShooting(true))) .onFalse(shooter.run(() -> shooter.setDriverRequestingShooting(false))); - operatorController.x().onTrue(shooter.runOnce(() -> shooter.zeroHood())); - operatorController.povUp().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(0.5))); - operatorController.povDown().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(-0.5))); + operatorController.povUp().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(0.1))); + operatorController.povDown().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(-0.1))); operatorController.povLeft().onTrue(turret.runOnce(() -> turret.changeTrim(2))); operatorController.povRight().onTrue(turret.runOnce(() -> turret.changeTrim(-2))); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 2dd3635..53a4624 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,11 +6,14 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.subsystem.SpikeSystem; +import frc.robot.subsystems.targeting.ShotData; import frc.robot.subsystems.targeting.Targeting; public class Shooter extends SpikeSystem { private static final double SHOOTER_READY_THRESHOLD_RPS = 2.0; // RPS threshold to consider the shooter ready + private boolean readFromData = true; + private ShooterIO shooterIO; private boolean driverRequestingShooting = false; // Whether the driver is currently requesting to shoot @@ -18,6 +21,7 @@ public Shooter() { super("Shooter", new ShooterIOInputsAutoLogged()); SmartDashboard.putNumber("TargetRPM", 0); SmartDashboard.putNumber("TargetHoodAngle", 0); + SmartDashboard.putBoolean("ReadFromData", true); } /** @@ -26,16 +30,28 @@ public Shooter() { @Override public void onPeriodic() { double distToTarget = getDistanceToTarget(); // distance in meters + readFromData = SmartDashboard.getBoolean("ReadFromData", true); // double targetRPM = ShotData.distanceToRPM.get(distToTarget); // double hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget); - double targetRPM = SmartDashboard.getNumber("TargetRPM", 0); - double hoodAngle = SmartDashboard.getNumber("TargetHoodAngle", 0); + double targetRPM = 0; + + if (readFromData) { + targetRPM = ShotData.distanceToRPM.get(distToTarget); + } else { + targetRPM = SmartDashboard.getNumber("TargetRPM", 0); + } + + double hoodAngle = 0; + if (readFromData) { + hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget); + } else { + hoodAngle = SmartDashboard.getNumber("TargetHoodAngle", 0); + } if (io.isZeroing) { shooterIO.runZeroingHood(); } else { - // compensate hood angle for rpm of flywheel lowering due to slowdowns shooterIO.setHoodAngle(hoodAngle); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 279a925..c98129f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; -import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; @@ -19,15 +18,13 @@ import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import frc.lib.subsystem.IORefresher; import frc.robot.CanID; -public class ShooterIOTalonFX implements IORefresher, ShooterIO { +public class ShooterIOTalonFX implements ShooterIO { private static final double HOOD_GEAR_RATIO = 1.0 / 1.0; // hood pulley teeth / motor pulley teeth (motor revs per // hood rev) private static final double SOFTWARE_LIMIT_SWITCH_CURRENT_THRESHOLD = 1.75; // amps at which we consider the hood to have hit a limit @@ -101,7 +98,7 @@ public ShooterIOTalonFX() { flywheelSlot0.kI = 0.0; flywheelSlot0.kD = 0.0; flywheelSlot0.kS = 0.225; - flywheelSlot0.kV = 0.125; + flywheelSlot0.kV = 0.133; // current limits changed from // 120, 70 to 80, 60 @@ -198,7 +195,7 @@ public void setFlywheelVelocity(double rps) { /** * Set the target hood angle * - * @param angle target angle TODO: relative to what + * @param angle target angle */ @Override public void setHoodAngle(double angle) { diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java index a356480..111cff4 100644 --- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -7,13 +7,28 @@ public class ShotData { public static final InterpolatingDoubleTreeMap distanceToHoodAngle = new InterpolatingDoubleTreeMap(); static { + distanceToRPM.put(1.5, 1900.0); + distanceToHoodAngle.put(1.5, 15.0); + + distanceToRPM.put(2.0, 2000.0); + distanceToHoodAngle.put(2.0, 17.0); + distanceToRPM.put(2.7, 1900.0); distanceToHoodAngle.put(2.7, 30.0); distanceToRPM.put(3.0, 2000.0); distanceToHoodAngle.put(3.0, 30.0); - distanceToRPM.put(4.0, 2400.0); + distanceToRPM.put(3.6, 2100.0); + distanceToHoodAngle.put(3.6, 30.0); + + distanceToRPM.put(4.0, 2250.0); distanceToHoodAngle.put(4.0, 30.0); + + distanceToRPM.put(5.5, 2400.0); + distanceToHoodAngle.put(5.5, 40.0); + + distanceToRPM.put(7.5, 3000.0); + distanceToHoodAngle.put(7.5, 40.0); } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 5ac1573..75430a0 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -103,7 +103,7 @@ public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) // absolute robot-relative target, in motor rotations double targetRobotRelativeDeg = fieldRelativeAngleDegrees - currentRobotHeading; - this.processedTargetTurretDegreesFieldRelative = wrap180(targetRobotRelativeDeg); + this.processedTargetTurretDegreesFieldRelative = wrap180(targetRobotRelativeDeg + turretTrimDegrees); setTurretAngleRobotRelativeDegrees(targetRobotRelativeDeg); } @@ -113,7 +113,7 @@ private void setTurretAngleRobotRelativeDegrees(double robotRelativeAngleDegrees } private void setTurretAngleTurretRelativeDegrees(double angleDegrees) { - angleDegrees += turretTrimDegrees; // add minor adjustment based on operator controller + angleDegrees += turretTrimDegrees; angleDegrees = wrap180(angleDegrees); double targetMotorRotations = angleDegrees / 180.0; this.targetTurretAngleMotorRevs = targetMotorRotations; From b96a58b50ab055b0e9ee6a661b8e7645bf8efd29 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Sat, 14 Mar 2026 16:50:20 -0400 Subject: [PATCH 19/29] Work to reduce memory and general aiming tweaks --- build.gradle | 9 ++++- .../deploy/pathplanner/paths/New Path.path | 24 ++++++++++--- src/main/java/frc/robot/Robot.java | 9 ++--- .../frc/robot/subsystems/shooter/Shooter.java | 14 ++++---- .../robot/subsystems/shooter/ShooterIO.java | 2 +- .../subsystems/shooter/ShooterIOTalonFX.java | 36 ++++++++++++++----- .../subsystems/trigger/TriggerIOTalonFX.java | 4 +++ .../frc/robot/subsystems/turret/Turret.java | 19 ---------- .../subsystems/turret/TurretIOTalonFX.java | 4 ++- .../frc/robot/subsystems/vision/Vision.java | 4 --- 10 files changed, 77 insertions(+), 48 deletions(-) diff --git a/build.gradle b/build.gradle index ddf0ffe..c767c7a 100644 --- a/build.gradle +++ b/build.gradle @@ -29,7 +29,14 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - } + // Enable VisualVM connection + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-Djava.rmi.server.hostname=10.2.93.2") + } // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 473a347..a192fed 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.5437831125827817, - "y": 7.367309602649005 + "x": 7.202789735099339, + "y": 5.683004966887417 }, "prevControl": null, "nextControl": { - "x": 4.110057947019868, - "y": 6.641316225165561 + "x": 7.769064569536425, + "y": 4.957011589403972 }, "isLocked": false, "linkedName": null @@ -23,6 +23,22 @@ "x": -0.2149917218543047, "y": 5.937102649006622 }, + "nextControl": { + "x": 1.7850082781456953, + "y": 5.937102649006622 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.698112582781457, + "y": 6.975273178807948 + }, + "prevControl": { + "x": 2.7660347682119206, + "y": 7.229370860927153 + }, "nextControl": null, "isLocked": false, "linkedName": null diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 21b3fb2..25878c4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,6 +14,7 @@ package frc.robot; import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -35,7 +36,7 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ -public class Robot extends LoggedRobot { +public class Robot extends TimedRobot { private Command autonomousCommand; private RobotContainer robotContainer; @@ -56,7 +57,7 @@ public class Robot extends LoggedRobot { @Override public void robotInit() { // Setup log directory and free up space if needed - SetupLog(); + // SetupLog(); //Pathfinding.setPathfinder(new LocalADStarAK()); // Record metadata @@ -92,7 +93,7 @@ public void robotInit() { case REPLAY: // Replaying a log, set up replay source - setUseTiming(false); // Run as fast as possible + // setUseTiming(false); // Run as fast as possible String logPath = LogFileUtil.findReplayLog(); Logger.setReplaySource(new WPILOGReader(logPath)); Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); @@ -103,7 +104,7 @@ public void robotInit() { // Logger.disableDeterministicTimestamps() // Start AdvantageKit logger - Logger.start(); + // Logger.start(); // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 53a4624..9f2ddd1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -19,9 +19,9 @@ public class Shooter extends SpikeSystem { public Shooter() { super("Shooter", new ShooterIOInputsAutoLogged()); - SmartDashboard.putNumber("TargetRPM", 0); - SmartDashboard.putNumber("TargetHoodAngle", 0); - SmartDashboard.putBoolean("ReadFromData", true); + // SmartDashboard.putNumber("TargetRPM", 0); + // SmartDashboard.putNumber("TargetHoodAngle", 0); + // SmartDashboard.putBoolean("ReadFromData", true); } /** @@ -39,14 +39,14 @@ public void onPeriodic() { if (readFromData) { targetRPM = ShotData.distanceToRPM.get(distToTarget); } else { - targetRPM = SmartDashboard.getNumber("TargetRPM", 0); + // targetRPM = SmartDashboard.getNumber("TargetRPM", 0); } double hoodAngle = 0; if (readFromData) { hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget); } else { - hoodAngle = SmartDashboard.getNumber("TargetHoodAngle", 0); + // hoodAngle = SmartDashboard.getNumber("TargetHoodAngle", 0); } if (io.isZeroing) { @@ -55,7 +55,9 @@ public void onPeriodic() { shooterIO.setHoodAngle(hoodAngle); } - shooterIO.setFlywheelVelocity(targetRPM / 60.0); // convert RPM to RPS + // put to recovery mode if the driver is requesting to shoot + // more direct control rather than smooth trajectory generation + shooterIO.setFlywheelVelocity(targetRPM / 60.0, driverRequestingShooting); // convert RPM to RPS } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index d6e4f7f..fea2b8e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -18,7 +18,7 @@ public static class ShooterIOInputs extends BaseInputClass { public boolean isZeroing = false; } - void setFlywheelVelocity(double rps); + void setFlywheelVelocity(double rps, boolean isRecovery); void setHoodAngle(double angle); void runZeroingHood(); void zeroHood(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index c98129f..fc5c23b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.shooter; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -9,7 +11,7 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -18,6 +20,7 @@ import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -33,7 +36,9 @@ public class ShooterIOTalonFX implements ShooterIO { private final TalonFXS hoodMotor; private final CANcoder hoodEncoder; // using wcp throughbore; you interface through 'CANcoder' class - private final VelocityVoltage flywheelVelocityControl = new VelocityVoltage(0); + private final VelocityTorqueCurrentFOC flywheelSetPointControl = new VelocityTorqueCurrentFOC(0); + private final VelocityTorqueCurrentFOC flywheelRecoveryControl = new VelocityTorqueCurrentFOC(0); + private final PositionVoltage hoodPositionControl = new PositionVoltage(0); private final StatusSignal hoodMotorVoltage; // volts private final StatusSignal motorVelocity; // rps @@ -94,11 +99,11 @@ public ShooterIOTalonFX() { // flywheel configs var flywheelSlot0 = new Slot0Configs(); - flywheelSlot0.kP = 0.15; + flywheelSlot0.kP = 20; flywheelSlot0.kI = 0.0; flywheelSlot0.kD = 0.0; - flywheelSlot0.kS = 0.225; - flywheelSlot0.kV = 0.133; + flywheelSlot0.kS = 0.25; + flywheelSlot0.kV = 0.75; // current limits changed from // 120, 70 to 80, 60 @@ -185,11 +190,26 @@ public void updateInputs(ShooterIOInputs inputs) { * @param rps - target rotations per second */ @Override - public void setFlywheelVelocity(double rps) { + public void setFlywheelVelocity(double rps, boolean isRecovery) { this.flywheelRPSSetPoint = rps; - this.flywheelVelocityControl.withVelocity(rps); + if (isRecovery) { + this.flywheelRecoveryControl.withVelocity(rps); + + // calculate the error from set point to current + double error = rps - this.motorVelocity.getValueAsDouble(); + double feedForwardConstantBoost = 6; + double ffBost = MathUtil.inputModulus((0.133 * error) + feedForwardConstantBoost, 0.0, 10); // simple proportional feedforward based on velocity error + Logger.recordOutput("Shooter/FeedForwardBoost", ffBost); + + if (error >= 1) { + this.flywheelRecoveryControl.withFeedForward(ffBost); + } - flywheelMotor.setControl(this.flywheelVelocityControl); + flywheelMotor.setControl(this.flywheelRecoveryControl); + } else { + this.flywheelSetPointControl.withVelocity(rps); + flywheelMotor.setControl(this.flywheelSetPointControl); + } } /** diff --git a/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java b/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java index 65441c0..5ce7990 100644 --- a/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java @@ -15,6 +15,10 @@ public TriggerIOTalonFX(int proxChannel) { this.motor = new TalonFX(CanID.TRIGGER_MOTOR.getID()); this.motorRps = motor.getRotorVelocity(); this.proximitySensor = new DigitalInput(proxChannel); + + motorRps.setUpdateFrequency(50); + + motor.optimizeBusUtilization(); } // Refresh all signals diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 7052db2..4de857e 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -54,25 +54,6 @@ public void onPeriodic() { // } this.turretIO.setTurretAngleFieldRelativeDegrees(getTurretAngleDegreesFieldRelative()); - - Pose2d robotPose = RobotContainer.getDrive().getPose(); - Pose2d turretTranslatedPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Math.toRadians(io.turretAngleDegreesFieldRelative))); - Logger.recordOutput("Turret/RobotTurretPose", turretTranslatedPose); - - Translation2d turretPivot = TURRET_OFFSET_FROM_CENTER - .rotateBy(robotPose.getRotation()) - .plus(robotPose.getTranslation()); - Translation2d hub = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); - Translation2d aimVec = hub.minus(turretPivot); - Rotation2d aimAngle = aimVec.getAngle(); - - final int NUM_POINTS = 6; - Pose2d[] aimingRay = new Pose2d[NUM_POINTS]; - for (int i = 0; i < NUM_POINTS; i++) { - double t = (double) i / (NUM_POINTS - 1); - aimingRay[i] = new Pose2d(turretPivot.plus(aimVec.times(t)), aimAngle); - } - Logger.recordOutput("Turret/AimingRay", aimingRay); } public double getTurretAngleDegreesFieldRelative() { diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 75430a0..1ea2513 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -117,8 +117,10 @@ private void setTurretAngleTurretRelativeDegrees(double angleDegrees) { angleDegrees = wrap180(angleDegrees); double targetMotorRotations = angleDegrees / 180.0; this.targetTurretAngleMotorRevs = targetMotorRotations; + + mmRequest.Position = targetMotorRotations; this.turretMotor.setControl( - mmRequest.withPosition(targetMotorRotations) + mmRequest ); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 3a669f1..f19331b 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -23,13 +23,11 @@ public Vision(CommandSwerveDrivetrain drive) { @Override public void onPeriodic() { - int index = 0; // update drive with vision measurements for (EstimatedRobotPose pose : visionIO.getEstimatedRobotPoses()) { if (pose == null) { continue; } - Logger.recordOutput("EstimatedPose/" + index, pose.estimatedPose.toPose2d()); double avgDist = 0; // calculate the average distance to the targets @@ -41,7 +39,6 @@ public void onPeriodic() { } avgDist = totalDist / pose.targetsUsed.size(); - Logger.recordOutput("EstimatedPose/" + index + "/AvgTargetDist", avgDist); } drive.addVisionMeasurement( @@ -49,7 +46,6 @@ public void onPeriodic() { pose.timestampSeconds, CommandSwerveDrivetrain.kDefaultVisionStdDevs.times(1 + ((avgDist * avgDist) / 30)) ); - index++; } } From 09caf97a701b218adcb06c21081dbf256af19c08 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Fri, 20 Mar 2026 15:33:19 -0400 Subject: [PATCH 20/29] Update CAN IDs for mechanisms --- src/main/java/frc/robot/CanID.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/CanID.java b/src/main/java/frc/robot/CanID.java index 57b570c..39c7367 100644 --- a/src/main/java/frc/robot/CanID.java +++ b/src/main/java/frc/robot/CanID.java @@ -12,11 +12,11 @@ public enum CanID { TURRET_MOTOR(0), - INTAKE_MOTOR(6), + INTAKE_MOTOR(41), INTAKE_DEPLOY_MOTOR(11), - TURRET_PINION_CANCODER(19), - TURRET_FOLLOWER_CANCODER(20), + TURRET_PINION_CANCODER(20), + TURRET_FOLLOWER_CANCODER(19), FINDEXER_MOTOR(18); From 8ea39155cd762eca80958d313b706e8f5783d55f Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Fri, 20 Mar 2026 15:33:34 -0400 Subject: [PATCH 21/29] Update logging --- src/main/java/frc/robot/Robot.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 25878c4..3d4389b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -36,7 +36,7 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; @@ -57,7 +57,7 @@ public class Robot extends TimedRobot { @Override public void robotInit() { // Setup log directory and free up space if needed - // SetupLog(); + SetupLog(); //Pathfinding.setPathfinder(new LocalADStarAK()); // Record metadata @@ -104,7 +104,7 @@ public void robotInit() { // Logger.disableDeterministicTimestamps() // Start AdvantageKit logger - // Logger.start(); + Logger.start(); // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. From 6c24de8ba2c8035e0ef4410887d29eb57801db9d Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Fri, 20 Mar 2026 15:48:30 -0400 Subject: [PATCH 22/29] Tune components, add backups for mechanisms, update driver/operator controls --- src/main/java/frc/robot/RobotContainer.java | 31 ++-- .../frc/robot/generated/TunerConstants.java | 65 +++++---- .../findexer/FindexerIOTalonFX.java | 9 +- .../subsystems/intake/IntakeIOTalonFX.java | 34 +++-- .../frc/robot/subsystems/shooter/Shooter.java | 38 +++-- .../robot/subsystems/shooter/ShooterIO.java | 19 ++- .../subsystems/shooter/ShooterIOTalonFX.java | 138 ++++++------------ .../robot/subsystems/targeting/ShotData.java | 35 +++-- .../robot/subsystems/targeting/Targeting.java | 32 +++- .../frc/robot/subsystems/trigger/Trigger.java | 19 ++- .../frc/robot/subsystems/turret/Turret.java | 14 +- .../subsystems/turret/TurretIOTalonFX.java | 22 +-- .../vision/photon/CameraManager.java | 57 +++----- 13 files changed, 269 insertions(+), 244 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index baf3a5c..193aeb6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,21 +50,21 @@ public class RobotContainer { public static CommandSwerveDrivetrain drive; private final Vision vision; private final Turret turret; - // private final Intake intake; + private final Intake intake; private final Trigger trigger; private final Shooter shooter; - // private final Targeting targeting; - // private final Findexer findexer; + private final Targeting targeting; + private final Findexer findexer; public RobotContainer() { drive = TunerConstants.createDrivetrain(); this.turret = new Turret(); this.vision = new Vision(drive); - // this.intake = new Intake(drive); + this.intake = new Intake(drive); this.shooter = new Shooter(); - // this.targeting = new Targeting(drive); + this.targeting = new Targeting(drive); this.trigger = new Trigger(shooter, turret); - // this.findexer = new Findexer(trigger); + this.findexer = new Findexer(trigger); autoChooser = drive.getAutoChooser(); SmartDashboard.putData("Auto Path", autoChooser); @@ -93,7 +93,7 @@ private void setupSwerveBindings() { // negative Y // (forward) .withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(driverController.getRightX() * MaxAngularRate) // Drive counterclockwise + .withRotationalRate(-driverController.getRightX() * MaxAngularRate) // Drive counterclockwise // with negative X (left) )); @@ -128,13 +128,14 @@ private void setupSwerveBindings() { } private void setupIntakeBindings() { - // toggle intake on B press - // operatorController.b().onTrue(intake.run(intake::toggleIntake)); + // toggle intake on A press + // operatorController.a().onTrue(intake.run(intake::toggleIntake)); } private void setupTargetingBindings() { - // operatorController.rightBumper().onTrue(targeting.run(targeting::setTargetingHub)); - // operatorController.leftBumper().onTrue(targeting.run(targeting::setTargetingShuttle)); + operatorController.y().onTrue(targeting.run(targeting::setTargetingHub)); + operatorController.x().onTrue(targeting.runOnce(targeting::setTargetingShuttleLeft)); + operatorController.b().onTrue(targeting.runOnce(targeting::setTargetingShuttleRight)); } private void setupShooterBindings() { @@ -142,8 +143,14 @@ private void setupShooterBindings() { driverController.rightTrigger() .onTrue(shooter.run(() -> shooter.setDriverRequestingShooting(true))) .onFalse(shooter.run(() -> shooter.setDriverRequestingShooting(false))); + driverController.leftTrigger() + .onTrue(shooter.run(() -> shooter.setRequestingWithForce(true))) + .onFalse(shooter.run(() -> shooter.setRequestingWithForce(false))); - operatorController.x().onTrue(shooter.runOnce(() -> shooter.zeroHood())); + operatorController.rightStick().onTrue(shooter.runOnce(() -> shooter.zeroHood())); + + operatorController.leftStick().onTrue(trigger.run(() -> trigger.setReverseTrigger(true))); + operatorController.leftStick().onFalse(trigger.run(() -> trigger.setReverseTrigger(false))); operatorController.povUp().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(0.1))); operatorController.povDown().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(-0.1))); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 1693682..273bd0c 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; + import frc.robot.subsystems.drive.CommandSwerveDrivetrain; // Generated by the 2026 Tuner X Swerve Project Generator @@ -23,7 +24,7 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(15).withKI(0).withKD(0.5) + .withKP(100).withKI(0).withKD(0.5) .withKS(0.1).withKV(1.59).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control @@ -73,7 +74,7 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(3.79); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(3.70); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot @@ -81,12 +82,12 @@ public class TunerConstants { private static final double kDriveGearRatio = 8.142857142857142; private static final double kSteerGearRatio = 12.8; - private static final Distance kWheelRadius = Inches.of(2); + private static final Distance kWheelRadius = Inches.of(1.95); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 0; + private static final int kPigeonId = 62; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -124,49 +125,49 @@ public class TunerConstants { .withDriveFrictionVoltage(kDriveFrictionVoltage); - // Front Left (was Back Right) - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 24; - private static final int kFrontLeftEncoderId = 5; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.344482421875); + // Front Left + private static final int kFrontLeftDriveMotorId = 4; + private static final int kFrontLeftSteerMotorId = 5; + private static final int kFrontLeftEncoderId = 6; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.327880859375); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(-11.5); - private static final Distance kFrontLeftYPos = Inches.of(-11.5); + private static final Distance kFrontLeftXPos = Inches.of(10.25); + private static final Distance kFrontLeftYPos = Inches.of(10.25); - // Front Right (was Back Left) - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 30; - private static final int kFrontRightEncoderId = 22; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.20166015625); + // Front Right + private static final int kFrontRightDriveMotorId = 2; + private static final int kFrontRightSteerMotorId = 1; + private static final int kFrontRightEncoderId = 3; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.484619140625); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(-11.5); - private static final Distance kFrontRightYPos = Inches.of(11.5); + private static final Distance kFrontRightXPos = Inches.of(10.25); + private static final Distance kFrontRightYPos = Inches.of(-10.25); - // Back Left (was Front Right) - private static final int kBackLeftDriveMotorId = 61; - private static final int kBackLeftSteerMotorId = 27; - private static final int kBackLeftEncoderId = 25; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.306396484375); + // Back Left + private static final int kBackLeftDriveMotorId = 8; + private static final int kBackLeftSteerMotorId = 7; + private static final int kBackLeftEncoderId = 9; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.052001953125); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(11.5); - private static final Distance kBackLeftYPos = Inches.of(-11.5); + private static final Distance kBackLeftXPos = Inches.of(-10.25); + private static final Distance kBackLeftYPos = Inches.of(10.25); - // Back Right (was Front Left) - private static final int kBackRightDriveMotorId = 0; - private static final int kBackRightSteerMotorId = 50; - private static final int kBackRightEncoderId = 31; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.463134765625); + // Back Right + private static final int kBackRightDriveMotorId = 11; + private static final int kBackRightSteerMotorId = 10; + private static final int kBackRightEncoderId = 12; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.497314453125); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(11.5); - private static final Distance kBackRightYPos = Inches.of(11.5); + private static final Distance kBackRightXPos = Inches.of(-10.25); + private static final Distance kBackRightYPos = Inches.of(-10.25); public static final SwerveModuleConstants FrontLeft = diff --git a/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java index 495c9cd..f4b2c40 100644 --- a/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/findexer/FindexerIOTalonFX.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.CanID; @@ -10,17 +11,20 @@ public class FindexerIOTalonFX implements FindexerIO { private final TalonFX motor; + private final VelocityVoltage velocityControl = new VelocityVoltage(0.0); private final StatusSignal motorRps; // Rotations per second public FindexerIOTalonFX() { this.motor = new TalonFX(CanID.FINDEXER_MOTOR.getID()); this.motorRps = motor.getVelocity(); Slot0Configs config = new Slot0Configs(); - config.kP = 0.5; + config.kP = 0.1; config.kI = 0.0; config.kD = 0.0; config.kS = 0.0; + config.kV = 0.1; + this.motor.getConfigurator().apply(config); this.motor.optimizeBusUtilization(); @@ -32,7 +36,8 @@ public FindexerIOTalonFX() { */ @Override public void setSpeed(double rps) { - this.motor.set(rps); + this.velocityControl.Velocity = rps; + this.motor.setControl(velocityControl); } /** diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java index 45a48d3..45b7438 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java @@ -3,7 +3,9 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.AngularAcceleration; @@ -17,48 +19,50 @@ public class IntakeIOTalonFX implements IntakeIO, IORefresher { // TalonFX Motors private final TalonFX intakeMotor; - private final TalonFX deployMotor; + // private final TalonFX deployMotor; + + private final VelocityVoltage velocityControl = new VelocityVoltage(0.0); // Status Signals private final StatusSignal intakeVelocity; private final StatusSignal intakeCurrent; - private final StatusSignal deployVelocity; - private final StatusSignal deployCurrent; + // private final StatusSignal deployVelocity; + // private final StatusSignal deployCurrent; // Inputs for logging private IntakeIOInputs intakeIO; // IntakeIOTalonFX constructor public IntakeIOTalonFX() { - intakeMotor = new TalonFX(CanID.INTAKE_MOTOR.getID()); // Setup the intake motor with the CAN ID - deployMotor = new TalonFX(CanID.INTAKE_DEPLOY_MOTOR.getID()); // Set up deploy motor with the CAN ID + intakeMotor = new TalonFX(CanID.INTAKE_MOTOR.getID(), "Canivore_Drivetrain"); // Setup the intake motor with the CAN ID + // deployMotor = new TalonFX(CanID.INTAKE_DEPLOY_MOTOR.getID()); // Set up deploy motor with the CAN ID // Configure motors - deployMotor.getConfigurator().apply(getDeployMotorConfig()); + // deployMotor.getConfigurator().apply(getDeployMotorConfig()); intakeMotor.getConfigurator().apply(getIntakeMotorConfig()); // Configure motor signals intakeVelocity = intakeMotor.getVelocity(); intakeCurrent = intakeMotor.getStatorCurrent(); - deployVelocity = deployMotor.getVelocity(); - deployCurrent = deployMotor.getStatorCurrent(); + // deployVelocity = deployMotor.getVelocity(); + // deployCurrent = deployMotor.getStatorCurrent(); intakeMotor.optimizeBusUtilization(); - deployMotor.optimizeBusUtilization(); + // deployMotor.optimizeBusUtilization(); } // Fetches data from the motors @Override public void refreshData() { - BaseStatusSignal.refreshAll(intakeVelocity, intakeCurrent, deployVelocity, deployCurrent); + BaseStatusSignal.refreshAll(intakeVelocity, intakeCurrent); } @Override public void updateInputs(IntakeIOInputs inputs) { inputs.intakeVelocityRPS = intakeVelocity.getValueAsDouble(); inputs.intakeCurrentAmps = intakeCurrent.getValueAsDouble(); - inputs.deployVelocityRPS = deployVelocity.getValueAsDouble(); - inputs.deployCurrentAmps = deployCurrent.getValueAsDouble(); + // inputs.deployVelocityRPS = deployVelocity.getValueAsDouble(); + // inputs.deployCurrentAmps = deployCurrent.getValueAsDouble(); intakeIO = inputs; } @@ -73,7 +77,8 @@ public void setIntakeSpeed(double speed) { // double speed - Speed to set the motor to in Rotations Per Second @Override public void setDeploySpeed(double speed) { - deployMotor.set(speed); + this.velocityControl.withVelocity(speed); + // deployMotor.setControl(this.velocityControl); } // Return the state of the Intake @@ -99,9 +104,10 @@ public static TalonFXConfiguration getIntakeMotorConfig() { // Set Intake motor to Coast when not on intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + intakeConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // Intake motor PID values - intakeConfig.Slot0.kP = 0.1; + intakeConfig.Slot0.kP = 1; intakeConfig.Slot0.kI = 0.0; intakeConfig.Slot0.kD = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9f2ddd1..3d1808b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -16,12 +16,13 @@ public class Shooter extends SpikeSystem { private ShooterIO shooterIO; private boolean driverRequestingShooting = false; // Whether the driver is currently requesting to shoot + private boolean requestingWithForce = false; // Whether the driver is requesting to shoot with force, which bypasses the normal checks for whether the shooter is ready and just runs the flywheel and hood at the target values public Shooter() { super("Shooter", new ShooterIOInputsAutoLogged()); - // SmartDashboard.putNumber("TargetRPM", 0); - // SmartDashboard.putNumber("TargetHoodAngle", 0); - // SmartDashboard.putBoolean("ReadFromData", true); + SmartDashboard.putNumber("TargetRPM", 0); + SmartDashboard.putNumber("TargetHoodAngle", 0); + SmartDashboard.putBoolean("ReadFromData", true); } /** @@ -39,25 +40,29 @@ public void onPeriodic() { if (readFromData) { targetRPM = ShotData.distanceToRPM.get(distToTarget); } else { - // targetRPM = SmartDashboard.getNumber("TargetRPM", 0); + targetRPM = SmartDashboard.getNumber("TargetRPM", 0); } double hoodAngle = 0; if (readFromData) { hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget); } else { - // hoodAngle = SmartDashboard.getNumber("TargetHoodAngle", 0); + hoodAngle = SmartDashboard.getNumber("TargetHoodAngle", 0); } if (io.isZeroing) { shooterIO.runZeroingHood(); } else { - shooterIO.setHoodAngle(hoodAngle); + if (driverRequestingShooting) { + shooterIO.setHoodAngle(hoodAngle); + } else { + shooterIO.setHoodAngle(15); // set hood to default position when not shooting + } } // put to recovery mode if the driver is requesting to shoot // more direct control rather than smooth trajectory generation - shooterIO.setFlywheelVelocity(targetRPM / 60.0, driverRequestingShooting); // convert RPM to RPS + shooterIO.setFlywheelVelocity(targetRPM / 60.0); // convert RPM to RPS } /** @@ -75,7 +80,7 @@ protected Runnable setupDataRefresher() { */ @AutoLogOutput(key="Shooter/IsAtTargetRPS") public boolean isAtTargetRPS() { - return Math.abs((super.io.motorRPS - 0.5) - io.flywheelSetPointRPS) < SHOOTER_READY_THRESHOLD_RPS; + return Math.abs((super.io.flywheelVelocityRPS - 0.5) - io.flywheelSetPointRPS) < SHOOTER_READY_THRESHOLD_RPS; } /** @@ -85,7 +90,7 @@ public boolean isAtTargetRPS() { public double getDistanceToTarget() { Translation2d toGoal = Targeting.differenceBetweenRobotAndTarget(); Logger.recordOutput("Targeting/DistanceToTarget", toGoal.getNorm()); - return toGoal.getNorm() + io.distanceTrim; // add distance trim to adjust the distance based on operator controller input + return toGoal.getNorm() + io.distanceTrimMeters; // add distance trim to adjust the distance based on operator controller input } /** @@ -96,6 +101,10 @@ public void setDriverRequestingShooting(boolean isRequesting) { this.driverRequestingShooting = isRequesting; } + public void setRequestingWithForce(boolean isRequestingWithForce) { + this.requestingWithForce = isRequestingWithForce; + } + public void zeroHood() { shooterIO.zeroHood(); } @@ -104,10 +113,19 @@ public void zeroHood() { * Returns whether the driver is currently requesting to shoot. * @return true if the driver is requesting to shoot, false otherwise */ - public boolean isDriverRequestingShooting() { + public boolean isShootingRequested() { return this.driverRequestingShooting; } + public boolean isRequestingWithForce() { + return this.requestingWithForce; + } + + /** + * Changes the distance trim by a certain amount of meters. This is used to make minor adjustments to the distance based on operator controller input. + * @param deltaDistance the amount of meters to change the distance trim by. Positive values add to the distance, and negative values subtract from the distance. + */ + public void changeDistanceTrim(double deltaDistance) { shooterIO.changeDistanceTrim(deltaDistance); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index fea2b8e..ed951bd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -8,19 +8,18 @@ public interface ShooterIO extends BaseIO, IORefresher { @AutoLog public static class ShooterIOInputs extends BaseInputClass { - public double motorRPS = 0.0; // Motor rotations per second - public double hoodAngle = 15; // Hood angle in degrees from - public double hoodMotorPosition = 0.0; // TODO: position is in relation to what? - public double flywheelSetPointRPS = 0.0; // Flywheel set point rotations per second - public double hoodSetPointAngle = 0.0; // TODO: Hood setpoint in relation to what? - public double hoodMotorCurrent = 0.0; // Current being drawn by the hood motor - public double distanceTrim = 0.0; // minor adjustment to the returned distance based on operator controller input, in degrees - public boolean isZeroing = false; + public double flywheelVelocityRPS = 0.0; // measured flywheel speed, in rotations per second + public double hoodAngleDeg = 15.0; // measured hood angle, in degrees [15, 45] + public double flywheelSetPointRPS = 0.0; // commanded flywheel speed setpoint, in rotations per second + public double hoodAngleSetPointDeg = 0.0; // commanded hood angle setpoint, in degrees [15, 45] + public double hoodMotorCurrentAmps = 0.0; // measured hood motor supply current, in amps + public double distanceTrimMeters = 0.0; // operator fine-trim added to the target distance, in meters + public boolean isZeroing = false; // true while the hood is running its zeroing routine } - void setFlywheelVelocity(double rps, boolean isRecovery); + void setFlywheelVelocity(double rps); void setHoodAngle(double angle); void runZeroingHood(); void zeroHood(); void changeDistanceTrim(double deltaDistance); -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index fc5c23b..8d55030 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -1,15 +1,9 @@ package frc.robot.subsystems.shooter; -import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CommutationConfigs; -import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; @@ -20,7 +14,6 @@ import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -28,17 +21,17 @@ import frc.robot.CanID; public class ShooterIOTalonFX implements ShooterIO { - private static final double HOOD_GEAR_RATIO = 1.0 / 1.0; // hood pulley teeth / motor pulley teeth (motor revs per - // hood rev) - private static final double SOFTWARE_LIMIT_SWITCH_CURRENT_THRESHOLD = 1.75; // amps at which we consider the hood to have hit a limit + private static final double HOOD_ZERO_CURRENT = 1.75; // amps at which we consider the hood to have hit a limit + private static final double FLYWHEEL_SETPOINT_UPDATE_DEADBAND_RPS = 0.35; // ignore tiny target changes private final TalonFX flywheelMotor; private final TalonFXS hoodMotor; private final CANcoder hoodEncoder; // using wcp throughbore; you interface through 'CANcoder' class - private final VelocityTorqueCurrentFOC flywheelSetPointControl = new VelocityTorqueCurrentFOC(0); - private final VelocityTorqueCurrentFOC flywheelRecoveryControl = new VelocityTorqueCurrentFOC(0); - + private final VelocityTorqueCurrentFOC flywheelControl = new VelocityTorqueCurrentFOC(0); + + private final VoltageOut hoodZeroingControl = new VoltageOut(-4); + private final PositionVoltage hoodPositionControl = new PositionVoltage(0); private final StatusSignal hoodMotorVoltage; // volts private final StatusSignal motorVelocity; // rps @@ -48,6 +41,7 @@ public class ShooterIOTalonFX implements ShooterIO { private double hoodAngleSetPoint = 0.0; private double flywheelRPSSetPoint = 0.0; + private double lastAppliedFlywheelRPSSetPoint = Double.NaN; private double hoodTargetEncoder = 0.0; private boolean isZeroing = true; @@ -61,7 +55,7 @@ public ShooterIOTalonFX() { // hood encoder configs CANcoderConfiguration hoodEncoderConfig = new CANcoderConfiguration(); - hoodEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + hoodEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; this.hoodEncoder.getConfigurator().apply(hoodEncoderConfig); // apply default configs to encoder before // using it for feedback // constrains the range to [0, 1) @@ -97,16 +91,13 @@ public ShooterIOTalonFX() { this.hoodMotor.getConfigurator().apply(hoodCommutation); this.hoodMotor.getConfigurator().apply(hoodMotorOutputConfigs); - // flywheel configs + // flywheel configs (units in AMPS) var flywheelSlot0 = new Slot0Configs(); - flywheelSlot0.kP = 20; + flywheelSlot0.kP = 20; // amps / rps of error flywheelSlot0.kI = 0.0; flywheelSlot0.kD = 0.0; - flywheelSlot0.kS = 0.25; - flywheelSlot0.kV = 0.75; - - // current limits changed from - // 120, 70 to 80, 60 + flywheelSlot0.kS = 0.0; // amps needed to overcome static friction + flywheelSlot0.kV = 0.0; // not used for torque control this.flywheelMotor.getConfigurator().apply(flywheelSlot0); @@ -119,17 +110,15 @@ public ShooterIOTalonFX() { // force refresh before zero calculations BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition, hoodMotorVoltage, hoodMotorCurrent); - this.hoodEncoder.setPosition(0); - flywheelMotor.optimizeBusUtilization(); hoodMotor.optimizeBusUtilization(); } @Override public void runZeroingHood() { - hoodMotor.setControl(new VoltageOut(-4)); // move hood down at a slow speed + hoodMotor.setControl(hoodZeroingControl); // move hood down at a slow speed - if (hoodMotorCurrent.getValueAsDouble() > SOFTWARE_LIMIT_SWITCH_CURRENT_THRESHOLD) { // if we hit the floor, the current will spike up + if (hoodMotorCurrent.getValueAsDouble() > HOOD_ZERO_CURRENT) { // if we hit the floor, the current will spike up hoodMotor.stopMotor(); hoodEncoder.setPosition(0); // set encoder position to 0 when we hit the limit isZeroing = false; @@ -151,77 +140,60 @@ public void changeDistanceTrim(double deltaDistance) { } /** - * Periodically refreshes encoder signal - * - * @note This is called automatically + * Periodically refreshes encoder signals. + * Called automatically by the subsystem data refresher. */ @Override public void refreshData() { BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition, hoodMotorCurrent, hoodMotorVoltage); - - // double current = hoodAngle.getValueAsDouble(); - - // // if (Math.abs(current - hoodTargetEncoder) < 0.005) { - // // hoodMotor.stopMotor(); - // // } } /** * Periodically called to update the shooter information for logging - * + * * @param inputs ShooterIOInputs object to update */ @Override public void updateInputs(ShooterIOInputs inputs) { - inputs.motorRPS = this.motorVelocity.getValueAsDouble(); - inputs.hoodAngle = this.hoodAngle.getValueAsDouble() * 30.0 + 15.0; // convert rotations to degrees - inputs.hoodMotorPosition = this.hoodAngle.getValueAsDouble(); - inputs.hoodMotorCurrent = this.hoodMotorCurrent.getValueAsDouble(); + inputs.flywheelVelocityRPS = this.motorVelocity.getValueAsDouble(); + inputs.hoodAngleDeg = this.hoodAngle.getValueAsDouble() * 30.0 + 15.0; // convert [0,1] encoder rotations to degrees [15, 45] + inputs.hoodMotorCurrentAmps = this.hoodMotorCurrent.getValueAsDouble(); inputs.flywheelSetPointRPS = this.flywheelRPSSetPoint; - inputs.hoodSetPointAngle = this.hoodAngleSetPoint; + inputs.hoodAngleSetPointDeg = this.hoodAngleSetPoint; inputs.isZeroing = this.isZeroing; - inputs.distanceTrim = this.distanceTrim; + inputs.distanceTrimMeters = this.distanceTrim; } /** * Set the target flywheel velocity - * + * * @param rps - target rotations per second */ @Override - public void setFlywheelVelocity(double rps, boolean isRecovery) { + public void setFlywheelVelocity(double rps) { this.flywheelRPSSetPoint = rps; - if (isRecovery) { - this.flywheelRecoveryControl.withVelocity(rps); - - // calculate the error from set point to current - double error = rps - this.motorVelocity.getValueAsDouble(); - double feedForwardConstantBoost = 6; - double ffBost = MathUtil.inputModulus((0.133 * error) + feedForwardConstantBoost, 0.0, 10); // simple proportional feedforward based on velocity error - Logger.recordOutput("Shooter/FeedForwardBoost", ffBost); - - if (error >= 1) { - this.flywheelRecoveryControl.withFeedForward(ffBost); - } - - flywheelMotor.setControl(this.flywheelRecoveryControl); - } else { - this.flywheelSetPointControl.withVelocity(rps); - flywheelMotor.setControl(this.flywheelSetPointControl); - } - } + // boolean firstCommand = Double.isNaN(lastAppliedFlywheelRPSSetPoint); + // boolean meaningfulChange = firstCommand + // || (Math.abs(rps - lastAppliedFlywheelRPSSetPoint) >= FLYWHEEL_SETPOINT_UPDATE_DEADBAND_RPS); + + // if (!meaningfulChange) { + // return; + // } + + this.flywheelControl.withVelocity(rps); + flywheelMotor.setControl(this.flywheelControl); + this.lastAppliedFlywheelRPSSetPoint = rps; + } /** - * Set the target hood angle - * + * Set the target hood angle. * @param angle target angle */ - @Override public void setHoodAngle(double angle) { - angle = Math.max(15, Math.min(45, angle)); + angle = Math.max(15.0, Math.min(45.0, angle)); this.hoodAngleSetPoint = angle; - + angle = Math.max(15, Math.min(45, angle)); // convert target angle -> encoder rotations this.hoodTargetEncoder = angleToEncoder(angle); @@ -229,32 +201,16 @@ public void setHoodAngle(double angle) { } /** - * Converts angle in degrees to motor rotations per second - * - * @param angle Input angle in degrees - * @return double motor rotations per second + * Returns a position from zero to 1 representing the hood position, + * where 0 is 15 degrees and 1 is 45 degrees. + * @param angle angle in degrees, expected to be in the range [15, 45] + * @return normalized encoder position in the range [0, 1] */ - private static double angleToMotorRotations(double angle) { - // rotations = (angle_deg * gear_ratio) / 360 - return angle * HOOD_GEAR_RATIO / 360.0; - } - - /** - * Returns a position from zero to 1 representing the position of the hood, - * where 0 is 15 degrees and 1 is 45 degrees - * - * @param angle - * @return - */ - private static double angleToEncoder(double angle) { + private double angleToEncoder(double angle) { angle -= 14.0; // shift so that 0 is at 15 degrees angle /= 30.0; // scale so that 1 is at 45 degrees if (angle < 0.0) { return 0.0; - } else if (angle > 1.0) { - return 1.0; - } else { - return angle; - } + } else return Math.min(angle, 1.0); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java index 111cff4..c3930d2 100644 --- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -7,28 +7,37 @@ public class ShotData { public static final InterpolatingDoubleTreeMap distanceToHoodAngle = new InterpolatingDoubleTreeMap(); static { + distanceToRPM.put(3.14, 2200.0); + distanceToHoodAngle.put(3.14, 30.0); + + distanceToRPM.put(4.7, 2550.0); + distanceToHoodAngle.put(4.7, 36.0); + distanceToRPM.put(1.5, 1900.0); distanceToHoodAngle.put(1.5, 15.0); - distanceToRPM.put(2.0, 2000.0); - distanceToHoodAngle.put(2.0, 17.0); + distanceToRPM.put(2.5, 2300.0); + distanceToHoodAngle.put(2.5, 25.0); + + // distanceToRPM.put(2.96, 2300.0); + // distanceToHoodAngle.put(2.96, 30.0); - distanceToRPM.put(2.7, 1900.0); - distanceToHoodAngle.put(2.7, 30.0); + // distanceToRPM.put(2.7, 1900.0); + // distanceToHoodAngle.put(2.7, 30.0); - distanceToRPM.put(3.0, 2000.0); - distanceToHoodAngle.put(3.0, 30.0); + // distanceToRPM.put(3.0, 2000.0); + // distanceToHoodAngle.put(3.0, 30.0); - distanceToRPM.put(3.6, 2100.0); - distanceToHoodAngle.put(3.6, 30.0); + // distanceToRPM.put(3.6, 2100.0); + // distanceToHoodAngle.put(3.6, 30.0); - distanceToRPM.put(4.0, 2250.0); - distanceToHoodAngle.put(4.0, 30.0); + // distanceToRPM.put(4.0, 2250.0); + // distanceToHoodAngle.put(4.0, 30.0); - distanceToRPM.put(5.5, 2400.0); - distanceToHoodAngle.put(5.5, 40.0); + // distanceToRPM.put(5.5, 2400.0); + // distanceToHoodAngle.put(5.5, 40.0); - distanceToRPM.put(7.5, 3000.0); + distanceToRPM.put(7.5, 3100.0); distanceToHoodAngle.put(7.5, 40.0); } } diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index 6650684..aeb7a36 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.Elastic; import frc.lib.Elastic.Notification; @@ -21,6 +22,9 @@ public class Targeting extends SubsystemBase { private Translation2d targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); private final CommandSwerveDrivetrain drive; + private static final double FIELD_WIDTH = 8.07; // meters + private static final double FIELD_LENGTH = 16.54; // meters + public Targeting(CommandSwerveDrivetrain drive) { this.drive = drive; setTargetingHub(); @@ -67,6 +71,8 @@ public void periodic() { ); Logger.recordOutput("Targeting/TurretPivot", turretPivotPose); + + Logger.recordOutput("Targeting/TargetPose", new Pose2d(targetPos, new Rotation2d())); } /** @@ -77,18 +83,38 @@ public void setTargetingHub() { new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SCORING mode") ); Elastic.selectTab("Scoring Mode"); - targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + if (DriverStation.getAlliance().isPresent() &&DriverStation.getAlliance().equals(DriverStation.Alliance.Red)) { + targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + } else { + targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + } } /** * Set the target location to 0, 0 */ - public void setTargetingShuttle() { + public void setTargetingShuttleRight() { + Elastic.sendNotification( + new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SHUTTLING mode") + ); + Elastic.selectTab("Shuttling Mode"); + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().equals(DriverStation.Alliance.Red)) { + targetPos = new Translation2d(FIELD_WIDTH, FIELD_LENGTH); + } else { + targetPos = new Translation2d(0, 0); + } + } + + public void setTargetingShuttleLeft() { Elastic.sendNotification( new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SHUTTLING mode") ); Elastic.selectTab("Shuttling Mode"); - targetPos = new Translation2d(0, 0); + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().equals(DriverStation.Alliance.Red)) { + targetPos = new Translation2d(0, FIELD_LENGTH); + } else { + targetPos = new Translation2d(FIELD_WIDTH, 0); + } } /** diff --git a/src/main/java/frc/robot/subsystems/trigger/Trigger.java b/src/main/java/frc/robot/subsystems/trigger/Trigger.java index 0f1b2ee..4561604 100644 --- a/src/main/java/frc/robot/subsystems/trigger/Trigger.java +++ b/src/main/java/frc/robot/subsystems/trigger/Trigger.java @@ -7,13 +7,15 @@ public class Trigger extends SpikeSystem { private static final int PROXIMITY_SENSOR_CHANNEL = 2; // DIO channel for the proximity sensor - private final static double TRIGGER_SPEED = 20.0; // Rotations per second + private final static double TRIGGER_SPEED = 30.0; // Rotations per second private final Shooter shooter; private final Turret turret; private TriggerIO triggerIO; + private boolean reverseTrigger = false; + public Trigger(Shooter shooter, Turret turret) { super("Trigger", new TriggerIOInputsAutoLogged()); @@ -24,7 +26,9 @@ public Trigger(Shooter shooter, Turret turret) { // Activate motor if proximity sensor detects a ball in the indexer @Override public void onPeriodic() { - if (mechanismReadyForBalls()) { + if (this.reverseTrigger) { + triggerIO.setSpeed(-TRIGGER_SPEED); + } else if (mechanismReadyForBalls()) { // run the indexer if the mechanisms are ready for balls // run it regardless of ball in indexer, so that it can feed a ball in if there is one queued up triggerIO.setSpeed(TRIGGER_SPEED); @@ -46,12 +50,19 @@ private boolean hasBallQueued() { return false; } + public void setReverseTrigger(boolean reverse) { + this.reverseTrigger = reverse; + } + /** * Determines if the shooter and turret are ready to receive a ball. * @return true if the shooter is at target RPS, the turret is at target angle, and the driver is requesting to shoot, false otherwise */ private boolean mechanismReadyForBalls() { - return shooter.isAtTargetRPS() && turret.isAtTargetAngle() && shooter.isDriverRequestingShooting(); + if (shooter.isRequestingWithForce()) { + return true; + } + return shooter.isAtTargetRPS() && turret.isAtTargetAngle() && shooter.isShootingRequested(); } /** @@ -60,7 +71,7 @@ private boolean mechanismReadyForBalls() { * @return true if the indexer needs a ball, false otherwise */ public boolean needsFeeding() { - return !hasBallQueued(); + return mechanismReadyForBalls(); } @Override diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 4de857e..8f26703 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,6 +1,6 @@ package frc.robot.subsystems.turret; -import frc.lib.FieldConstants; + import frc.lib.subsystem.SpikeSystem; import frc.robot.RobotContainer; import frc.robot.subsystems.targeting.Targeting; @@ -9,8 +9,6 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; public class Turret extends SpikeSystem { @@ -31,10 +29,10 @@ public class Turret extends SpikeSystem { public static final double ENCODER_COMBINED_PERIOD_TURRET_REV = ENCODER_COMBINED_PERIOD_REV * (PINION_ENCODER_TEETH / TURRET_GEAR_TEETH); - public static final double TURRET_CENTER_OFFSET_DEG = -17; // subtracted from robot relative heading - public static final double TURRET_ROBOT_OFFSET_DEG = 120; // subtracted from robot relative heading to get turret relative heading + public static final double TURRET_CENTER_OFFSET_DEG = -88.1; // subtracted from robot relative heading + public static final double TURRET_ROBOT_OFFSET_DEG = 51.8; // subtracted from robot relative heading to get turret relative heading - public static final Translation2d TURRET_OFFSET_FROM_CENTER = new Translation2d(0.3, 0); // distance from the center of the robot to the center of the turret, in meters + public static final Translation2d TURRET_OFFSET_FROM_CENTER = new Translation2d(-0.3, -0.2); // distance from the center of the robot to the center of the turret, in meters private TurretIO turretIO; @@ -54,6 +52,7 @@ public void onPeriodic() { // } this.turretIO.setTurretAngleFieldRelativeDegrees(getTurretAngleDegreesFieldRelative()); + // this.turretIO.setTurretAngleFieldRelativeDegrees(0); } public double getTurretAngleDegreesFieldRelative() { @@ -80,7 +79,8 @@ public boolean isAtTargetAngle() { double error = Math.abs(io.turretAngleDegreesRobotRelative - io.processedTargetTurretDegrees); - return error <= TURRET_AIMING_TOLERANCE_DEGREES; + // return error <= TURRET_AIMING_TOLERANCE_DEGREES; + return true; } public void changeTrim(double deltaDegrees) { diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 1ea2513..af8f081 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; @@ -48,6 +49,7 @@ public class TurretIOTalonFX implements TurretIO { private double processedTargetTurretDegreesFieldRelative; // processed target angle of the turret in degrees, relative to the field private double targetTurretAngleMotorRevs; // target angle of the turret in motor rotations private double calculatedMotorOffsetRevs; // calculated offset in motor rotations based on the current position of the turret and the pinion encoder reading + private double targetTurretDegreesTurretRelative = 0; // COMMANDS private final MotionMagicVoltage mmRequest = new MotionMagicVoltage(0.0); @@ -113,11 +115,14 @@ private void setTurretAngleRobotRelativeDegrees(double robotRelativeAngleDegrees } private void setTurretAngleTurretRelativeDegrees(double angleDegrees) { + this.targetTurretDegreesTurretRelative = angleDegrees; + angleDegrees += turretTrimDegrees; angleDegrees = wrap180(angleDegrees); double targetMotorRotations = angleDegrees / 180.0; this.targetTurretAngleMotorRevs = targetMotorRotations; + mmRequest.Position = targetMotorRotations; this.turretMotor.setControl( mmRequest @@ -159,7 +164,7 @@ public void updateInputs(TurretIOInputs inputs) { inputs.turretAngleDegreesRobotRelative = getTurretAngleRobotRelative(); inputs.targetTurretMotorRotations = this.targetTurretAngleMotorRevs; inputs.turretOffsetRotations = this.calculatedMotorOffsetRevs; - inputs.targetTurretDegrees = this.targetTurretDegreesFieldRelative; + inputs.targetTurretDegrees = this.targetTurretDegreesTurretRelative; inputs.processedTargetTurretDegrees = this.processedTargetTurretDegreesFieldRelative; inputs.turretMotorPositionRotations = this.turretMotorPosition.getValueAsDouble(); inputs.pinionEncoderRotations = this.pinionEncoderSignal.getValueAsDouble(); @@ -182,7 +187,7 @@ private Pair getTurretMotionConfigs() { configs.kD = 1; configs.kS = 0.4; //kS; - configs.kV = 0.2; //kV; + configs.kV = 0.4; //kV; MotionMagicConfigs mmConfigs = new MotionMagicConfigs(); @@ -242,6 +247,7 @@ public CANcoderConfiguration getEncoderConfigs() { public CANcoderConfiguration getPinionEncoderConfigs() { CANcoderConfiguration configs = getEncoderConfigs(); // configs.MagnetSensor.MagnetOffset = Turret.PINION_ENCODER_OFFSET; + configs.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; return configs; } @@ -249,6 +255,7 @@ public CANcoderConfiguration getPinionEncoderConfigs() { public CANcoderConfiguration getFollowerEncoderConfigs() { CANcoderConfiguration configs = getEncoderConfigs(); // configs.MagnetSensor.MagnetOffset = Turret.FOLLOWER_ENCODER_OFFSET; + configs.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; return configs; } @@ -267,24 +274,19 @@ private double getPinionEncoderRevs() { double bestError = Double.MAX_VALUE; double bestPosition = lastPinionRevs; -// int searchCount = (int) Turret.FOLLOWER_ENCODER_TEETH; - - for (int k = -20; k < 20; k++) { + for (int k = -15; k <= 15; k++) { double assumedPinionRevs = pinionEncoderReading + k; double predictedFollowerReading = positiveMod(assumedPinionRevs * (Turret.PINION_ENCODER_TEETH / Turret.FOLLOWER_ENCODER_TEETH), 1.0); double predictionError = Math.abs(predictedFollowerReading - followerEncoderReading); - if (predictionError > 0.5) { predictionError = 1.0 - predictionError; } - // continuity penalty - // double continuityError = Math.abs(assumedPinionRevs - lastPinionRevs); - - double score = predictionError * 0.1; + double continuityError = Math.abs(assumedPinionRevs - lastPinionRevs); + double score = predictionError + continuityError * 0.001; if (score < bestError) { bestError = score; diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java index 97650e8..f696f6e 100644 --- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java +++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java @@ -22,49 +22,34 @@ public static List getCameras() { static { // register cameras - // registerCamera( - // new Camera( - // "left", - // new Transform3d( - // Inches.of(-13.8), - // Inches.of(7.5), - // Inches.of(22.3/4), - // new Rotation3d( - // Degrees.of(0), - // Degrees.of(60), - // Degrees.of(90) - // ) - // ) - // ) - // ); - // registerCamera( - // new Camera( - // "right", - // new Transform3d( - // Inches.of(13.8), - // Inches.of(5), - // Inches.of(6.3/4), - // new Rotation3d( - // Degrees.of(0), - // Degrees.of(60), - // Degrees.of(-90) - // ) - // ) - // ) - // ); + registerCamera( + new Camera( + "back", + new Transform3d( + Inches.of(-13.75), + Inches.of(-0.2), + Inches.of(8.475), + new Rotation3d( + Degrees.of(0), + Degrees.of(-65), + Degrees.of(180) + ) + ) + ) + ); registerCamera( new Camera( - "left", + "right", new Transform3d( - Inches.of(-29.5/2), - Inches.of(0), - Inches.of(6.75), + Inches.of(-0.95), + Inches.of(-13.75), + Inches.of(8.475), new Rotation3d( Degrees.of(0), - Degrees.of(-60), // negative pitch = tilted upward - Degrees.of(180) + Degrees.of(-65), // negative pitch = tilted upward + Degrees.of(-90) ) ) ) From f981f59bfd17f9b23e6443f038dae167ad5cd947 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Fri, 20 Mar 2026 19:50:36 -0400 Subject: [PATCH 23/29] Add more driver/operator controls --- src/main/java/frc/robot/RobotContainer.java | 8 +++++-- .../frc/robot/subsystems/intake/Intake.java | 24 ++++++++++++++----- .../robot/subsystems/targeting/Targeting.java | 19 +++++++-------- .../frc/robot/subsystems/turret/Turret.java | 15 ++++++++++-- .../frc/robot/subsystems/turret/TurretIO.java | 5 ++++ .../subsystems/turret/TurretIOTalonFX.java | 2 +- 6 files changed, 52 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 193aeb6..11c59c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -129,11 +129,13 @@ private void setupSwerveBindings() { private void setupIntakeBindings() { // toggle intake on A press - // operatorController.a().onTrue(intake.run(intake::toggleIntake)); + operatorController.rightBumper().onTrue(intake.runOnce(intake::toggleIntake)); + + operatorController.a().onTrue(intake.runOnce(intake::switchDirection)); } private void setupTargetingBindings() { - operatorController.y().onTrue(targeting.run(targeting::setTargetingHub)); + operatorController.y().onTrue(targeting.runOnce(targeting::setTargetingHub)); operatorController.x().onTrue(targeting.runOnce(targeting::setTargetingShuttleLeft)); operatorController.b().onTrue(targeting.runOnce(targeting::setTargetingShuttleRight)); } @@ -149,6 +151,8 @@ private void setupShooterBindings() { operatorController.rightStick().onTrue(shooter.runOnce(() -> shooter.zeroHood())); + operatorController.leftBumper().onTrue(turret.runOnce(turret::toggleAimingOverride)); + operatorController.leftStick().onTrue(trigger.run(() -> trigger.setReverseTrigger(true))); operatorController.leftStick().onFalse(trigger.run(() -> trigger.setReverseTrigger(false))); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 3bd4cb1..62e7bd8 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -20,7 +20,8 @@ public enum IntakeState { private IntakeIO intakeIO; private final CommandSwerveDrivetrain drivetrain; - private boolean running = false; // True if the intake is running, False otherwise + private boolean running = true; // True if the intake is running, False otherwise + private boolean forward = true; // Intake constructor public Intake(CommandSwerveDrivetrain drivetrain) { @@ -57,6 +58,8 @@ public void onPeriodic() { // break; // } doDeployedState(); + } else { + doRetractedState(); } } @@ -84,6 +87,10 @@ private void doDeployedState() { // Cap at MAX_SPEED double speed = Math.min(BASE_SPEED_INTAKE + robotAbsoluteVelocity * SPEED_PER_MPS, MAX_SPEED); + if (forward == false) { + speed *= -1; + } + // Update the intakeIO on speed intakeIO.setIntakeSpeed(speed); } @@ -118,6 +125,10 @@ public void enable() { running = true; } + public void switchDirection() { + forward = !forward; + } + // Disables the intake public void disable() { running = false; @@ -154,10 +165,11 @@ protected Runnable setupDataRefresher() { // Toggles the intake between deployed and retracted states public void toggleIntake() { - if (intakeIO.getIntakeState() == IntakeState.DEPLOYED || intakeIO.getIntakeState() == IntakeState.DEPLOYING) { - retract(); - } else { - deploy(); - } + this.running = !this.running; + // if (intakeIO.getIntakeState() == IntakeState.DEPLOYED || intakeIO.getIntakeState() == IntakeState.DEPLOYING) { + // retract(); + // } else { + // deploy(); + // } } } diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index aeb7a36..9616a5a 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -19,7 +19,7 @@ public class Targeting extends SubsystemBase { private static final double NOMINAL_SHOT_TIME_S = 0.3; // see github issue #23 (https://github.com/Team293/Rebuilt/issues/23) private static ShotCompensation.AdjustedShot shotData = new ShotCompensation.AdjustedShot(0.0, 0.0, 0.0, 0.0, 0.0); - private Translation2d targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + private static Translation2d targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); private final CommandSwerveDrivetrain drive; private static final double FIELD_WIDTH = 8.07; // meters @@ -43,10 +43,11 @@ public static Translation2d differenceBetweenRobotAndTarget() { .plus(robotPose.getTranslation()); // vector from the turret pivot directly to the goal - Translation2d goalPose = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + Translation2d goalPose = targetPos; Translation2d toGoal = goalPose.minus(turretPivot); Logger.recordOutput("Turret/TurretPivot", new Pose2d(turretPivot, toGoal.getAngle())); + Logger.recordOutput("Targeting/TargetPose", new Pose2d(goalPose, new Rotation2d())); return toGoal; } @@ -71,8 +72,6 @@ public void periodic() { ); Logger.recordOutput("Targeting/TurretPivot", turretPivotPose); - - Logger.recordOutput("Targeting/TargetPose", new Pose2d(targetPos, new Rotation2d())); } /** @@ -83,7 +82,7 @@ public void setTargetingHub() { new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SCORING mode") ); Elastic.selectTab("Scoring Mode"); - if (DriverStation.getAlliance().isPresent() &&DriverStation.getAlliance().equals(DriverStation.Alliance.Red)) { + if (DriverStation.getAlliance().isPresent() &&DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); } else { targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); @@ -98,8 +97,8 @@ public void setTargetingShuttleRight() { new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SHUTTLING mode") ); Elastic.selectTab("Shuttling Mode"); - if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().equals(DriverStation.Alliance.Red)) { - targetPos = new Translation2d(FIELD_WIDTH, FIELD_LENGTH); + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { + targetPos = new Translation2d(FIELD_LENGTH, FIELD_WIDTH); } else { targetPos = new Translation2d(0, 0); } @@ -110,10 +109,10 @@ public void setTargetingShuttleLeft() { new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SHUTTLING mode") ); Elastic.selectTab("Shuttling Mode"); - if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().equals(DriverStation.Alliance.Red)) { - targetPos = new Translation2d(0, FIELD_LENGTH); + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { + targetPos = new Translation2d(FIELD_LENGTH, 0); } else { - targetPos = new Translation2d(FIELD_WIDTH, 0); + targetPos = new Translation2d(0, FIELD_WIDTH); } } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 8f26703..c830fe2 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -34,6 +34,8 @@ public class Turret extends SpikeSystem { public static final Translation2d TURRET_OFFSET_FROM_CENTER = new Translation2d(-0.3, -0.2); // distance from the center of the robot to the center of the turret, in meters + private boolean overrideAutomaticAiming = false; + private TurretIO turretIO; public Turret() { @@ -51,8 +53,12 @@ public void onPeriodic() { // this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg); // } - this.turretIO.setTurretAngleFieldRelativeDegrees(getTurretAngleDegreesFieldRelative()); - // this.turretIO.setTurretAngleFieldRelativeDegrees(0); + if (this.overrideAutomaticAiming) { + this.turretIO.setTurretAngleRobotRelativeDegrees(0); + } else { + this.turretIO.setTurretAngleFieldRelativeDegrees(getTurretAngleDegreesFieldRelative()); + } + } public double getTurretAngleDegreesFieldRelative() { @@ -63,12 +69,17 @@ public double getTurretAngleDegreesFieldRelative() { return angleToTarget; } + @Override protected Runnable setupDataRefresher() { turretIO = new TurretIOTalonFX(RobotContainer.getDrive()); return useAsyncDataRefresher(turretIO); } + public void toggleAimingOverride() { + this.overrideAutomaticAiming = !this.overrideAutomaticAiming; + } + /** * Checks if the turret is at the target angle, within the tolerance defined by TURRET_AIMING_TOLERANCE_DEGREES. * @return true if the turret is at the target angle, false otherwise diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index a2b232b..3e5918d 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -29,6 +29,11 @@ public static class TurretIOInputs extends BaseInputClass { */ void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees); + /** + * Set the angle of the turret relative to the robot. + */ + void setTurretAngleRobotRelativeDegrees(double robotRelativeDegrees); + /** * Recalculates the turret motor zero position to fix any encoder drift. */ diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index af8f081..98525d6 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -110,7 +110,7 @@ public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) setTurretAngleRobotRelativeDegrees(targetRobotRelativeDeg); } - private void setTurretAngleRobotRelativeDegrees(double robotRelativeAngleDegrees) { + public void setTurretAngleRobotRelativeDegrees(double robotRelativeAngleDegrees) { setTurretAngleTurretRelativeDegrees(robotRelativeAngleDegrees + Turret.TURRET_ROBOT_OFFSET_DEG); } From a29be9d4659a7d5e56736638a02ee6a07965f5b6 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Fri, 20 Mar 2026 20:08:57 -0400 Subject: [PATCH 24/29] Add initial auto code --- .../deploy/pathplanner/autos/Outpost.auto | 31 ++++++ .../pathplanner/paths/Drive to Outpost.path | 105 ++++++++++++++++++ .../deploy/pathplanner/paths/New Path.path | 70 ------------ src/main/java/frc/robot/RobotContainer.java | 4 + .../java/frc/robot/commands/EmptyHopper.java | 34 ++++++ 5 files changed, 174 insertions(+), 70 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Outpost.auto create mode 100644 src/main/deploy/pathplanner/paths/Drive to Outpost.path delete mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/java/frc/robot/commands/EmptyHopper.java diff --git a/src/main/deploy/pathplanner/autos/Outpost.auto b/src/main/deploy/pathplanner/autos/Outpost.auto new file mode 100644 index 0000000..ecf1b35 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Outpost.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Drive to Outpost" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "emptyHopper" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Drive to Outpost.path b/src/main/deploy/pathplanner/paths/Drive to Outpost.path new file mode 100644 index 0000000..9137a9d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Drive to Outpost.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.923285883748518, + "y": 0.6835112692763936 + }, + "prevControl": null, + "nextControl": { + "x": 2.4923131672597876, + "y": 0.7480664294187432 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3201660735468574, + "y": 0.6835112692763936 + }, + "prevControl": { + "x": 3.759471141341823, + "y": 0.5778742000804336 + }, + "nextControl": { + "x": 1.1474139976275215, + "y": 0.7695848161328578 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3201660735468574, + "y": 0.6835112692763936 + }, + "prevControl": { + "x": 1.588540925266905, + "y": 0.7157888493475679 + }, + "nextControl": { + "x": 3.05179122182681, + "y": 0.6512336892052193 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5018623962040341, + "y": 0.6835112692763936 + }, + "prevControl": { + "x": 0.910711743772243, + "y": 1.2107117437722423 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.1474245115453034, + "rotationDegrees": 178.13462360139246 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0123734533183526, + "maxWaypointRelativePos": 1.9775028121484612, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 30.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 88.94881925045625 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 175.68397248013417 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path deleted file mode 100644 index a192fed..0000000 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.202789735099339, - "y": 5.683004966887417 - }, - "prevControl": null, - "nextControl": { - "x": 7.769064569536425, - "y": 4.957011589403972 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.7850082781456953, - "y": 5.937102649006622 - }, - "prevControl": { - "x": -0.2149917218543047, - "y": 5.937102649006622 - }, - "nextControl": { - "x": 1.7850082781456953, - "y": 5.937102649006622 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.698112582781457, - "y": 6.975273178807948 - }, - "prevControl": { - "x": 2.7660347682119206, - "y": 7.229370860927153 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -61.76255446183274 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 11c59c9..19c369d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.pathplanner.lib.auto.NamedCommands; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Rotation2d; @@ -18,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.lib.SpikeController; +import frc.robot.commands.EmptyHopper; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; import frc.robot.subsystems.findexer.Findexer; @@ -70,6 +72,8 @@ public RobotContainer() { SmartDashboard.putData("Auto Path", autoChooser); configureBindings(); + + NamedCommands.registerCommand("emptyHopper", new EmptyHopper(shooter, 15)); } public static CommandSwerveDrivetrain getDrive() { diff --git a/src/main/java/frc/robot/commands/EmptyHopper.java b/src/main/java/frc/robot/commands/EmptyHopper.java new file mode 100644 index 0000000..e868c60 --- /dev/null +++ b/src/main/java/frc/robot/commands/EmptyHopper.java @@ -0,0 +1,34 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooter.Shooter; + +public class EmptyHopper extends Command { + private Shooter shooter; + + private double scoringTime; + private Timer scoringTimer = new Timer(); + + public EmptyHopper(Shooter shooter, double forTime) { + this.scoringTime = forTime; + this.shooter = shooter; + + this.shooter.setDriverRequestingShooting(true); + } + + public void initialize() { + this.scoringTimer.restart(); + } + + @Override + public boolean isFinished() { + return this.scoringTimer.hasElapsed(scoringTime); + } + + @Override + public void end(boolean interrupted) { + // TODO Auto-generated method stub + this.shooter.setDriverRequestingShooting(false); + } +} From 2e9a7a890ee5f85ab85ca103305e71acb146ce55 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Fri, 20 Mar 2026 21:01:15 -0400 Subject: [PATCH 25/29] auto tuning --- .../deploy/pathplanner/paths/Drive to Outpost.path | 12 ++++++------ src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/EmptyHopper.java | 9 +++++++-- .../frc/robot/subsystems/targeting/Targeting.java | 11 +++++++---- 4 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Drive to Outpost.path b/src/main/deploy/pathplanner/paths/Drive to Outpost.path index 9137a9d..c730664 100644 --- a/src/main/deploy/pathplanner/paths/Drive to Outpost.path +++ b/src/main/deploy/pathplanner/paths/Drive to Outpost.path @@ -20,12 +20,12 @@ "y": 0.6835112692763936 }, "prevControl": { - "x": 3.759471141341823, - "y": 0.5778742000804336 + "x": 3.6650652431791224, + "y": 0.6727520759193351 }, "nextControl": { - "x": 1.1474139976275215, - "y": 0.7695848161328578 + "x": 1.144297204873398, + "y": 0.6929182202257816 }, "isLocked": false, "linkedName": null @@ -52,8 +52,8 @@ "y": 0.6835112692763936 }, "prevControl": { - "x": 0.910711743772243, - "y": 1.2107117437722423 + "x": 1.1581731909845794, + "y": 0.7588256227758006 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 19c369d..519ca3f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,7 +73,7 @@ public RobotContainer() { configureBindings(); - NamedCommands.registerCommand("emptyHopper", new EmptyHopper(shooter, 15)); + NamedCommands.registerCommand("emptyHopper", new EmptyHopper(shooter, targeting, 15)); } public static CommandSwerveDrivetrain getDrive() { diff --git a/src/main/java/frc/robot/commands/EmptyHopper.java b/src/main/java/frc/robot/commands/EmptyHopper.java index e868c60..2db778e 100644 --- a/src/main/java/frc/robot/commands/EmptyHopper.java +++ b/src/main/java/frc/robot/commands/EmptyHopper.java @@ -3,22 +3,27 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.targeting.Targeting; public class EmptyHopper extends Command { private Shooter shooter; + private Targeting targeting; private double scoringTime; private Timer scoringTimer = new Timer(); - public EmptyHopper(Shooter shooter, double forTime) { - this.scoringTime = forTime; + public EmptyHopper(Shooter shooter, Targeting targeting, double forTime) { this.shooter = shooter; + this.targeting = targeting; + + this.scoringTime = forTime; this.shooter.setDriverRequestingShooting(true); } public void initialize() { this.scoringTimer.restart(); + this.targeting.setTargetingHub(); } @Override diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index 9616a5a..62fe41b 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -25,6 +25,9 @@ public class Targeting extends SubsystemBase { private static final double FIELD_WIDTH = 8.07; // meters private static final double FIELD_LENGTH = 16.54; // meters + private static final double shuttlingXOffset = 1.5; + private static final double shuttlingYOffset = 1.5; + public Targeting(CommandSwerveDrivetrain drive) { this.drive = drive; setTargetingHub(); @@ -98,9 +101,9 @@ public void setTargetingShuttleRight() { ); Elastic.selectTab("Shuttling Mode"); if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { - targetPos = new Translation2d(FIELD_LENGTH, FIELD_WIDTH); + targetPos = new Translation2d(FIELD_LENGTH - shuttlingXOffset, FIELD_WIDTH - shuttlingYOffset); } else { - targetPos = new Translation2d(0, 0); + targetPos = new Translation2d(0 + shuttlingXOffset, 0 + shuttlingYOffset); } } @@ -110,9 +113,9 @@ public void setTargetingShuttleLeft() { ); Elastic.selectTab("Shuttling Mode"); if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { - targetPos = new Translation2d(FIELD_LENGTH, 0); + targetPos = new Translation2d(FIELD_LENGTH - shuttlingXOffset, 0 + shuttlingYOffset); } else { - targetPos = new Translation2d(0, FIELD_WIDTH); + targetPos = new Translation2d(0 + shuttlingXOffset, FIELD_WIDTH - shuttlingYOffset); } } From 462dc3abd504cf83dd06e66f279abaed3195de08 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Sat, 21 Mar 2026 14:39:34 -0400 Subject: [PATCH 26/29] Update auto paths and operator commands --- .../pathplanner/autos/Center Score Only.auto | 32 +++++++++++ .../autos/Offcenter Score Only.auto | 32 +++++++++++ ...tpost.auto => Outpost Pickup + Score.auto} | 0 .../pathplanner/autos/Outpost Score Only.auto | 32 +++++++++++ .../pathplanner/paths/Center Right Start.path | 54 +++++++++++++++++++ .../pathplanner/paths/Center Start.path | 54 +++++++++++++++++++ .../pathplanner/paths/Drive to Outpost.path | 14 ++--- .../pathplanner/paths/Left Side Start.path | 54 +++++++++++++++++++ .../pathplanner/paths/Right Side Start.path | 54 +++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 6 +-- src/main/java/frc/robot/RobotContainer.java | 12 ++--- .../java/frc/robot/commands/EmptyHopper.java | 4 ++ .../drive/CommandSwerveDrivetrain.java | 9 +++- .../robot/subsystems/findexer/Findexer.java | 2 +- .../frc/robot/subsystems/intake/Intake.java | 3 +- 15 files changed, 342 insertions(+), 20 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Center Score Only.auto create mode 100644 src/main/deploy/pathplanner/autos/Offcenter Score Only.auto rename src/main/deploy/pathplanner/autos/{Outpost.auto => Outpost Pickup + Score.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/Outpost Score Only.auto create mode 100644 src/main/deploy/pathplanner/paths/Center Right Start.path create mode 100644 src/main/deploy/pathplanner/paths/Center Start.path create mode 100644 src/main/deploy/pathplanner/paths/Left Side Start.path create mode 100644 src/main/deploy/pathplanner/paths/Right Side Start.path diff --git a/src/main/deploy/pathplanner/autos/Center Score Only.auto b/src/main/deploy/pathplanner/autos/Center Score Only.auto new file mode 100644 index 0000000..68f5377 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Score Only.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Start" + } + }, + { + "type": "named", + "data": { + "name": "emptyHopper" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Offcenter Score Only.auto b/src/main/deploy/pathplanner/autos/Offcenter Score Only.auto new file mode 100644 index 0000000..df1cdbe --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Offcenter Score Only.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Right Start" + } + }, + { + "type": "named", + "data": { + "name": "emptyHopper" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Outpost.auto b/src/main/deploy/pathplanner/autos/Outpost Pickup + Score.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Outpost.auto rename to src/main/deploy/pathplanner/autos/Outpost Pickup + Score.auto diff --git a/src/main/deploy/pathplanner/autos/Outpost Score Only.auto b/src/main/deploy/pathplanner/autos/Outpost Score Only.auto new file mode 100644 index 0000000..1cfd10d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Outpost Score Only.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Side Start" + } + }, + { + "type": "named", + "data": { + "name": "emptyHopper" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Right Start.path b/src/main/deploy/pathplanner/paths/Center Right Start.path new file mode 100644 index 0000000..52a928c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Right Start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.482158956109135, + "y": 3.4163463819691584 + }, + "prevControl": null, + "nextControl": { + "x": 3.7317275494028843, + "y": 3.4016658764812915 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.482158956109135, + "y": 3.4163463819691584 + }, + "prevControl": { + "x": 3.232158956109135, + "y": 3.4163463819691584 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Start.path b/src/main/deploy/pathplanner/paths/Center Start.path new file mode 100644 index 0000000..e8f8bd1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.482158956109135, + "y": 4.115693950177936 + }, + "prevControl": null, + "nextControl": { + "x": 3.7317275494028843, + "y": 4.101013444690069 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.482158956109135, + "y": 4.115693950177936 + }, + "prevControl": { + "x": 3.232158956109135, + "y": 4.115693950177936 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Drive to Outpost.path b/src/main/deploy/pathplanner/paths/Drive to Outpost.path index c730664..80b8deb 100644 --- a/src/main/deploy/pathplanner/paths/Drive to Outpost.path +++ b/src/main/deploy/pathplanner/paths/Drive to Outpost.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.4923131672597876, - "y": 0.7480664294187432 + "x": 2.524590747330962, + "y": 0.6727520759193353 }, "isLocked": false, "linkedName": null @@ -48,11 +48,11 @@ }, { "anchor": { - "x": 0.5018623962040341, + "x": 0.4050296559905108, "y": 0.6835112692763936 }, "prevControl": { - "x": 1.1581731909845794, + "x": 1.0613404507710558, "y": 0.7588256227758006 }, "nextControl": null, @@ -63,7 +63,7 @@ "rotationTargets": [ { "waypointRelativePos": 2.1474245115453034, - "rotationDegrees": 178.13462360139246 + "rotationDegrees": 180.0 } ], "constraintZones": [ @@ -93,13 +93,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 88.94881925045625 + "rotation": 89.23097531742198 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 175.68397248013417 + "rotation": 178.8542371618249 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Side Start.path b/src/main/deploy/pathplanner/paths/Left Side Start.path new file mode 100644 index 0000000..69d14fc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Side Start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.977081850533809, + "y": 7.354211150652431 + }, + "prevControl": null, + "nextControl": { + "x": 4.226594995154531, + "y": 7.369805722191225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.977081850533809, + "y": 7.354211150652431 + }, + "prevControl": { + "x": 3.727081850533809, + "y": 7.354211150652431 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Side Start.path b/src/main/deploy/pathplanner/paths/Right Side Start.path new file mode 100644 index 0000000..b808861 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Side Start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.977081850533809, + "y": 0.6835112692763936 + }, + "prevControl": null, + "nextControl": { + "x": 4.226594995154531, + "y": 0.6991058408151881 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.977081850533809, + "y": 0.6835112692763936 + }, + "prevControl": { + "x": 3.727081850533809, + "y": 0.6835112692763936 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index b381d04..15e092c 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.965, - "robotLength": 0.965, + "robotWidth": 0.8255, + "robotLength": 0.8255, "holonomicMode": true, "pathFolders": [], "autoFolders": [], @@ -15,7 +15,7 @@ "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, - "driveMotorType": "falcon500", + "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, "flModuleX": 0.273, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 519ca3f..07e9c9e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -147,18 +147,18 @@ private void setupTargetingBindings() { private void setupShooterBindings() { // toggle shooter on right trigger hold driverController.rightTrigger() - .onTrue(shooter.run(() -> shooter.setDriverRequestingShooting(true))) - .onFalse(shooter.run(() -> shooter.setDriverRequestingShooting(false))); + .onTrue(shooter.runOnce(() -> shooter.setDriverRequestingShooting(true))) + .onFalse(shooter.runOnce(() -> shooter.setDriverRequestingShooting(false))); driverController.leftTrigger() - .onTrue(shooter.run(() -> shooter.setRequestingWithForce(true))) - .onFalse(shooter.run(() -> shooter.setRequestingWithForce(false))); + .onTrue(shooter.runOnce(() -> shooter.setRequestingWithForce(true))) + .onFalse(shooter.runOnce(() -> shooter.setRequestingWithForce(false))); operatorController.rightStick().onTrue(shooter.runOnce(() -> shooter.zeroHood())); operatorController.leftBumper().onTrue(turret.runOnce(turret::toggleAimingOverride)); - operatorController.leftStick().onTrue(trigger.run(() -> trigger.setReverseTrigger(true))); - operatorController.leftStick().onFalse(trigger.run(() -> trigger.setReverseTrigger(false))); + operatorController.leftStick().onTrue(trigger.runOnce(() -> trigger.setReverseTrigger(true))); + operatorController.leftStick().onFalse(trigger.runOnce(() -> trigger.setReverseTrigger(false))); operatorController.povUp().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(0.1))); operatorController.povDown().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(-0.1))); diff --git a/src/main/java/frc/robot/commands/EmptyHopper.java b/src/main/java/frc/robot/commands/EmptyHopper.java index 2db778e..d0e98c3 100644 --- a/src/main/java/frc/robot/commands/EmptyHopper.java +++ b/src/main/java/frc/robot/commands/EmptyHopper.java @@ -26,6 +26,10 @@ public void initialize() { this.targeting.setTargetingHub(); } + public void execute() { + this.targeting.setTargetingHub(); + } + @Override public boolean isFinished() { return this.scoringTimer.hasElapsed(scoringTime); diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java index 77519a3..96cea7e 100644 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.*; +import java.util.Optional; import java.util.function.Supplier; import com.ctre.phoenix6.SignalLogger; @@ -336,7 +337,13 @@ private void configureAutoBuilder() { new PIDConstants(5.0, 0, 0, 0) ), config, - () -> false, + () -> { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, this ); Pathfinding.setPathfinder(new LocalADStarAK()); diff --git a/src/main/java/frc/robot/subsystems/findexer/Findexer.java b/src/main/java/frc/robot/subsystems/findexer/Findexer.java index 8f8cbfa..6100bea 100644 --- a/src/main/java/frc/robot/subsystems/findexer/Findexer.java +++ b/src/main/java/frc/robot/subsystems/findexer/Findexer.java @@ -4,7 +4,7 @@ import frc.robot.subsystems.trigger.Trigger; public class Findexer extends SpikeSystem { - private static final double FEEDING_RPS = -20.0; // feeding velocity in rotations per second + private static final double FEEDING_RPS = -30.0 * 9; // feeding velocity in rotations per second private final Trigger trigger; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 62e7bd8..3474252 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -20,14 +20,13 @@ public enum IntakeState { private IntakeIO intakeIO; private final CommandSwerveDrivetrain drivetrain; - private boolean running = true; // True if the intake is running, False otherwise + private boolean running = false; // True if the intake is running, False otherwise private boolean forward = true; // Intake constructor public Intake(CommandSwerveDrivetrain drivetrain) { super("Intake", new IntakeIOInputsAutoLogged()); this.drivetrain = drivetrain; - enable(); } // Intake periodic function From 8ac936176ee2c50f3defbcb5322645760f3bcb09 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Sat, 21 Mar 2026 16:13:12 -0400 Subject: [PATCH 27/29] Update auto paths --- .../autos/Center Left Score Only.auto | 32 +++++++++++ ...Only.auto => Center Right Score Only.auto} | 0 .../pathplanner/paths/Center Left Start.path | 54 ++++++++++++++++++ .../java/frc/robot/commands/EmptyHopper.java | 4 ++ .../frc/robot/subsystems/shooter/Shooter.java | 2 + .../robot/subsystems/targeting/Targeting.java | 55 ++++++++++++++++--- 6 files changed, 139 insertions(+), 8 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Center Left Score Only.auto rename src/main/deploy/pathplanner/autos/{Offcenter Score Only.auto => Center Right Score Only.auto} (100%) create mode 100644 src/main/deploy/pathplanner/paths/Center Left Start.path diff --git a/src/main/deploy/pathplanner/autos/Center Left Score Only.auto b/src/main/deploy/pathplanner/autos/Center Left Score Only.auto new file mode 100644 index 0000000..f274100 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Left Score Only.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Left Start" + } + }, + { + "type": "named", + "data": { + "name": "emptyHopper" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Offcenter Score Only.auto b/src/main/deploy/pathplanner/autos/Center Right Score Only.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Offcenter Score Only.auto rename to src/main/deploy/pathplanner/autos/Center Right Score Only.auto diff --git a/src/main/deploy/pathplanner/paths/Center Left Start.path b/src/main/deploy/pathplanner/paths/Center Left Start.path new file mode 100644 index 0000000..e7ea659 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Left Start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.600510083036774, + "y": 5.159335705812574 + }, + "prevControl": null, + "nextControl": { + "x": 3.850078676330523, + "y": 5.144655200324707 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.600510083036774, + "y": 5.159335705812574 + }, + "prevControl": { + "x": 3.350510083036774, + "y": 5.159335705812574 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/EmptyHopper.java b/src/main/java/frc/robot/commands/EmptyHopper.java index d0e98c3..cd72c64 100644 --- a/src/main/java/frc/robot/commands/EmptyHopper.java +++ b/src/main/java/frc/robot/commands/EmptyHopper.java @@ -19,13 +19,17 @@ public EmptyHopper(Shooter shooter, Targeting targeting, double forTime) { this.scoringTime = forTime; this.shooter.setDriverRequestingShooting(true); + + this.targeting.setTargetingHub(); } + @Override public void initialize() { this.scoringTimer.restart(); this.targeting.setTargetingHub(); } + @Override public void execute() { this.targeting.setTargetingHub(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 3d1808b..a582612 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,6 +4,7 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.subsystem.SpikeSystem; import frc.robot.subsystems.targeting.ShotData; @@ -30,6 +31,7 @@ public Shooter() { */ @Override public void onPeriodic() { + double distToTarget = getDistanceToTarget(); // distance in meters readFromData = SmartDashboard.getBoolean("ReadFromData", true); // double targetRPM = ShotData.distanceToRPM.get(distToTarget); diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index 62fe41b..61e5da2 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.Elastic; import frc.lib.Elastic.Notification; @@ -28,11 +29,18 @@ public class Targeting extends SubsystemBase { private static final double shuttlingXOffset = 1.5; private static final double shuttlingYOffset = 1.5; + private boolean overrideRedAlliance = false; + private boolean overrideBlueAlliance = false; + + public Targeting(CommandSwerveDrivetrain drive) { this.drive = drive; setTargetingHub(); Logger.recordOutput("HubTarget", FieldConstants.Hub.oppTopCenterPoint); Logger.recordOutput("ShuttleTarget", new Pose2d(0, 0, new Rotation2d())); + + SmartDashboard.putBoolean("OverrideBlueAlliance", overrideBlueAlliance); + SmartDashboard.putBoolean("OverrideRedAlliance", overrideRedAlliance); } public static Translation2d differenceBetweenRobotAndTarget() { @@ -67,12 +75,16 @@ public void periodic() { .plus(robotPose.getTranslation()); Pose2d turretPivotPose = new Pose2d(turretPivot, robotPose.getRotation()); - shotData = ShotCompensation.compensateForMovement( - turretPivotPose, - drive.getState().Speeds, - new Pose2d(targetPos, new Rotation2d()), - NOMINAL_SHOT_TIME_S - ); + // shotData = ShotCompensation.compensateForMovement( + // turretPivotPose, + // drive.getState().Speeds, + // new Pose2d(targetPos, new Rotation2d()), + // NOMINAL_SHOT_TIME_S + // ); + + if (DriverStation.isAutonomous()) { + setTargetingHub(); + } Logger.recordOutput("Targeting/TurretPivot", turretPivotPose); } @@ -85,10 +97,37 @@ public void setTargetingHub() { new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SCORING mode") ); Elastic.selectTab("Scoring Mode"); - if (DriverStation.getAlliance().isPresent() &&DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { + + overrideBlueAlliance = SmartDashboard.getBoolean("OverrideBlueAlliance", overrideBlueAlliance); + overrideRedAlliance = SmartDashboard.getBoolean("OverrideRedAlliance", overrideRedAlliance); + + // if (!DriverStation.getAlliance().isPresent() && isRedAlliance) { + // if (isRedAlliance) { + // targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + // } else { + // targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + // } + // return; + // } + + if (overrideRedAlliance) { targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); - } else { + return; + } + + if (overrideBlueAlliance) { targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + return; + } + + + if (DriverStation.getAlliance().isPresent()) { + if (DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { + targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); + } + if (DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue)) { + targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + } } } From 1399d4986257732796f7cca4b6124a498480b502 Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Sun, 22 Mar 2026 08:29:30 -0400 Subject: [PATCH 28/29] Fix merge problems --- .../subsystems/turret/TurretIOTalonFX.java | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index b001a84..4b3dc67 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -15,10 +15,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; -import frc.lib.AngleUtils; import frc.lib.LowPassFilter; import frc.robot.CanID; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; @@ -154,25 +151,10 @@ private double calculateFeedforward() { return feedforward.calculate(-mechanismRotationsPerSecond); } - @Override - public void refreshData() { - StatusSignal.refreshAll(this.turretMotorPosition, this.pinionEncoderSignal, this.followerEncoderSignal); - } - - @Override - public void recalculateTurretMotorZeroPosition() { - // ensure the cache is fresh before using it for zeroing - this.cachedPinionEncoderRevs = computePinionEncoderRevs(); - // absolute turret position from CRT - this.calculatedMotorOffsetRevs = getTurretAngle() / 180.0; - turretMotor.setPosition(this.calculatedMotorOffsetRevs); - } - @Override public void refreshData() { StatusSignal.refreshAll(this.turretMotorPosition, this.pinionEncoderSignal, this.followerEncoderSignal); - this.cachedPinionEncoderRevs = computePinionEncoderRevs(); } @Override From b48a599c677eb873dd340d347692db81f11ac21a Mon Sep 17 00:00:00 2001 From: NathanEdg Date: Sun, 22 Mar 2026 08:30:22 -0400 Subject: [PATCH 29/29] Stop flywheel from always spinning, tune intake --- .../frc/robot/subsystems/intake/Intake.java | 18 +++++++++--------- .../subsystems/intake/IntakeIOTalonFX.java | 8 ++++++-- .../frc/robot/subsystems/shooter/Shooter.java | 6 +++++- 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 3474252..fa22074 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -79,19 +79,19 @@ private void doDeployingState() { // Run the DEPLOYED state periodic actions private void doDeployedState() { - // Get the absolute velocity of the entire robot - double robotAbsoluteVelocity = Math.abs(drivetrain.getState().Speeds.vxMetersPerSecond); + // // Get the absolute velocity of the entire robot + // double robotAbsoluteVelocity = Math.abs(drivetrain.getState().Speeds.vxMetersPerSecond); - // Adjust the intake speed, increase the intake as the robot moves faster - // Cap at MAX_SPEED - double speed = Math.min(BASE_SPEED_INTAKE + robotAbsoluteVelocity * SPEED_PER_MPS, MAX_SPEED); + // // Adjust the intake speed, increase the intake as the robot moves faster + // // Cap at MAX_SPEED + // double speed = Math.min(BASE_SPEED_INTAKE + robotAbsoluteVelocity * SPEED_PER_MPS, MAX_SPEED); - if (forward == false) { - speed *= -1; - } + // if (forward == false) { + // speed *= -1; + // } // Update the intakeIO on speed - intakeIO.setIntakeSpeed(speed); + intakeIO.setIntakeSpeed(70); } // Run the RETRACTING state periodic actions diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java index 45b7438..0d5535d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java @@ -70,7 +70,9 @@ public void updateInputs(IntakeIOInputs inputs) { // double speed - Speed to set the motor to in Rotations Per Second @Override public void setIntakeSpeed(double speed) { - intakeMotor.set(speed); + // intakeMotor.set(speed); + this.velocityControl.withVelocity(speed); + intakeMotor.setControl(this.velocityControl); } // Set the speed of the deploy motor @@ -107,10 +109,12 @@ public static TalonFXConfiguration getIntakeMotorConfig() { intakeConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // Intake motor PID values - intakeConfig.Slot0.kP = 1; + intakeConfig.Slot0.kP = 0.6; intakeConfig.Slot0.kI = 0.0; intakeConfig.Slot0.kD = 0.0; + intakeConfig.Slot0.kV = 0.15; + return intakeConfig; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a582612..00b2455 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -64,7 +64,11 @@ public void onPeriodic() { // put to recovery mode if the driver is requesting to shoot // more direct control rather than smooth trajectory generation - shooterIO.setFlywheelVelocity(targetRPM / 60.0); // convert RPM to RPS + if (driverRequestingShooting) { + shooterIO.setFlywheelVelocity(targetRPM / 60.0); // convert RPM to RPS + } else { + shooterIO.setFlywheelVelocity(0); + } } /**