From f322aa24918d1fe05f5b9d097ae217edbcf746c3 Mon Sep 17 00:00:00 2001 From: team3492backup Date: Sun, 15 Feb 2026 17:03:42 -0500 Subject: [PATCH 01/16] Add gear ratio (WIP) --- src/main/java/frc/robot/constants/IntakeConstants.java | 2 ++ src/main/java/frc/robot/subsystems/Intake/IntakePhys.java | 6 +++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index a5f1883..eabdc1b 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -3,4 +3,6 @@ public class IntakeConstants { public static final int INTAKE_MOTOR_ID = 36; public static final int PIVOT_MOTOR_ID = 38; + + public static final double PIVOT_GEAR_RATIO = 12/1; } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java index 9827ede..b647a2c 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java @@ -25,6 +25,10 @@ public IntakePhys() { intakeConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; intakeMotor.getConfigurator().apply(intakeConfig); pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); + TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); + pivotMotor.getConfigurator().apply(pivotConfig); + pivotMotor.getConfigurator().setPosition(0); + } @Override @@ -44,7 +48,7 @@ public double getIntakeSpeed() { @Override public PARTsUnit getPivotPosition() { - return new PARTsUnit(pivotMotor.getPosition().getValueAsDouble(), PARTsUnitType.Rotations); + return new PARTsUnit(pivotMotor.getPosition().getValueAsDouble() / IntakeConstants.PIVOT_GEAR_RATIO, PARTsUnitType.Rotations); } @Override From 5d9b44c23f80b253a0a6f422f718454deb364075 Mon Sep 17 00:00:00 2001 From: team3492backup Date: Fri, 20 Feb 2026 17:52:41 -0500 Subject: [PATCH 02/16] Add new gear ratios for intake and turret. (Also add note for intake.) --- src/main/java/frc/robot/constants/IntakeConstants.java | 3 ++- src/main/java/frc/robot/constants/TurretConstants.java | 2 ++ src/main/java/frc/robot/subsystems/Intake/IntakePhys.java | 4 ++++ 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index eabdc1b..38f098e 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -4,5 +4,6 @@ public class IntakeConstants { public static final int INTAKE_MOTOR_ID = 36; public static final int PIVOT_MOTOR_ID = 38; - public static final double PIVOT_GEAR_RATIO = 12/1; + /** The pivot gear ratio. The total is {@code 36/1}. */ + public static final double PIVOT_GEAR_RATIO = (12/1)*(3/1); } diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java index d660436..d26596e 100644 --- a/src/main/java/frc/robot/constants/TurretConstants.java +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -2,6 +2,8 @@ public class TurretConstants { public static final int TURRET_MOTOR_ID = 39; + /** The turret gear ratio. The total ratio is {@code 40/1}. */ + public static final double TURRET_GEAR_RATIO = (200/1)*(1/20)*(4/1); // PID Controller public static final double P = 0; diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java index b647a2c..aa517a7 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java @@ -14,6 +14,10 @@ import frc.robot.constants.IntakeConstants; +/** + * The physical intake subsystem.

+ * WARNING: The pivot arm MUST be in home position when the robot starts. + */ public class IntakePhys extends Intake { TalonFX intakeMotor; TalonFX pivotMotor; From 2acbafebb818bac4982668551cae5389c42ec8fb Mon Sep 17 00:00:00 2001 From: team3492backup Date: Fri, 20 Feb 2026 20:05:46 -0500 Subject: [PATCH 03/16] small work to make subsystems work --- .../frc/robot/constants/IntakeConstants.java | 2 +- .../frc/robot/constants/ShooterConstants.java | 2 +- .../frc/robot/constants/TurretConstants.java | 2 +- .../robot/subsystems/Shooter/ShooterPhys.java | 17 +++++++++-------- .../frc/robot/subsystems/Turret/TurretPhys.java | 10 +++++----- .../frc/robot/subsystems/Turret/TurretSim.java | 5 ++++- 6 files changed, 21 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 38f098e..748bf4b 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -5,5 +5,5 @@ public class IntakeConstants { public static final int PIVOT_MOTOR_ID = 38; /** The pivot gear ratio. The total is {@code 36/1}. */ - public static final double PIVOT_GEAR_RATIO = (12/1)*(3/1); + public static final double PIVOT_GEAR_RATIO = (12.0/1.0)*(3.0/1.0); } diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index f71a32a..0e5ccaa 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -6,7 +6,7 @@ public class ShooterConstants { public static final double SHOOTER_RPM = 3000; public static final int LEFT_MOTOR_ID = 33; - public static final int RIGHT_MOTOR_ID = 0; + public static final int RIGHT_MOTOR_ID = 35; public static final PARTsUnit SHOOTER_WHEEL_RADIUS = new PARTsUnit(1.5, PARTsUnitType.Inch); diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java index d26596e..c2bd2a0 100644 --- a/src/main/java/frc/robot/constants/TurretConstants.java +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -3,7 +3,7 @@ public class TurretConstants { public static final int TURRET_MOTOR_ID = 39; /** The turret gear ratio. The total ratio is {@code 40/1}. */ - public static final double TURRET_GEAR_RATIO = (200/1)*(1/20)*(4/1); + public static final double TURRET_GEAR_RATIO = (200.0/1.0)*(1.0/20.0)*(4.0/1.0); // PID Controller public static final double P = 0; diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java index e57bcf9..a0c927d 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.Shooter; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -11,7 +12,7 @@ public class ShooterPhys extends Shooter { protected final TalonFX leftMotor; - //protected final TalonFX rightMotor; + protected final TalonFX rightMotor; public ShooterPhys() { super(); @@ -19,27 +20,27 @@ public ShooterPhys() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; leftMotor = new TalonFX(ShooterConstants.LEFT_MOTOR_ID); leftMotor.getConfigurator().apply(config); - //rightMotor = new TalonFX(ShooterConstants.RIGHT_MOTOR_ID); + rightMotor = new TalonFX(ShooterConstants.RIGHT_MOTOR_ID); //rightMotor.setControl(new Follower(ShooterConstants.LEFT_MOTOR_ID, MotorAlignmentValue.Opposed)); leftMotor.setNeutralMode(NeutralModeValue.Coast); - //rightMotor.setNeutralMode(NeutralModeValue.Coast); + rightMotor.setNeutralMode(NeutralModeValue.Coast); } @Override public void outputTelemetry() { super.outputTelemetry(); partsNT.putDouble("Current/Left", leftMotor.getSupplyCurrent().getValueAsDouble()); - //partsNT.putDouble("Current/Right", rightMotor.getSupplyCurrent().getValueAsDouble()); + partsNT.putDouble("Current/Right", rightMotor.getSupplyCurrent().getValueAsDouble()); partsNT.putDouble("Output/Left", leftMotor.getStatorCurrent().getValueAsDouble()); - //partsNT.putDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); + partsNT.putDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); } @Override protected void setSpeed(double speed) { - leftMotor.set(speed); + rightMotor.set(speed); } @Override @@ -66,9 +67,9 @@ public void periodic() { public void log() { super.log(); partsLogger.logDouble("Current/Left", leftMotor.getSupplyCurrent().getValueAsDouble()); - //partsLogger.logDouble("Current/Right", rightMotor.getSupplyCurrent().getValueAsDouble()); + partsLogger.logDouble("Current/Right", rightMotor.getSupplyCurrent().getValueAsDouble()); partsLogger.logDouble("Output/Left", leftMotor.getStatorCurrent().getValueAsDouble()); - //partsLogger.logDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); + partsLogger.logDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); } } diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java index d864cdc..52dba55 100644 --- a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java +++ b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java @@ -2,6 +2,7 @@ import java.util.function.Supplier; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; @@ -20,11 +21,10 @@ public class TurretPhys extends Turret { public TurretPhys(Supplier robotPoseSupplier) { super(robotPoseSupplier); - SparkMaxConfig turretConfig = new SparkMaxConfig(); - turretConfig.idleMode(IdleMode.kCoast); - turretConfig.inverted(true); - turretMotor = new TalonFX(TurretConstants.TURRET_MOTOR_ID); + TalonFXConfiguration turretConfig = new TalonFXConfiguration(); + turretMotor.getConfigurator().apply(turretConfig); + turretMotor.setPosition(100); } @Override @@ -65,6 +65,6 @@ public void log() { @Override protected double getAngle() { - return turretMotor.getPosition().getValueAsDouble() * 360 % 360; + return turretMotor.getPosition().getValueAsDouble() * 360 / TurretConstants.TURRET_GEAR_RATIO; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretSim.java b/src/main/java/frc/robot/subsystems/Turret/TurretSim.java index fe27453..e60798a 100644 --- a/src/main/java/frc/robot/subsystems/Turret/TurretSim.java +++ b/src/main/java/frc/robot/subsystems/Turret/TurretSim.java @@ -2,13 +2,16 @@ import java.util.function.Supplier; +import com.ctre.phoenix6.hardware.TalonFX; + import edu.wpi.first.math.geometry.Pose2d; public class TurretSim extends Turret { + //protected final TalonFX turretMotor; public TurretSim(Supplier robotPoseSupplier) { super(robotPoseSupplier); -} + } @Override protected void setSpeed(double speed) { From 5a936d5209e9b08e3f60516c8c72f06965e19257 Mon Sep 17 00:00:00 2001 From: team3492backup Date: Sat, 21 Feb 2026 12:04:21 -0500 Subject: [PATCH 04/16] before lunch changes --- src/main/java/frc/robot/RobotContainer.java | 34 +++++++++-- .../frc/robot/constants/CandleConstants.java | 1 + .../robot/constants/DrivetrainConstants.java | 6 -- .../frc/robot/constants/HopperConstants.java | 1 + .../frc/robot/constants/IntakeConstants.java | 12 ++++ .../frc/robot/constants/KickerConstants.java | 1 + .../frc/robot/constants/ShooterConstants.java | 1 + .../frc/robot/constants/TurretConstants.java | 1 + .../frc/robot/constants/VisionConstants.java | 11 ---- .../java/frc/robot/states/CandleState.java | 2 +- .../java/frc/robot/states/IntakeState.java | 18 ++++-- .../java/frc/robot/subsystems/Candle.java | 2 +- .../frc/robot/subsystems/Hopper/Hopper.java | 6 -- .../robot/subsystems/Hopper/HopperPhys.java | 2 +- .../frc/robot/subsystems/Intake/Intake.java | 56 ++++++++++++++--- .../robot/subsystems/Intake/IntakePhys.java | 24 +++++--- .../robot/subsystems/Intake/IntakeSim.java | 16 +++-- .../robot/subsystems/Intake/IntakeSysid.java | 61 +++++++++++++++++++ .../frc/robot/subsystems/Kicker/Kicker.java | 4 -- .../robot/subsystems/Kicker/KickerPhys.java | 2 +- .../frc/robot/subsystems/LimelightVision.java | 2 + .../frc/robot/subsystems/Shooter/Shooter.java | 54 +++++++++------- .../robot/subsystems/Shooter/ShooterPhys.java | 8 +-- .../subsystems/Shooter/ShooterSysid.java | 2 - .../frc/robot/subsystems/Turret/Turret.java | 50 ++++++++++----- .../robot/subsystems/Turret/TurretPhys.java | 4 +- 26 files changed, 269 insertions(+), 112 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Intake/IntakeSysid.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 52b78a3..ad60c34 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -45,6 +45,7 @@ import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.Intake.IntakePhys; import frc.robot.subsystems.Intake.IntakeSim; +import frc.robot.subsystems.Intake.IntakeSysid; import frc.robot.subsystems.Kicker.Kicker; import frc.robot.subsystems.Kicker.KickerPhys; import frc.robot.subsystems.Kicker.KickerSim; @@ -77,7 +78,7 @@ public class RobotContainer { private static Alliance alliance; - //region Subsystems + // region Subsystems public final PARTsDrivetrain drivetrain = new PARTsDrivetrain( TunerConstants.DrivetrainConstants, @@ -99,25 +100,29 @@ public class RobotContainer { private final Hopper hopper = Robot.isReal() ? new HopperPhys() : new HopperSim(); - private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim(); + //private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim(); + // private final ShooterSysid shooter = new ShooterSysid(); //for sysid + private final IntakeSysid intake = new IntakeSysid(); //for sysid private final ArrayList subsystems = new ArrayList( Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake)); - //endregion End Subsystems + // endregion End Subsystems public RobotContainer() { configureDrivetrainBindings(); configureCandleBindings(); configureShooterBindings(); + configureTurretBindings(); configureAutonomousCommands(); + configureIntakeBindings(); partsNT.putSmartDashboardSendable("field", Field.FIELD2D); hubFieldObject2d = Field.FIELD2D.getObject("hub"); } - //region Configs + // region Configs private void configureDrivetrainBindings() { @@ -196,18 +201,35 @@ private void configureCandleBindings() { } + private void configureTurretBindings() { + //operatorController.a().onTrue(turret.track()); + //operatorController.b().onTrue(turret.idle()); + } + + private void configureIntakeBindings() { + operatorController.a().and(operatorController.rightBumper()) + .whileTrue(intake.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + operatorController.b().and(operatorController.rightBumper()) + .whileTrue(intake.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + operatorController.x().and(operatorController.rightBumper()) + .whileTrue(intake.sysIdDynamic(SysIdRoutine.Direction.kForward)); + operatorController.y().and(operatorController.rightBumper()) + .whileTrue(intake.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + } + public void configureAutonomousCommands() { autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); } - //endregion End Configs + // endregion End Configs public Command getAutonomousCommand() { return autoChooser.getSelected(); } - //region Custom Public Functions + // region Custom Public Functions public void outputTelemetry() { subsystems.forEach(s -> s.outputTelemetry()); diff --git a/src/main/java/frc/robot/constants/CandleConstants.java b/src/main/java/frc/robot/constants/CandleConstants.java index 6943c83..8cffec0 100644 --- a/src/main/java/frc/robot/constants/CandleConstants.java +++ b/src/main/java/frc/robot/constants/CandleConstants.java @@ -3,4 +3,5 @@ public class CandleConstants { public static final int CAN_ID = 33; public static final int LED_LENGTH = 1000; + public static final String CAN_BUS_NAME = "bye"; } diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 8616a4a..422970c 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -13,12 +13,6 @@ public class DrivetrainConstants { public static final double MAX_RANGE_VELOCITY = 2;// m/s public static final double MAX_RANGE_ACCELERATION = 2;// m/2^s - /*SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND = 5.41; - SWERVE_CONSTANTS.MAX_DRIVING_ACCELERATION_METERS_PER_SECOND_SQUARED = 32; - SWERVE_CONSTANTS.MAX_STEER_VELOCITY_RADIANS_PER_SECOND = 125.5 * 2 * Math.PI; - // max velocity in 1/10 sec - SWERVE_CONSTANTS.MAX_STEER_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 10 * 125.5 * 2 * Math.PI;*/ - public static final double THETA_P = 10; // Proprotinal //4.5 15 public static final double THETA_I = 0; // Gradual corretction public static final double THETA_D = 0; // Smooth oscilattions diff --git a/src/main/java/frc/robot/constants/HopperConstants.java b/src/main/java/frc/robot/constants/HopperConstants.java index 54aabd4..b55cf88 100644 --- a/src/main/java/frc/robot/constants/HopperConstants.java +++ b/src/main/java/frc/robot/constants/HopperConstants.java @@ -4,4 +4,5 @@ public class HopperConstants { public static final double ROLL_SPEED = 0.3; public static final double BACKROLL_SPEED = -0.3; public static final int HOPPER_MOTOR_ID = 37; + public static final String CAN_BUS_NAME = "bye"; } diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 748bf4b..0546ecb 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -3,7 +3,19 @@ public class IntakeConstants { public static final int INTAKE_MOTOR_ID = 36; public static final int PIVOT_MOTOR_ID = 38; + public static final String CAN_BUS_NAME = "bye"; /** The pivot gear ratio. The total is {@code 36/1}. */ public static final double PIVOT_GEAR_RATIO = (12.0/1.0)*(3.0/1.0); + + // PID Controller + public static final double P = 0; + public static final double I = 0; + public static final double D = 0; + public static final int PID_THRESHOLD = 5; + + // Feedforward + public static final double S = 0; + public static final double V = 0; + public static final double A = 0; } diff --git a/src/main/java/frc/robot/constants/KickerConstants.java b/src/main/java/frc/robot/constants/KickerConstants.java index ec9b887..8e23155 100644 --- a/src/main/java/frc/robot/constants/KickerConstants.java +++ b/src/main/java/frc/robot/constants/KickerConstants.java @@ -3,4 +3,5 @@ public class KickerConstants { public static final double KICKER_SPEED = 0.1; // TBD public static final int KICKER_MOTOR_ID = 34; + public static final String CAN_BUS_NAME = "bye"; } diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index 0e5ccaa..ea59de5 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -7,6 +7,7 @@ public class ShooterConstants { public static final double SHOOTER_RPM = 3000; public static final int LEFT_MOTOR_ID = 33; public static final int RIGHT_MOTOR_ID = 35; + public static final String CAN_BUS_NAME = "bye"; public static final PARTsUnit SHOOTER_WHEEL_RADIUS = new PARTsUnit(1.5, PARTsUnitType.Inch); diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java index c2bd2a0..28c907d 100644 --- a/src/main/java/frc/robot/constants/TurretConstants.java +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -2,6 +2,7 @@ public class TurretConstants { public static final int TURRET_MOTOR_ID = 39; + public static final String CAN_BUS_NAME = "bye"; /** The turret gear ratio. The total ratio is {@code 40/1}. */ public static final double TURRET_GEAR_RATIO = (200.0/1.0)*(1.0/20.0)*(4.0/1.0); diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 8a99127..ac8463c 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -2,19 +2,8 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N3; -import org.parts3492.partslib.PARTsUnit; -import org.parts3492.partslib.PARTsUnit.PARTsUnitType; public class VisionConstants { - /*public static final String FRONT_LIMELIGHT = "limelight-slimmy"; // The_Real - public static final String BACK_LIMELIGHT = "limelight-thereal"; - public static final PARTsUnit LIMELIGHT_LENS_HEIGHT = new PARTsUnit(9, PARTsUnitType.Inch); // Inches - public static final PARTsUnit LIMELIGHT_ANGLE = new PARTsUnit(0, PARTsUnitType.Angle); // Degrees - - //public static final double REEF_APRILTAG_HEIGHT = 16; // Distance.ofBaseUnits(6.875, Inches); - //public static final double PROCCESSOR_APRILTAG_HEIGHT = 45.875; // Inches - //public static final double CORAL_APRILTAG_HEIGHT = 53.25; // Inches*/ - public static final edu.wpi.first.math.Vector MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0); public static final edu.wpi.first.math.Vector MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 3492); } diff --git a/src/main/java/frc/robot/states/CandleState.java b/src/main/java/frc/robot/states/CandleState.java index 8644efa..8a7f395 100644 --- a/src/main/java/frc/robot/states/CandleState.java +++ b/src/main/java/frc/robot/states/CandleState.java @@ -4,7 +4,7 @@ package frc.robot.states; -/** Add your docs here. */ +/* The state the Candle is in */ public enum CandleState { IDLE, DISABLED, diff --git a/src/main/java/frc/robot/states/IntakeState.java b/src/main/java/frc/robot/states/IntakeState.java index 8166198..03ad38f 100644 --- a/src/main/java/frc/robot/states/IntakeState.java +++ b/src/main/java/frc/robot/states/IntakeState.java @@ -1,18 +1,26 @@ package frc.robot.states; +/** The state the Intake is in */ public enum IntakeState { - IDLE(0), - DISABLED(0), - INTAKING(0), - OUTTAKING(0); + IDLE(0, 0), + DISABLED(0, 0), + INTAKING(0.1, 90), + OUTTAKING(-0.1, 90), + SHOOTING(0, 15); private double speed; + private double angle; - private IntakeState(double speed){ + private IntakeState(double speed, double angle){ this.speed = speed; + this.angle = angle; } public double getSpeed(){ return speed; } + + public double getAngle() { + return angle; + } } diff --git a/src/main/java/frc/robot/subsystems/Candle.java b/src/main/java/frc/robot/subsystems/Candle.java index f14b119..dca0ed3 100644 --- a/src/main/java/frc/robot/subsystems/Candle.java +++ b/src/main/java/frc/robot/subsystems/Candle.java @@ -20,7 +20,7 @@ public class Candle extends PARTsCandle { private Set candleStates = new HashSet<>(); public Candle() { - super("Candle", CandleConstants.CAN_ID, CandleConstants.LED_LENGTH); + super("Candle", CandleConstants.CAN_ID, CandleConstants.LED_LENGTH, CandleConstants.CAN_BUS_NAME); } /*---------------------------------- Custom Public Functions ----------------------------------*/ diff --git a/src/main/java/frc/robot/subsystems/Hopper/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper/Hopper.java index 9863277..e68a052 100644 --- a/src/main/java/frc/robot/subsystems/Hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper/Hopper.java @@ -47,14 +47,8 @@ public void periodic() { else { switch(hopperstate) { case DISABLED: - setSpeed(hopperstate.getSpeed()); - break; case ROLLING: - setSpeed(hopperstate.getSpeed()); - break; case IDLE: - setSpeed(hopperstate.getSpeed()); - break; case BACKROLLING: setSpeed(hopperstate.getSpeed()); break; diff --git a/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java b/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java index 5813899..c52ee16 100644 --- a/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java +++ b/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java @@ -20,7 +20,7 @@ public HopperPhys() { super(); TalonFXConfiguration hopperConfig = new TalonFXConfiguration(); hopperConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - hopperMotor = new TalonFX(HopperConstants.HOPPER_MOTOR_ID); + hopperMotor = new TalonFX(HopperConstants.HOPPER_MOTOR_ID, HopperConstants.CAN_BUS_NAME); hopperMotor.getConfigurator().apply(hopperConfig); } diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 42da017..17a8e46 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -4,12 +4,19 @@ import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import org.parts3492.partslib.command.PARTsSubsystem; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import frc.robot.constants.IntakeConstants; import frc.robot.constants.RobotConstants; +import frc.robot.constants.ShooterConstants; import frc.robot.states.IntakeState; public abstract class Intake extends PARTsSubsystem { - IntakeState state = IntakeState.IDLE; + IntakeState intakeState = IntakeState.IDLE; + + PIDController intakePIDController; + SimpleMotorFeedforward intakeFeedForward; public Intake() { super("Intake"); @@ -18,23 +25,28 @@ public Intake() { partsNT.putDouble("Intake Speed", 0); partsNT.putDouble("Pivot Speed", 0); } + + intakePIDController = new PIDController(IntakeConstants.P, IntakeConstants.I, IntakeConstants.D); + intakeFeedForward = new SimpleMotorFeedforward(IntakeConstants.S, IntakeConstants.V, IntakeConstants.A); + intakePIDController.setTolerance(IntakeConstants.PID_THRESHOLD); + } @Override public void outputTelemetry() { - partsNT.putDouble("Pivot Position", getPivotPosition().to(PARTsUnitType.Angle)); + partsNT.putDouble("Pivot Position", getPivotAngle().to(PARTsUnitType.Angle)); partsNT.putDouble("Current Intake Speed", getIntakeSpeed()); - partsNT.putString("Intake State", state.toString()); + partsNT.putString("Intake State", intakeState.toString()); } @Override public void stop() { - state = IntakeState.DISABLED; + intakeState = IntakeState.DISABLED; } @Override public void reset() { - state = IntakeState.IDLE; + intakeState = IntakeState.IDLE; } @Override @@ -42,23 +54,47 @@ public void periodic() { if (RobotConstants.DEBUGGING) { setIntakeSpeed(partsNT.getDouble("Intake Speed")); setPivotSpeed(partsNT.getDouble("Pivot Speed")); + } else { + switch (intakeState) { + case DISABLED: + case IDLE: + setIntakeSpeed(intakeState.getSpeed()); + setPivotSpeed(0); + break; + case INTAKING: + case OUTTAKING: + case SHOOTING: + setIntakeSpeed(intakeState.getSpeed()); + intakePIDController.setSetpoint(intakeState.getAngle()); + double pidCalc = intakePIDController.calculate(getPivotAngle().to(PARTsUnitType.Angle), intakeState.getAngle()); + double ffCalc = intakeFeedForward.calculate(intakePIDController.getSetpoint()); + + setPivotVoltage(pidCalc + ffCalc); + break; + default: + setIntakeSpeed(0); + setPivotSpeed(0); + break; + } } } @Override public void log() { - partsLogger.logDouble("Pivot Position", getPivotPosition().to(PARTsUnitType.Angle)); + partsLogger.logDouble("Pivot Position", getPivotAngle().to(PARTsUnitType.Angle)); partsLogger.logDouble("Intake Speed", getIntakeSpeed()); - partsLogger.logString("Intake State", state.toString()); + partsLogger.logString("Intake State", intakeState.toString()); } public abstract void setIntakeSpeed(double speed); public abstract void setPivotSpeed(double speed); - public abstract void setPivotPosition(PARTsUnit position); - public abstract double getIntakeSpeed(); - public abstract PARTsUnit getPivotPosition(); + public abstract PARTsUnit getPivotAngle(); + + public abstract void setPivotVoltage(double voltage); + + public abstract double getPivotRotationSpeed(); } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java index aa517a7..f2a4a37 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java @@ -19,16 +19,17 @@ * WARNING: The pivot arm MUST be in home position when the robot starts. */ public class IntakePhys extends Intake { - TalonFX intakeMotor; - TalonFX pivotMotor; + protected final TalonFX intakeMotor; + protected final TalonFX pivotMotor; public IntakePhys() { super(); TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); - intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); + intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID, IntakeConstants.CAN_BUS_NAME); intakeConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; intakeMotor.getConfigurator().apply(intakeConfig); - pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); + + pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID, IntakeConstants.CAN_BUS_NAME); TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); pivotMotor.getConfigurator().apply(pivotConfig); pivotMotor.getConfigurator().setPosition(0); @@ -41,17 +42,17 @@ public void setIntakeSpeed(double speed) { } @Override - public void setPivotPosition(PARTsUnit position) { - pivotMotor.setPosition(position.to(PARTsUnitType.Rotations)); + public double getIntakeSpeed() { + return intakeMotor.get(); } @Override - public double getIntakeSpeed() { - return intakeMotor.get(); + public double getPivotRotationSpeed() { + return pivotMotor.getVelocity().getValueAsDouble(); } @Override - public PARTsUnit getPivotPosition() { + public PARTsUnit getPivotAngle() { return new PARTsUnit(pivotMotor.getPosition().getValueAsDouble() / IntakeConstants.PIVOT_GEAR_RATIO, PARTsUnitType.Rotations); } @@ -91,4 +92,9 @@ public void log() { public void setPivotSpeed(double speed) { pivotMotor.set(speed); } + + @Override + public void setPivotVoltage(double voltage) { + pivotMotor.setVoltage(voltage); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java b/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java index 2f7ae9f..3c31b3e 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java @@ -13,18 +13,22 @@ public void setIntakeSpeed(double speed) { public void setPivotSpeed(double speed) { } - @Override - public void setPivotPosition(PARTsUnit position) { - } - @Override public double getIntakeSpeed() { return 0; } @Override - public PARTsUnit getPivotPosition() { + public PARTsUnit getPivotAngle() { return new PARTsUnit(0, PARTsUnitType.Angle); } - + + @Override + public void setPivotVoltage(double voltage) { + } + + @Override + public double getPivotRotationSpeed() { + return 0; + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeSysid.java b/src/main/java/frc/robot/subsystems/Intake/IntakeSysid.java new file mode 100644 index 0000000..4719fc2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeSysid.java @@ -0,0 +1,61 @@ +package frc.robot.subsystems.Intake; + +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import org.parts3492.partslib.PARTsUnit.PARTsUnitType; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class IntakeSysid extends IntakePhys { + + private MutVoltage appliedVoltage; + + private MutAngle pivotAngle; + + private MutAngularVelocity pivotVelocity; + + private SysIdRoutine routine; + + public IntakeSysid() { + super(); + + appliedVoltage = Volts.mutable(0); + + pivotAngle = Units.Radian.mutable(0); + + pivotVelocity = Units.RadiansPerSecond.mutable(0); + + routine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (voltage) -> this.setPivotVoltage(voltage.in(Volts)), + + log -> { + // Record a frame for the shooter motor. + log.motor("pivotarm") + .voltage( + appliedVoltage.mut_replace( + super.pivotMotor.getStatorCurrent().getValueAsDouble(), Volts)) + .angularPosition(pivotAngle.mut_replace( + getPivotAngle().toPARTsUnit(PARTsUnitType.Angle).to(PARTsUnitType.Radian), Radians)) + .angularVelocity( + pivotVelocity.mut_replace(getPivotRotationSpeed(), RotationsPerSecond)); + }, + this)); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return routine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return routine.dynamic(direction); + } +} diff --git a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java index c3f9e7b..ec6c2c4 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java +++ b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java @@ -50,11 +50,7 @@ public void periodic() { } else { switch (kickerState) { case ROLLING: - setSpeed(kickerState.getSpeed()); - break; case DISABLED: - setSpeed(kickerState.getSpeed()); - break; case IDLE: setSpeed(kickerState.getSpeed()); break; diff --git a/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java b/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java index 501b4b7..f93d13e 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java +++ b/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java @@ -19,7 +19,7 @@ public KickerPhys() { super(); TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - kickerMotor = new TalonFX(KickerConstants.KICKER_MOTOR_ID); + kickerMotor = new TalonFX(KickerConstants.KICKER_MOTOR_ID, KickerConstants.CAN_BUS_NAME); kickerMotor.getConfigurator().apply(config); } diff --git a/src/main/java/frc/robot/subsystems/LimelightVision.java b/src/main/java/frc/robot/subsystems/LimelightVision.java index 84eeea7..a4ac581 100644 --- a/src/main/java/frc/robot/subsystems/LimelightVision.java +++ b/src/main/java/frc/robot/subsystems/LimelightVision.java @@ -205,6 +205,8 @@ public void periodic() { 0, 0, 0); + double [] hw = LimelightHelpers.getLimelightDoubleArrayEntry("limelight", "hw").get(); + partsNT.putDouble(camera.getName() + "/temp", hw.length > 0 ? hw [0]: -1); if (camera.isEnabled()) { PoseEstimate poseEstimate = (megaTagMode == MegaTagMode.MEGATAG2) ? getMegaTag2PoseEstimate(camera.getName()) diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java index f1e30f1..6a5d8cd 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 @@ import frc.robot.states.ShooterState; import org.parts3492.partslib.command.PARTsSubsystem; -public abstract class Shooter extends PARTsSubsystem{ +public abstract class Shooter extends PARTsSubsystem { private ShooterState shooterState = ShooterState.IDLE; private PIDController shooterPIDController; @@ -30,7 +30,7 @@ public Shooter() { shooterPIDController.setTolerance(ShooterConstants.PID_THRESHOLD); } - //region Generic Subsystem Functions + // region Generic Subsystem Functions @Override public void outputTelemetry() { partsNT.putString("Shooter State", shooterState.toString()); @@ -56,31 +56,41 @@ public void log() { partsLogger.logString("Shooter State", shooterState.toString()); } - - @Override public void periodic() { if (RobotConstants.DEBUGGING) { setSpeed(partsNT.getDouble("Shooter Speed")); - } - else { - double voltage = 0; - shooterPIDController.setSetpoint(shooterState.getRPM()); - - double pidCalc = shooterPIDController.calculate(getRPM(), shooterState.getRPM()); - double ffCalc = shooterFeedforward.calculate((shooterPIDController.getSetpoint() * Math.PI * ShooterConstants.SHOOTER_WHEEL_RADIUS.to(PARTsUnitType.Meter) * 2) / 60); - - voltage = pidCalc + ffCalc; - - setVoltage(voltage); + } else { + switch (shooterState) { + case CHARGING: + case DISABLED: + case IDLE: + case SHOOTING: + double voltage = 0; + shooterPIDController.setSetpoint(shooterState.getRPM()); + + double pidCalc = shooterPIDController.calculate(getRPM(), shooterState.getRPM()); + double ffCalc = shooterFeedforward.calculate((shooterPIDController.getSetpoint() * Math.PI + * ShooterConstants.SHOOTER_WHEEL_RADIUS.to(PARTsUnitType.Meter) * 2) / 60); + + voltage = pidCalc + ffCalc; + + setVoltage(voltage); + break; + default: + setSpeed(0); + break; + } } } - //endregion + // endregion - //region Custom Public Functions - /** Sets the speed of the Shooter. + // region Custom Public Functions + /** + * Sets the speed of the Shooter. + * * @param speed The speed between -1.0 and 1.0. - */ + */ protected abstract void setSpeed(double speed); protected abstract void setVoltage(double voltage); @@ -89,7 +99,9 @@ public void periodic() { protected abstract double getRPM(); - public ShooterState getState() { return shooterState; } + public ShooterState getState() { + return shooterState; + } public Command shoot() { return PARTsCommandUtils.setCommandName("Command Shoot", this.runOnce(() -> { @@ -102,5 +114,5 @@ public Command idle() { shooterState = ShooterState.IDLE; })); } - //endregion + // endregion } diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java index a0c927d..4ec9109 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java @@ -18,11 +18,11 @@ public ShooterPhys() { super(); TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - leftMotor = new TalonFX(ShooterConstants.LEFT_MOTOR_ID); + leftMotor = new TalonFX(ShooterConstants.LEFT_MOTOR_ID, ShooterConstants.CAN_BUS_NAME); leftMotor.getConfigurator().apply(config); - rightMotor = new TalonFX(ShooterConstants.RIGHT_MOTOR_ID); + rightMotor = new TalonFX(ShooterConstants.RIGHT_MOTOR_ID, ShooterConstants.CAN_BUS_NAME); - //rightMotor.setControl(new Follower(ShooterConstants.LEFT_MOTOR_ID, MotorAlignmentValue.Opposed)); + rightMotor.setControl(new Follower(ShooterConstants.LEFT_MOTOR_ID, MotorAlignmentValue.Opposed)); leftMotor.setNeutralMode(NeutralModeValue.Coast); rightMotor.setNeutralMode(NeutralModeValue.Coast); @@ -40,7 +40,7 @@ public void outputTelemetry() { @Override protected void setSpeed(double speed) { - rightMotor.set(speed); + leftMotor.set(speed); } @Override diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java index efa977e..95ee919 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java @@ -2,12 +2,10 @@ import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.InchesPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; -import edu.wpi.first.units.measure.MutVelocity; import edu.wpi.first.units.measure.MutVoltage; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/subsystems/Turret/Turret.java b/src/main/java/frc/robot/subsystems/Turret/Turret.java index d5cc1a3..c91d559 100644 --- a/src/main/java/frc/robot/subsystems/Turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/Turret/Turret.java @@ -27,8 +27,8 @@ public abstract class Turret extends PARTsSubsystem { public Turret(Supplier robotPoseSupplier) { super("Turret", RobotConstants.LOGGING); - if (RobotConstants.DEBUGGING) { - partsNT.putDouble("Turret Speed", 0); + if (RobotConstants.DEBUGGING) { + partsNT.putDouble("Turret Speed", 0); } this.robotPoseSupplier = robotPoseSupplier; @@ -73,14 +73,30 @@ public void periodic() { setSpeed(partsNT.getDouble("Turret Speed")); } else { double voltage = 0; - turretPIDController.setSetpoint(turretState.getAngle()); - - double pidCalc = turretPIDController.calculate(getAngle(), turretState.getAngle()); - double ffCalc = turretFeedforward.calculate(turretPIDController.getSetpoint()); - - voltage = pidCalc + ffCalc; - setVoltage(voltage); + switch (turretState) { + case DISABLED: + case IDLE: + setSpeed(0); + break; + case TRACKING: + if (Math.abs(getAngleToTarget()) <= 90) { + turretPIDController.setSetpoint(getAngleToTarget()); + double pidCalc = turretPIDController.calculate(getAngle(), turretState.getAngle()); + double ffCalc = turretFeedforward.calculate(turretPIDController.getSetpoint()); + + voltage = pidCalc + ffCalc; + + setVoltage(voltage); + } + else { + setSpeed(0); + } + break; + default: + setSpeed(0); + break; + } } } @@ -129,13 +145,15 @@ private double getAngleToTarget() { Field.getAllianceHubPose().getX() - robotPoseSupplier.get().getX()) * 180 / Math.PI); return angleToTarget; - /*if (angleToTarget > 180) { - return 180 - angleToTarget; - } else if (angleToTarget < -180) { - return 360 + angleToTarget; - } else { - return angleToTarget; - }*/ + /* + * if (angleToTarget > 180) { + * return 180 - angleToTarget; + * } else if (angleToTarget < -180) { + * return 360 + angleToTarget; + * } else { + * return angleToTarget; + * } + */ } // endregion private functions } diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java index 52dba55..88d6f12 100644 --- a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java +++ b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java @@ -21,10 +21,10 @@ public class TurretPhys extends Turret { public TurretPhys(Supplier robotPoseSupplier) { super(robotPoseSupplier); - turretMotor = new TalonFX(TurretConstants.TURRET_MOTOR_ID); + turretMotor = new TalonFX(TurretConstants.TURRET_MOTOR_ID, TurretConstants.CAN_BUS_NAME); TalonFXConfiguration turretConfig = new TalonFXConfiguration(); turretMotor.getConfigurator().apply(turretConfig); - turretMotor.setPosition(100); + turretMotor.getConfigurator().setPosition(0); } @Override From f8e3873be17299a063023c95562626f129fb4a4f Mon Sep 17 00:00:00 2001 From: team3492backup Date: Sat, 21 Feb 2026 15:00:56 -0500 Subject: [PATCH 05/16] first after lunch commit --- PARTsLib | 2 +- src/main/java/frc/robot/Robot.java | 4 -- src/main/java/frc/robot/RobotContainer.java | 58 ++++++++++++----- .../frc/robot/constants/IntakeConstants.java | 2 + .../frc/robot/constants/RobotConstants.java | 2 +- .../frc/robot/constants/TurretConstants.java | 4 +- .../java/frc/robot/states/CandleState.java | 2 +- .../java/frc/robot/states/HopperState.java | 2 +- .../java/frc/robot/states/IntakeState.java | 6 +- .../java/frc/robot/states/KickerState.java | 2 +- .../java/frc/robot/states/PivotState.java | 1 + .../java/frc/robot/states/ShooterState.java | 2 +- .../java/frc/robot/states/TurretState.java | 1 + .../Drivetrain/PARTsDrivetrain.java | 2 +- .../frc/robot/subsystems/Intake/Intake.java | 11 +++- .../robot/subsystems/Kicker/KickerPhys.java | 2 + .../frc/robot/subsystems/LimelightVision.java | 1 - .../frc/robot/subsystems/Shooter/Shooter.java | 1 - .../robot/subsystems/Shooter/ShooterPhys.java | 1 - .../frc/robot/subsystems/Turret/Turret.java | 27 ++------ .../robot/subsystems/Turret/TurretPhys.java | 9 +-- .../robot/subsystems/Turret/TurretSim.java | 2 - .../robot/subsystems/Turret/TurretSysid.java | 62 +++++++++++++++++++ 23 files changed, 138 insertions(+), 68 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Turret/TurretSysid.java diff --git a/PARTsLib b/PARTsLib index 7d8c81d..d05cbf9 160000 --- a/PARTsLib +++ b/PARTsLib @@ -1 +1 @@ -Subproject commit 7d8c81db87d1a11ba1fce7ffbf892417923578c2 +Subproject commit d05cbf9e8880d79b23d4e14f8ef67b0cad44b475 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ca98322..9600ce3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,16 +9,12 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; 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; import frc.robot.constants.RobotConstants; import frc.robot.subsystems.LimelightVision.MegaTagMode; import frc.robot.util.Hub; -import org.parts3492.partslib.network.PARTsDashboard; -import org.parts3492.partslib.network.PARTsDashboard.DashboardTab; - import com.pathplanner.lib.commands.FollowPathCommand; import org.parts3492.partslib.PARTsLogger; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ad60c34..22bfec0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -55,6 +55,7 @@ import frc.robot.subsystems.Turret.Turret; import frc.robot.subsystems.Turret.TurretPhys; import frc.robot.subsystems.Turret.TurretSim; +import frc.robot.subsystems.Turret.TurretSysid; import frc.robot.util.Field; import org.parts3492.partslib.input.PARTsButtonBoxController; @@ -100,10 +101,12 @@ public class RobotContainer { private final Hopper hopper = Robot.isReal() ? new HopperPhys() : new HopperSim(); - //private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim(); - + private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim(); + // private final ShooterSysid shooter = new ShooterSysid(); //for sysid - private final IntakeSysid intake = new IntakeSysid(); //for sysid + // private final IntakeSysid intake = new IntakeSysid(); //for sysid + // private final TurretSysid turret = new + // TurretSysid(drivetrain.supplierGetPose()); private final ArrayList subsystems = new ArrayList( Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake)); @@ -117,6 +120,7 @@ public RobotContainer() { configureTurretBindings(); configureAutonomousCommands(); configureIntakeBindings(); + configureHopperBindings(); partsNT.putSmartDashboardSendable("field", Field.FIELD2D); hubFieldObject2d = Field.FIELD2D.getObject("hub"); @@ -147,10 +151,10 @@ private void configureDrivetrainBindings() { // reset the field-centric heading on left bumper press driveController.leftBumper().onTrue(drivetrain.commandSeedFieldCentric()); - driveController.x().onTrue( - drivetrain.targetPoseCommand(() -> Field.blueHubCenter, () -> driveController.y().getAsBoolean())); - driveController.a().onTrue(drivetrain.commandSnapToAngle(90)); - driveController.b().onTrue(drivetrain.commandAlign(Field.getTag(28).getLocation().toPose2d())); + //driveController.x().onTrue( + //drivetrain.targetPoseCommand(() -> Field.blueHubCenter, () -> driveController.y().getAsBoolean())); + //driveController.a().onTrue(drivetrain.commandSnapToAngle(90)); + //driveController.b().onTrue(drivetrain.commandAlign(Field.getTag(28).getLocation().toPose2d())); /* * if (RobotConstants.DEBUGGING) { @@ -201,20 +205,39 @@ private void configureCandleBindings() { } + private void configureHopperBindings() { + //driveController.a().onTrue(hopper.roll()); + } + private void configureTurretBindings() { - //operatorController.a().onTrue(turret.track()); - //operatorController.b().onTrue(turret.idle()); + //driveController.a().onTrue(turret.track()); + //driveController.b().onTrue(turret.idle()); + + /* + * operatorController.a().and(operatorController.rightBumper()) + * .whileTrue(turret.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + * operatorController.b().and(operatorController.rightBumper()) + * .whileTrue(turret.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + * operatorController.x().and(operatorController.rightBumper()) + * .whileTrue(turret.sysIdDynamic(SysIdRoutine.Direction.kForward)); + * operatorController.y().and(operatorController.rightBumper()) + * .whileTrue(turret.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + */ } private void configureIntakeBindings() { - operatorController.a().and(operatorController.rightBumper()) - .whileTrue(intake.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - operatorController.b().and(operatorController.rightBumper()) - .whileTrue(intake.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - operatorController.x().and(operatorController.rightBumper()) - .whileTrue(intake.sysIdDynamic(SysIdRoutine.Direction.kForward)); - operatorController.y().and(operatorController.rightBumper()) - .whileTrue(intake.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + driveController.a().onTrue(intake.intake()); + + /* + * operatorController.a().and(operatorController.rightBumper()) + * .whileTrue(intake.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + * operatorController.b().and(operatorController.rightBumper()) + * .whileTrue(intake.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + * operatorController.x().and(operatorController.rightBumper()) + * .whileTrue(intake.sysIdDynamic(SysIdRoutine.Direction.kForward)); + * operatorController.y().and(operatorController.rightBumper()) + * .whileTrue(intake.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + */ } @@ -292,6 +315,7 @@ public void runOnEnabled() { setLimelightMainMode(); setIdleCandleState(); hubFieldObject2d.setPose(Field.getAllianceHubPose()); + subsystems.forEach(s -> s.reset()); CommandScheduler.getInstance().schedule(new WaitCommand(2).andThen(Commands.runOnce(() -> { /* * if (!RobotContainer.isBlue()) { diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 0546ecb..9b0f461 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -8,6 +8,8 @@ public class IntakeConstants { /** The pivot gear ratio. The total is {@code 36/1}. */ public static final double PIVOT_GEAR_RATIO = (12.0/1.0)*(3.0/1.0); + public static final double INTAKE_SPEED = 0.5; + // PID Controller public static final double P = 0; public static final double I = 0; diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 96afedb..cbcca62 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -12,6 +12,6 @@ public class RobotConstants { PARTsUnitType.Inch); public static boolean LOGGING = true; - public static boolean DEBUGGING = true; + public static boolean DEBUGGING = false; public static boolean ALLOW_AUTO_CONTROLLER_DETECTION = false; } diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java index 28c907d..64c30c3 100644 --- a/src/main/java/frc/robot/constants/TurretConstants.java +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -7,10 +7,10 @@ public class TurretConstants { public static final double TURRET_GEAR_RATIO = (200.0/1.0)*(1.0/20.0)*(4.0/1.0); // PID Controller - public static final double P = 0; + public static final double P = 0.3; public static final double I = 0; public static final double D = 0; - public static final int PID_THRESHOLD = 0; + public static final int PID_THRESHOLD = 1; // Feedforward public static final double S = 0; diff --git a/src/main/java/frc/robot/states/CandleState.java b/src/main/java/frc/robot/states/CandleState.java index 8a7f395..0db5a8d 100644 --- a/src/main/java/frc/robot/states/CandleState.java +++ b/src/main/java/frc/robot/states/CandleState.java @@ -4,7 +4,7 @@ package frc.robot.states; -/* The state the Candle is in */ +/** The state the Candle is in */ public enum CandleState { IDLE, DISABLED, diff --git a/src/main/java/frc/robot/states/HopperState.java b/src/main/java/frc/robot/states/HopperState.java index 6a5a182..a449d65 100644 --- a/src/main/java/frc/robot/states/HopperState.java +++ b/src/main/java/frc/robot/states/HopperState.java @@ -2,7 +2,7 @@ import frc.robot.constants.HopperConstants; -/* The state the hopepr is in */ +/** The state the Hopper is in */ public enum HopperState { IDLE(0), DISABLED(0), diff --git a/src/main/java/frc/robot/states/IntakeState.java b/src/main/java/frc/robot/states/IntakeState.java index 03ad38f..6f4e0be 100644 --- a/src/main/java/frc/robot/states/IntakeState.java +++ b/src/main/java/frc/robot/states/IntakeState.java @@ -1,11 +1,13 @@ package frc.robot.states; +import frc.robot.constants.IntakeConstants; + /** The state the Intake is in */ public enum IntakeState { IDLE(0, 0), DISABLED(0, 0), - INTAKING(0.1, 90), - OUTTAKING(-0.1, 90), + INTAKING(IntakeConstants.INTAKE_SPEED, 90), + OUTTAKING(-IntakeConstants.INTAKE_SPEED, 90), SHOOTING(0, 15); private double speed; private double angle; diff --git a/src/main/java/frc/robot/states/KickerState.java b/src/main/java/frc/robot/states/KickerState.java index 5dc6533..d5c8fcc 100644 --- a/src/main/java/frc/robot/states/KickerState.java +++ b/src/main/java/frc/robot/states/KickerState.java @@ -2,7 +2,7 @@ import frc.robot.constants.KickerConstants; -/** The state that the kicker is in. */ +/** The state that the Kicker is in. */ public enum KickerState { IDLE(0), DISABLED(0), diff --git a/src/main/java/frc/robot/states/PivotState.java b/src/main/java/frc/robot/states/PivotState.java index e06a781..8bc54c9 100644 --- a/src/main/java/frc/robot/states/PivotState.java +++ b/src/main/java/frc/robot/states/PivotState.java @@ -1,5 +1,6 @@ package frc.robot.states; +/** The state that the Pivot is in. */ public enum PivotState { HOME(0), INTAKE(0); diff --git a/src/main/java/frc/robot/states/ShooterState.java b/src/main/java/frc/robot/states/ShooterState.java index 3a2a1c1..970c4a9 100644 --- a/src/main/java/frc/robot/states/ShooterState.java +++ b/src/main/java/frc/robot/states/ShooterState.java @@ -6,7 +6,7 @@ import frc.robot.constants.ShooterConstants; -/** The state that the shooter is in. */ +/** The state that the Shooter is in. */ public enum ShooterState { IDLE(0), DISABLED(0), diff --git a/src/main/java/frc/robot/states/TurretState.java b/src/main/java/frc/robot/states/TurretState.java index a8cc0bd..99625c8 100644 --- a/src/main/java/frc/robot/states/TurretState.java +++ b/src/main/java/frc/robot/states/TurretState.java @@ -1,5 +1,6 @@ package frc.robot.states; +/** The state that the Turret is in. */ public enum TurretState { DISABLED(0), IDLE(0), diff --git a/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java index 871e1fc..38654eb 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java @@ -575,7 +575,7 @@ public boolean acceptVisionMeasurement(Pose2d measurement, double timestamp) { return false; } - /*---------------------------------- Custom Private Functions ---------------------------------*/ + //region Custom Private Functions private void alignCommandInitTelemetry(Pose2d holdDist) { partsNT.putDouble("align/holdDistX", new PARTsUnit(holdDist.getX(), PARTsUnitType.Meter) .to(PARTsUnitType.Inch)); diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 17a8e46..c34665d 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -2,14 +2,17 @@ import org.parts3492.partslib.PARTsUnit; import org.parts3492.partslib.PARTsUnit.PARTsUnitType; +import org.parts3492.partslib.command.PARTsCommandUtils; import org.parts3492.partslib.command.PARTsSubsystem; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.IntakeConstants; import frc.robot.constants.RobotConstants; import frc.robot.constants.ShooterConstants; import frc.robot.states.IntakeState; +import frc.robot.states.ShooterState; public abstract class Intake extends PARTsSubsystem { @@ -69,7 +72,7 @@ public void periodic() { double pidCalc = intakePIDController.calculate(getPivotAngle().to(PARTsUnitType.Angle), intakeState.getAngle()); double ffCalc = intakeFeedForward.calculate(intakePIDController.getSetpoint()); - setPivotVoltage(pidCalc + ffCalc); + //setPivotVoltage(pidCalc + ffCalc); break; default: setIntakeSpeed(0); @@ -97,4 +100,10 @@ public void log() { public abstract void setPivotVoltage(double voltage); public abstract double getPivotRotationSpeed(); + + public Command intake() { + return PARTsCommandUtils.setCommandName("Command Intake", this.runOnce(() -> { + intakeState = IntakeState.INTAKING; + })); + } } diff --git a/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java b/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java index f93d13e..9116649 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java +++ b/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; @@ -21,6 +22,7 @@ public KickerPhys() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; kickerMotor = new TalonFX(KickerConstants.KICKER_MOTOR_ID, KickerConstants.CAN_BUS_NAME); kickerMotor.getConfigurator().apply(config); + kickerMotor.setNeutralMode(NeutralModeValue.Coast); } @Override diff --git a/src/main/java/frc/robot/subsystems/LimelightVision.java b/src/main/java/frc/robot/subsystems/LimelightVision.java index a4ac581..c30948f 100644 --- a/src/main/java/frc/robot/subsystems/LimelightVision.java +++ b/src/main/java/frc/robot/subsystems/LimelightVision.java @@ -1,6 +1,5 @@ package frc.robot.subsystems; -import java.util.function.BiConsumer; import java.util.function.BiFunction; import java.util.function.Consumer; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java index 6a5d8cd..b1d7563 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java @@ -9,7 +9,6 @@ import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import org.parts3492.partslib.command.PARTsCommandUtils; -import frc.robot.states.ShooterState; import org.parts3492.partslib.command.PARTsSubsystem; public abstract class Shooter extends PARTsSubsystem { diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java index 4ec9109..2a8c57d 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.Shooter; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; diff --git a/src/main/java/frc/robot/subsystems/Turret/Turret.java b/src/main/java/frc/robot/subsystems/Turret/Turret.java index c91d559..dd2f77b 100644 --- a/src/main/java/frc/robot/subsystems/Turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/Turret/Turret.java @@ -3,18 +3,14 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConstants; import frc.robot.constants.TurretConstants; import frc.robot.states.TurretState; import frc.robot.util.Field; -import frc.robot.util.Hub; import java.util.function.Supplier; -import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import org.parts3492.partslib.command.PARTsCommandUtils; import org.parts3492.partslib.command.PARTsSubsystem; @@ -82,10 +78,13 @@ public void periodic() { case TRACKING: if (Math.abs(getAngleToTarget()) <= 90) { turretPIDController.setSetpoint(getAngleToTarget()); - double pidCalc = turretPIDController.calculate(getAngle(), turretState.getAngle()); + double pidCalc = turretPIDController.calculate(getAngle(), getAngleToTarget()); double ffCalc = turretFeedforward.calculate(turretPIDController.getSetpoint()); - voltage = pidCalc + ffCalc; + partsNT.putDouble("Turret voltage", voltage); + partsNT.putBoolean("Turret at setpoint", turretPIDController.atSetpoint()); + + voltage = pidCalc; //+ ffCalc; setVoltage(voltage); } @@ -133,27 +132,11 @@ public Command idle() { // region private functions private double getAngleToTarget() { - /* - * Pose2d targetPose = Field.getAllianceHubPose(); - * Pose2d robotPose = robotPoseSupplier.get(); - * Rotation2d angleToTarget = - * targetPose.getTranslation().minus(robotPose.getTranslation()).getAngle(); - */ double angleToTarget = edu.wpi.first.math.MathUtil .inputModulus(robotPoseSupplier.get().getRotation().getDegrees(), -180, 180) - (Math.atan2(Field.getAllianceHubPose().getY() - robotPoseSupplier.get().getY(), Field.getAllianceHubPose().getX() - robotPoseSupplier.get().getX()) * 180 / Math.PI); return angleToTarget; - - /* - * if (angleToTarget > 180) { - * return 180 - angleToTarget; - * } else if (angleToTarget < -180) { - * return 360 + angleToTarget; - * } else { - * return angleToTarget; - * } - */ } // endregion private functions } diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java index 88d6f12..81498a1 100644 --- a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java +++ b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java @@ -4,15 +4,8 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.motorcontrol.Talon; import frc.robot.constants.TurretConstants; public class TurretPhys extends Turret { @@ -31,7 +24,7 @@ public TurretPhys(Supplier robotPoseSupplier) { public void outputTelemetry() { super.outputTelemetry(); partsNT.putDouble("Current/Turret", turretMotor.getSupplyCurrent().getValueAsDouble()); - + partsNT.putDouble("Output/Turret", turretMotor.getStatorCurrent().getValueAsDouble()); } diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretSim.java b/src/main/java/frc/robot/subsystems/Turret/TurretSim.java index e60798a..a842ef5 100644 --- a/src/main/java/frc/robot/subsystems/Turret/TurretSim.java +++ b/src/main/java/frc/robot/subsystems/Turret/TurretSim.java @@ -2,8 +2,6 @@ import java.util.function.Supplier; -import com.ctre.phoenix6.hardware.TalonFX; - import edu.wpi.first.math.geometry.Pose2d; public class TurretSim extends Turret { diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretSysid.java b/src/main/java/frc/robot/subsystems/Turret/TurretSysid.java new file mode 100644 index 0000000..e8fb480 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Turret/TurretSysid.java @@ -0,0 +1,62 @@ +package frc.robot.subsystems.Turret; + +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class TurretSysid extends TurretPhys { + + private MutVoltage appliedVoltage; + + private MutAngle pivotAngle; + + private MutAngularVelocity pivotVelocity; + + private SysIdRoutine routine; + + public TurretSysid(Supplier robotPoseSupplier) { + super(robotPoseSupplier); + + appliedVoltage = Volts.mutable(0); + + pivotAngle = Units.Radian.mutable(0); + + pivotVelocity = Units.RadiansPerSecond.mutable(0); + + routine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (voltage) -> this.setVoltage(voltage.in(Volts)), + + log -> { + // Record a frame for the shooter motor. + log.motor("turret") + .voltage( + appliedVoltage.mut_replace( + super.turretMotor.getStatorCurrent().getValueAsDouble(), Volts)) + .angularPosition(pivotAngle.mut_replace( + getAngle() * Math.PI / 180, Radians)) + .angularVelocity( + pivotVelocity.mut_replace(super.turretMotor.getVelocity().getValueAsDouble(), RotationsPerSecond)); + }, + this)); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return routine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return routine.dynamic(direction); + } +} From 5fbed0fd7eece88965ca92e5d2455e1900a2af06 Mon Sep 17 00:00:00 2001 From: team3492backup Date: Sat, 21 Feb 2026 16:17:46 -0500 Subject: [PATCH 06/16] Speed based on zone, lots of cleanup, trench util --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/constants/RobotConstants.java | 2 +- .../java/frc/robot/states/ShooterState.java | 19 +++++ .../robot/subsystems/Hopper/HopperPhys.java | 9 +-- .../frc/robot/subsystems/Intake/Intake.java | 8 ++- .../robot/subsystems/Intake/IntakePhys.java | 8 +-- .../frc/robot/subsystems/Kicker/Kicker.java | 2 - .../robot/subsystems/Kicker/KickerPhys.java | 7 -- .../frc/robot/subsystems/Shooter/Shooter.java | 17 ++++- .../robot/subsystems/Shooter/ShooterPhys.java | 7 +- .../robot/subsystems/Shooter/ShooterSim.java | 7 +- .../subsystems/Shooter/ShooterSysid.java | 7 +- .../frc/robot/subsystems/Superstructure.java | 69 +++++++++++++++++++ .../frc/robot/subsystems/Turret/Turret.java | 6 +- .../robot/subsystems/Turret/TurretPhys.java | 4 +- src/main/java/frc/robot/util/Field.java | 5 +- src/main/java/frc/robot/util/Trench.java | 35 ++++++++++ 17 files changed, 177 insertions(+), 37 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Superstructure.java create mode 100644 src/main/java/frc/robot/util/Trench.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 22bfec0..78b50b5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -92,7 +92,7 @@ public class RobotContainer { public final Candle candle = new Candle(); - private final Shooter shooter = Robot.isReal() ? new ShooterPhys() : new ShooterSim(); + private final Shooter shooter = Robot.isReal() ? new ShooterPhys(drivetrain.supplierGetPose()) : new ShooterSim(drivetrain.supplierGetPose()); private final Turret turret = Robot.isReal() ? new TurretPhys(drivetrain.supplierGetPose()) : new TurretSim(drivetrain.supplierGetPose()); diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index cbcca62..96afedb 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -12,6 +12,6 @@ public class RobotConstants { PARTsUnitType.Inch); public static boolean LOGGING = true; - public static boolean DEBUGGING = false; + public static boolean DEBUGGING = true; public static boolean ALLOW_AUTO_CONTROLLER_DETECTION = false; } diff --git a/src/main/java/frc/robot/states/ShooterState.java b/src/main/java/frc/robot/states/ShooterState.java index 970c4a9..188e0ed 100644 --- a/src/main/java/frc/robot/states/ShooterState.java +++ b/src/main/java/frc/robot/states/ShooterState.java @@ -5,6 +5,7 @@ package frc.robot.states; import frc.robot.constants.ShooterConstants; +import frc.robot.util.Hub.Targets; /** The state that the Shooter is in. */ public enum ShooterState { @@ -22,4 +23,22 @@ public enum ShooterState { public double getRPM() { return rpm; } + + public double getZoneRPM(Targets zone) { + if (zone == null) { + return 0; + } + switch (zone) { + case ZONE1: + return 1; + case ZONE2: + return 2; + case ZONE3: + return 3; + case ZONE4: + return 4; + default: + return 0; + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java b/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java index c52ee16..bd8c73f 100644 --- a/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java +++ b/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java @@ -3,13 +3,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.ctre.phoenix6.signals.NeutralModeValue; import frc.robot.constants.HopperConstants; @@ -22,6 +16,7 @@ public HopperPhys() { hopperConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; hopperMotor = new TalonFX(HopperConstants.HOPPER_MOTOR_ID, HopperConstants.CAN_BUS_NAME); hopperMotor.getConfigurator().apply(hopperConfig); + hopperMotor.setNeutralMode(NeutralModeValue.Brake); } @Override diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index c34665d..f6217d5 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -10,9 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.IntakeConstants; import frc.robot.constants.RobotConstants; -import frc.robot.constants.ShooterConstants; import frc.robot.states.IntakeState; -import frc.robot.states.ShooterState; public abstract class Intake extends PARTsSubsystem { @@ -106,4 +104,10 @@ public Command intake() { intakeState = IntakeState.INTAKING; })); } + + public Command intakeShooting() { + return PARTsCommandUtils.setCommandName("Command IntakeShooting", this.runOnce(() -> { + intakeState = IntakeState.SHOOTING; + })); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java index f2a4a37..9df322f 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java @@ -1,16 +1,12 @@ package frc.robot.subsystems.Intake; -import static edu.wpi.first.units.Units.Rotations; - import org.parts3492.partslib.PARTsUnit; import org.parts3492.partslib.PARTsUnit.PARTsUnitType; -import org.parts3492.partslib.network.PARTsNT; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; - -import edu.wpi.first.wpilibj.motorcontrol.Talon; +import com.ctre.phoenix6.signals.NeutralModeValue; import frc.robot.constants.IntakeConstants; @@ -28,11 +24,13 @@ public IntakePhys() { intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID, IntakeConstants.CAN_BUS_NAME); intakeConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; intakeMotor.getConfigurator().apply(intakeConfig); + intakeMotor.setNeutralMode(NeutralModeValue.Brake); pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID, IntakeConstants.CAN_BUS_NAME); TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); pivotMotor.getConfigurator().apply(pivotConfig); pivotMotor.getConfigurator().setPosition(0); + pivotMotor.setNeutralMode(NeutralModeValue.Brake); } diff --git a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java index ec6c2c4..c9a7055 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java +++ b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java @@ -1,12 +1,10 @@ package frc.robot.subsystems.Kicker; -import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import org.parts3492.partslib.command.PARTsCommandUtils; import org.parts3492.partslib.command.PARTsSubsystem; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConstants; -import frc.robot.constants.ShooterConstants; import frc.robot.states.KickerState; public abstract class Kicker extends PARTsSubsystem { diff --git a/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java b/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java index 9116649..7b60f6d 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java +++ b/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java @@ -4,13 +4,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import frc.robot.constants.KickerConstants; public class KickerPhys extends Kicker { diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java index b1d7563..2ad5333 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java @@ -2,10 +2,15 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConstants; import frc.robot.constants.ShooterConstants; import frc.robot.states.ShooterState; +import frc.robot.util.Hub; +import frc.robot.util.Hub.Targets; + +import java.util.function.Supplier; import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import org.parts3492.partslib.command.PARTsCommandUtils; @@ -16,9 +21,11 @@ public abstract class Shooter extends PARTsSubsystem { private PIDController shooterPIDController; private SimpleMotorFeedforward shooterFeedforward; + private Pose2d poseSupplier; - public Shooter() { + public Shooter(Supplier poseSupplier) { super("Shooter", RobotConstants.LOGGING); + this.poseSupplier = poseSupplier.get(); if (RobotConstants.DEBUGGING) { partsNT.putDouble("Shooter Speed", 0); } @@ -38,6 +45,9 @@ public void outputTelemetry() { partsNT.putDouble("Get Setpoint", shooterPIDController.getSetpoint()); partsNT.putBoolean("At Setpoint", shooterPIDController.atSetpoint()); partsNT.putDouble("Current Error", shooterPIDController.getError()); + + Targets zone = Hub.getZone(poseSupplier); + partsNT.putString("Zone", zone == null ? "No zone" : zone.toString()); } @Override @@ -66,9 +76,10 @@ public void periodic() { case IDLE: case SHOOTING: double voltage = 0; - shooterPIDController.setSetpoint(shooterState.getRPM()); + Targets zone = Hub.getZone(poseSupplier); + shooterPIDController.setSetpoint(shooterState.getZoneRPM(zone)); - double pidCalc = shooterPIDController.calculate(getRPM(), shooterState.getRPM()); + double pidCalc = shooterPIDController.calculate(getRPM(), shooterState.getZoneRPM(zone)); double ffCalc = shooterFeedforward.calculate((shooterPIDController.getSetpoint() * Math.PI * ShooterConstants.SHOOTER_WHEEL_RADIUS.to(PARTsUnitType.Meter) * 2) / 60); diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java index 2a8c57d..f8db85d 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.Shooter; +import java.util.function.Supplier; + import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; @@ -7,14 +9,15 @@ import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Pose2d; import frc.robot.constants.ShooterConstants; public class ShooterPhys extends Shooter { protected final TalonFX leftMotor; protected final TalonFX rightMotor; - public ShooterPhys() { - super(); + public ShooterPhys(Supplier poseSupplier) { + super(poseSupplier); TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; leftMotor = new TalonFX(ShooterConstants.LEFT_MOTOR_ID, ShooterConstants.CAN_BUS_NAME); diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterSim.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterSim.java index b3b2683..2e2b441 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterSim.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterSim.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.Shooter; +import java.util.function.Supplier; + import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import com.revrobotics.PersistMode; @@ -11,6 +13,7 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; @@ -29,8 +32,8 @@ public class ShooterSim extends Shooter { SparkRelativeEncoderSim motorEncoder; FlywheelSim shooterSim; - public ShooterSim() { - super(); + public ShooterSim(Supplier poseSupplier) { + super(poseSupplier); maxGearbox = DCMotor.getNEO(2); SparkMaxConfig shooterConfig = new SparkMaxConfig(); diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java index 95ee919..a5f79e3 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java @@ -4,6 +4,9 @@ import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.Volts; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.units.measure.MutVoltage; @@ -24,8 +27,8 @@ public class ShooterSysid extends ShooterPhys { private SysIdRoutine routine; - public ShooterSysid() { - super(); + public ShooterSysid(Supplier poseSupplier) { + super(poseSupplier); appliedVoltage = Volts.mutable(0); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java new file mode 100644 index 0000000..62a6414 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -0,0 +1,69 @@ +package frc.robot.subsystems; + +import java.util.function.BooleanSupplier; + +import org.parts3492.partslib.command.PARTsCommandUtils; +import org.parts3492.partslib.command.PARTsSubsystem; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Hopper.Hopper; +import frc.robot.subsystems.Intake.Intake; +import frc.robot.subsystems.Kicker.Kicker; +import frc.robot.subsystems.Shooter.Shooter; +import frc.robot.subsystems.Turret.Turret; + +public class Superstructure extends PARTsSubsystem { + private final Hopper hopper; + private final Intake intake; + private final Kicker kicker; + private final Shooter shooter; + private final Turret turret; + + public Superstructure(Hopper hopper, Intake intake, Kicker kicker, Shooter shooter, Turret turret) { + this.hopper = hopper; + this.intake = intake; + this.kicker = kicker; + this.shooter = shooter; + this.turret = turret; + } + + /** + * lift up pivot arm, roll hopper, roll kicker, shoot. Only happens if turret + * has valid angle + */ + public Command shoot(BooleanSupplier end) { + return PARTsCommandUtils.setCommandName("Superstructure.shoot", Commands.run(() -> { + if (turret.isValidAngle()) { + turret.track(); + intake.intakeShooting(); + hopper.roll(); + kicker.roll(); + shooter.shoot(); + } + }, hopper, intake, kicker, shooter, turret).until(() -> !turret.isValidAngle() || end.getAsBoolean()) + .andThen(Commands.runOnce(() -> { + turret.reset(); + intake.reset(); + hopper.reset(); + kicker.reset(); + shooter.reset(); + }, hopper, intake, kicker, shooter, turret))); + } + + @Override + public void outputTelemetry() { + } + + @Override + public void stop() { + } + + @Override + public void reset() { + } + + @Override + public void log() { + } +} diff --git a/src/main/java/frc/robot/subsystems/Turret/Turret.java b/src/main/java/frc/robot/subsystems/Turret/Turret.java index dd2f77b..1888432 100644 --- a/src/main/java/frc/robot/subsystems/Turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/Turret/Turret.java @@ -76,7 +76,7 @@ public void periodic() { setSpeed(0); break; case TRACKING: - if (Math.abs(getAngleToTarget()) <= 90) { + if (isValidAngle()) { turretPIDController.setSetpoint(getAngleToTarget()); double pidCalc = turretPIDController.calculate(getAngle(), getAngleToTarget()); double ffCalc = turretFeedforward.calculate(turretPIDController.getSetpoint()); @@ -113,6 +113,10 @@ public void periodic() { protected abstract double getAngle(); + public boolean isValidAngle() { + return Math.abs(getAngleToTarget()) <= 90; + } + public TurretState getState() { return turretState; } diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java index 81498a1..3b5cdfd 100644 --- a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java +++ b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Pose2d; import frc.robot.constants.TurretConstants; @@ -18,13 +19,14 @@ public TurretPhys(Supplier robotPoseSupplier) { TalonFXConfiguration turretConfig = new TalonFXConfiguration(); turretMotor.getConfigurator().apply(turretConfig); turretMotor.getConfigurator().setPosition(0); + turretMotor.setNeutralMode(NeutralModeValue.Brake); } @Override public void outputTelemetry() { super.outputTelemetry(); partsNT.putDouble("Current/Turret", turretMotor.getSupplyCurrent().getValueAsDouble()); - + partsNT.putDouble("Output/Turret", turretMotor.getStatorCurrent().getValueAsDouble()); } diff --git a/src/main/java/frc/robot/util/Field.java b/src/main/java/frc/robot/util/Field.java index 0ac3bf2..5ac7c9b 100644 --- a/src/main/java/frc/robot/util/Field.java +++ b/src/main/java/frc/robot/util/Field.java @@ -14,7 +14,6 @@ import java.util.List; import org.parts3492.partslib.game.AprilTag; -import org.parts3492.partslib.RobotUtils; /** This interface stores information about the field elements. */ public interface Field { @@ -146,6 +145,10 @@ public static Pose2d getAllianceHubPose() { return RobotContainer.isBlue() ? blueHubCenter : transformToOppositeAlliance(blueHubCenter); } + public static Pose2d [] getAllianceTrenchPoses() { + return RobotContainer.isBlue() ? new Pose2d [] {getTag(23).getLocation().toPose2d(), getTag(38).getLocation().toPose2d()}: new Pose2d [] {getTag(12).getLocation().toPose2d(), getTag(7).getLocation().toPose2d()}; + } + public static int[] getAllTagIDs() { int[] ids = new int[APRILTAGS.length]; diff --git a/src/main/java/frc/robot/util/Trench.java b/src/main/java/frc/robot/util/Trench.java new file mode 100644 index 0000000..5c18ca5 --- /dev/null +++ b/src/main/java/frc/robot/util/Trench.java @@ -0,0 +1,35 @@ +package frc.robot.util; + +import org.parts3492.partslib.command.PARTsCommandUtils; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Drivetrain.PARTsDrivetrain; + +public class Trench { + private static Pose2d goal; + + public static Command parkUnderTrench(PARTsDrivetrain drivetrain) { + return PARTsCommandUtils.setCommandName("Trench.parkUnderTrench", + Commands.runOnce(() -> goal = getNearestTrench(drivetrain.getPose(), Field.getAllianceTrenchPoses())) + .andThen(drivetrain.commandPathFindToPose(goal))); + } + + private static Pose2d getNearestTrench(Pose2d current, Pose2d[] poses) { + int index = 0; + double distance = getDistance(current, poses[0]); + for (int i = 0; i < poses.length; i++) { + double localDistance = getDistance(current, poses[i]); + if (localDistance < distance) { + distance = localDistance; + index = i; + } + } + return poses[index]; + } + + private static double getDistance(Pose2d current, Pose2d goal) { + return Math.sqrt(Math.pow(current.getX() - goal.getX(), 2) + Math.pow(current.getY() - goal.getY(), 2)); + } +} From 526aafe70c3e171b9ea0283d5cb8867d2fa57fb3 Mon Sep 17 00:00:00 2001 From: team3492backup Date: Sat, 21 Feb 2026 16:38:37 -0500 Subject: [PATCH 07/16] cleanup for enums and subsystems --- .../frc/robot/constants/HopperConstants.java | 2 - .../frc/robot/constants/IntakeConstants.java | 2 - .../frc/robot/constants/KickerConstants.java | 1 - .../frc/robot/constants/ShooterConstants.java | 1 - .../java/frc/robot/states/HopperState.java | 6 +-- .../java/frc/robot/states/IntakeState.java | 6 +-- .../java/frc/robot/states/KickerState.java | 4 +- .../java/frc/robot/states/PivotState.java | 17 -------- .../java/frc/robot/states/ShooterState.java | 5 +-- .../Drivetrain/PARTsDrivetrain.java | 11 +---- .../robot/subsystems/Hopper/HopperPhys.java | 10 ++--- .../frc/robot/subsystems/Intake/Intake.java | 5 ++- .../robot/subsystems/Intake/IntakePhys.java | 40 +++++++++---------- .../frc/robot/subsystems/Kicker/Kicker.java | 4 +- .../robot/subsystems/Kicker/KickerPhys.java | 10 ++--- .../robot/subsystems/Shooter/ShooterPhys.java | 30 +++++++------- .../frc/robot/subsystems/Turret/Turret.java | 4 +- .../robot/subsystems/Turret/TurretPhys.java | 30 +++++++------- src/main/java/frc/robot/util/Hub.java | 23 +++++------ 19 files changed, 88 insertions(+), 123 deletions(-) delete mode 100644 src/main/java/frc/robot/states/PivotState.java diff --git a/src/main/java/frc/robot/constants/HopperConstants.java b/src/main/java/frc/robot/constants/HopperConstants.java index b55cf88..7dddf85 100644 --- a/src/main/java/frc/robot/constants/HopperConstants.java +++ b/src/main/java/frc/robot/constants/HopperConstants.java @@ -1,8 +1,6 @@ package frc.robot.constants; public class HopperConstants { - public static final double ROLL_SPEED = 0.3; - public static final double BACKROLL_SPEED = -0.3; public static final int HOPPER_MOTOR_ID = 37; public static final String CAN_BUS_NAME = "bye"; } diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 9b0f461..0546ecb 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -8,8 +8,6 @@ public class IntakeConstants { /** The pivot gear ratio. The total is {@code 36/1}. */ public static final double PIVOT_GEAR_RATIO = (12.0/1.0)*(3.0/1.0); - public static final double INTAKE_SPEED = 0.5; - // PID Controller public static final double P = 0; public static final double I = 0; diff --git a/src/main/java/frc/robot/constants/KickerConstants.java b/src/main/java/frc/robot/constants/KickerConstants.java index 8e23155..a246475 100644 --- a/src/main/java/frc/robot/constants/KickerConstants.java +++ b/src/main/java/frc/robot/constants/KickerConstants.java @@ -1,7 +1,6 @@ package frc.robot.constants; public class KickerConstants { - public static final double KICKER_SPEED = 0.1; // TBD public static final int KICKER_MOTOR_ID = 34; public static final String CAN_BUS_NAME = "bye"; } diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index ea59de5..92f170c 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -4,7 +4,6 @@ import org.parts3492.partslib.PARTsUnit.PARTsUnitType; public class ShooterConstants { - public static final double SHOOTER_RPM = 3000; public static final int LEFT_MOTOR_ID = 33; public static final int RIGHT_MOTOR_ID = 35; public static final String CAN_BUS_NAME = "bye"; diff --git a/src/main/java/frc/robot/states/HopperState.java b/src/main/java/frc/robot/states/HopperState.java index a449d65..76391d0 100644 --- a/src/main/java/frc/robot/states/HopperState.java +++ b/src/main/java/frc/robot/states/HopperState.java @@ -1,13 +1,11 @@ package frc.robot.states; -import frc.robot.constants.HopperConstants; - /** The state the Hopper is in */ public enum HopperState { IDLE(0), DISABLED(0), - ROLLING(HopperConstants.ROLL_SPEED), - BACKROLLING(HopperConstants.BACKROLL_SPEED); + ROLLING(0.3), + BACKROLLING(-0.3); private final double speed; diff --git a/src/main/java/frc/robot/states/IntakeState.java b/src/main/java/frc/robot/states/IntakeState.java index 6f4e0be..3e87263 100644 --- a/src/main/java/frc/robot/states/IntakeState.java +++ b/src/main/java/frc/robot/states/IntakeState.java @@ -1,13 +1,11 @@ package frc.robot.states; -import frc.robot.constants.IntakeConstants; - /** The state the Intake is in */ public enum IntakeState { IDLE(0, 0), DISABLED(0, 0), - INTAKING(IntakeConstants.INTAKE_SPEED, 90), - OUTTAKING(-IntakeConstants.INTAKE_SPEED, 90), + INTAKING(0.5, 90), + OUTTAKING(-0.5, 90), SHOOTING(0, 15); private double speed; private double angle; diff --git a/src/main/java/frc/robot/states/KickerState.java b/src/main/java/frc/robot/states/KickerState.java index d5c8fcc..e0e1e1a 100644 --- a/src/main/java/frc/robot/states/KickerState.java +++ b/src/main/java/frc/robot/states/KickerState.java @@ -1,12 +1,10 @@ package frc.robot.states; -import frc.robot.constants.KickerConstants; - /** The state that the Kicker is in. */ public enum KickerState { IDLE(0), DISABLED(0), - ROLLING(KickerConstants.KICKER_SPEED); + ROLLING(0.1); private final double speed; diff --git a/src/main/java/frc/robot/states/PivotState.java b/src/main/java/frc/robot/states/PivotState.java deleted file mode 100644 index 8bc54c9..0000000 --- a/src/main/java/frc/robot/states/PivotState.java +++ /dev/null @@ -1,17 +0,0 @@ -package frc.robot.states; - -/** The state that the Pivot is in. */ -public enum PivotState { - HOME(0), - INTAKE(0); - - private double position; - - private PivotState(double position){ - this.position = position; - } - - public double getPosition(){ - return position; - } -} diff --git a/src/main/java/frc/robot/states/ShooterState.java b/src/main/java/frc/robot/states/ShooterState.java index 188e0ed..eec61b8 100644 --- a/src/main/java/frc/robot/states/ShooterState.java +++ b/src/main/java/frc/robot/states/ShooterState.java @@ -4,15 +4,14 @@ package frc.robot.states; -import frc.robot.constants.ShooterConstants; import frc.robot.util.Hub.Targets; /** The state that the Shooter is in. */ public enum ShooterState { IDLE(0), DISABLED(0), - CHARGING(ShooterConstants.SHOOTER_RPM), - SHOOTING(ShooterConstants.SHOOTER_RPM); + CHARGING(3000), + SHOOTING(3000); private final double rpm; ShooterState(double rpm) { diff --git a/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java index 38654eb..202f492 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.Drivetrain; -import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; @@ -24,7 +23,6 @@ import java.io.IOException; import java.util.Arrays; import java.util.HashSet; -import java.util.function.BiConsumer; import java.util.function.BiFunction; import java.util.function.BooleanSupplier; import java.util.function.Consumer; @@ -37,7 +35,6 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; @@ -45,9 +42,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -161,8 +155,6 @@ private void instantiate() { @Override public void outputTelemetry() { partsNT.putBoolean("Fine Grain Drive", fineGrainDrive); - Targets zone = Hub.getZone(getPose()); - partsNT.putString("Zone", zone == null ? "No zone" : zone.toString()); partsNT.putDouble("HUB X coordinate", new PARTsUnit(Field.getAllianceHubPose().getX(), PARTsUnitType.Meter) .to(PARTsUnitType.Inch)); @@ -565,7 +557,7 @@ public Consumer consumerResetPose() { public Supplier supplierGetPose() { return this::getPose; } - //endregion + public boolean acceptVisionMeasurement(Pose2d measurement, double timestamp) { if (Math.max(Math.abs(getXAngularVelocity()), Math.abs(getYAngularVelocity())) < 2 * Math.PI) { @@ -574,6 +566,7 @@ public boolean acceptVisionMeasurement(Pose2d measurement, double timestamp) { } return false; } + //endregion //region Custom Private Functions private void alignCommandInitTelemetry(Pose2d holdDist) { diff --git a/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java b/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java index bd8c73f..a43b151 100644 --- a/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java +++ b/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java @@ -25,11 +25,6 @@ public void outputTelemetry() { partsNT.putDouble("Current", hopperMotor.getSupplyCurrent().getValueAsDouble()); } - @Override - protected void setSpeed(double speed) { - hopperMotor.set(speed); - } - @Override public void periodic() { super.periodic(); @@ -40,4 +35,9 @@ public void log() { partsLogger.logDouble("Output", hopperMotor.getStatorCurrent().getValueAsDouble()); partsLogger.logDouble("Current", hopperMotor.getSupplyCurrent().getValueAsDouble()); } + + @Override + protected void setSpeed(double speed) { + hopperMotor.set(speed); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index f6217d5..de60ecd 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -32,7 +32,7 @@ public Intake() { intakePIDController.setTolerance(IntakeConstants.PID_THRESHOLD); } - + //region Generic Subsystem Functions @Override public void outputTelemetry() { partsNT.putDouble("Pivot Position", getPivotAngle().to(PARTsUnitType.Angle)); @@ -86,7 +86,9 @@ public void log() { partsLogger.logDouble("Intake Speed", getIntakeSpeed()); partsLogger.logString("Intake State", intakeState.toString()); } + //endregion + //region Custom Public Functions public abstract void setIntakeSpeed(double speed); public abstract void setPivotSpeed(double speed); @@ -110,4 +112,5 @@ public Command intakeShooting() { intakeState = IntakeState.SHOOTING; })); } + //endregion } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java index 9df322f..b1a34b0 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java @@ -34,26 +34,6 @@ public IntakePhys() { } - @Override - public void setIntakeSpeed(double speed) { - intakeMotor.set(speed); - } - - @Override - public double getIntakeSpeed() { - return intakeMotor.get(); - } - - @Override - public double getPivotRotationSpeed() { - return pivotMotor.getVelocity().getValueAsDouble(); - } - - @Override - public PARTsUnit getPivotAngle() { - return new PARTsUnit(pivotMotor.getPosition().getValueAsDouble() / IntakeConstants.PIVOT_GEAR_RATIO, PARTsUnitType.Rotations); - } - @Override public void outputTelemetry() { super.outputTelemetry(); @@ -95,4 +75,24 @@ public void setPivotSpeed(double speed) { public void setPivotVoltage(double voltage) { pivotMotor.setVoltage(voltage); } + + @Override + public void setIntakeSpeed(double speed) { + intakeMotor.set(speed); + } + + @Override + public double getIntakeSpeed() { + return intakeMotor.get(); + } + + @Override + public double getPivotRotationSpeed() { + return pivotMotor.getVelocity().getValueAsDouble(); + } + + @Override + public PARTsUnit getPivotAngle() { + return new PARTsUnit(pivotMotor.getPosition().getValueAsDouble() / IntakeConstants.PIVOT_GEAR_RATIO, PARTsUnitType.Rotations); + } } diff --git a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java index c9a7055..d4c0b30 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java +++ b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java @@ -39,7 +39,6 @@ public void reset() { public void log() { partsLogger.logString("Kicker State", kickerState.toString()); } - // endregion @Override public void periodic() { @@ -58,6 +57,7 @@ public void periodic() { } } } + //endregion // region Custom Public Functions /** @@ -82,5 +82,5 @@ public Command idle() { kickerState = KickerState.IDLE; })); } + //endregion } -// endregion diff --git a/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java b/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java index 7b60f6d..c2916f0 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java +++ b/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java @@ -25,11 +25,6 @@ public void outputTelemetry() { partsNT.putDouble("KickerOutput", kickerMotor.getStatorCurrent().getValueAsDouble()); } - @Override - protected void setSpeed(double speed) { - kickerMotor.set(speed); - } - @Override public void periodic() { super.periodic(); @@ -41,4 +36,9 @@ public void log() { partsLogger.logDouble("KickerOutput", kickerMotor.getStatorCurrent().getValueAsDouble()); } + + @Override + protected void setSpeed(double speed) { + kickerMotor.set(speed); + } } diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java index f8db85d..28f564d 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java @@ -40,6 +40,21 @@ public void outputTelemetry() { partsNT.putDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); } + @Override + public void periodic() { + super.periodic(); + } + + @Override + public void log() { + super.log(); + partsLogger.logDouble("Current/Left", leftMotor.getSupplyCurrent().getValueAsDouble()); + partsLogger.logDouble("Current/Right", rightMotor.getSupplyCurrent().getValueAsDouble()); + + partsLogger.logDouble("Output/Left", leftMotor.getStatorCurrent().getValueAsDouble()); + partsLogger.logDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); + } + @Override protected void setSpeed(double speed) { leftMotor.set(speed); @@ -59,19 +74,4 @@ protected void setVoltage(double voltage) { protected double getVoltage() { return leftMotor.getSupplyVoltage().getValueAsDouble(); } - - @Override - public void periodic() { - super.periodic(); - } - - @Override - public void log() { - super.log(); - partsLogger.logDouble("Current/Left", leftMotor.getSupplyCurrent().getValueAsDouble()); - partsLogger.logDouble("Current/Right", rightMotor.getSupplyCurrent().getValueAsDouble()); - - partsLogger.logDouble("Output/Left", leftMotor.getStatorCurrent().getValueAsDouble()); - partsLogger.logDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); - } } diff --git a/src/main/java/frc/robot/subsystems/Turret/Turret.java b/src/main/java/frc/robot/subsystems/Turret/Turret.java index 1888432..b67f6d0 100644 --- a/src/main/java/frc/robot/subsystems/Turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/Turret/Turret.java @@ -61,7 +61,6 @@ public void reset() { public void log() { partsLogger.logString("Turret State", turretState.toString()); } - // endregion @Override public void periodic() { @@ -79,7 +78,7 @@ public void periodic() { if (isValidAngle()) { turretPIDController.setSetpoint(getAngleToTarget()); double pidCalc = turretPIDController.calculate(getAngle(), getAngleToTarget()); - double ffCalc = turretFeedforward.calculate(turretPIDController.getSetpoint()); + //double ffCalc = turretFeedforward.calculate(turretPIDController.getSetpoint()); partsNT.putDouble("Turret voltage", voltage); partsNT.putBoolean("Turret at setpoint", turretPIDController.atSetpoint()); @@ -98,6 +97,7 @@ public void periodic() { } } } + // endregion // region Custom Public Functions /** diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java index 3b5cdfd..13aa0c9 100644 --- a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java +++ b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java @@ -30,21 +30,6 @@ public void outputTelemetry() { partsNT.putDouble("Output/Turret", turretMotor.getStatorCurrent().getValueAsDouble()); } - @Override - protected void setSpeed(double speed) { - turretMotor.set(speed); - } - - @Override - protected void setVoltage(double voltage) { - turretMotor.setVoltage(voltage); - } - - @Override - protected double getVoltage() { - return turretMotor.getSupplyCurrent().getValueAsDouble(); - } - @Override public void periodic() { super.periodic(); @@ -62,4 +47,19 @@ public void log() { protected double getAngle() { return turretMotor.getPosition().getValueAsDouble() * 360 / TurretConstants.TURRET_GEAR_RATIO; } + + @Override + protected void setSpeed(double speed) { + turretMotor.set(speed); + } + + @Override + protected void setVoltage(double voltage) { + turretMotor.setVoltage(voltage); + } + + @Override + protected double getVoltage() { + return turretMotor.getSupplyCurrent().getValueAsDouble(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Hub.java b/src/main/java/frc/robot/util/Hub.java index 3b3427a..0ecc800 100644 --- a/src/main/java/frc/robot/util/Hub.java +++ b/src/main/java/frc/robot/util/Hub.java @@ -2,6 +2,7 @@ import java.util.Optional; +import org.parts3492.partslib.PARTsUnit; import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import org.parts3492.partslib.network.PARTsNT; @@ -9,7 +10,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; -import frc.robot.constants.FieldConstants; public class Hub { private static Pose2d hubPose2d = Field.getAllianceHubPose(); @@ -18,10 +18,10 @@ public class Hub { private static PARTsNT partsNT = new PARTsNT("Hub"); public static enum Targets { - ZONE1(FieldConstants.ZONE1_RADIUS.to(PARTsUnitType.Meter)), - ZONE2(FieldConstants.ZONE2_RADIUS.to(PARTsUnitType.Meter)), - ZONE3(FieldConstants.ZONE3_RADIUS.to(PARTsUnitType.Meter)), - ZONE4(FieldConstants.ZONE4_RADIUS.to(PARTsUnitType.Meter)); + ZONE1(new PARTsUnit(4, PARTsUnitType.Foot).to(PARTsUnitType.Meter)), + ZONE2(new PARTsUnit(6, PARTsUnitType.Foot).to(PARTsUnitType.Meter)), + ZONE3(new PARTsUnit(8, PARTsUnitType.Foot).to(PARTsUnitType.Meter)), + ZONE4(new PARTsUnit(10, PARTsUnitType.Foot).to(PARTsUnitType.Meter)); private double radius; @@ -34,6 +34,12 @@ public double getRadius() { } } + public static void outputTelemetry() { + partsNT.putBoolean("Hub Active", Hub.isHubActive()); + partsNT.putDouble("Time Left", timer.get() <=25 ? 25 - timer.get() : 0); + checkHubActivity(); + } + public static boolean isInRadius(Pose2d center, Pose2d point, double radius) { double centerPoseX = center.getX(); double centerPoseY = center.getY(); @@ -42,7 +48,6 @@ public static boolean isInRadius(Pose2d center, Pose2d point, double radius) { double pointPoseY = point.getY(); double distanceSquared = (Math.pow(centerPoseX - pointPoseX, 2) + Math.pow(centerPoseY - pointPoseY, 2)); - // System.out.println(Math.sqrt(distanceSquared)); return distanceSquared <= Math.pow(radius, 2); } @@ -132,12 +137,6 @@ public static void startHubActiveTimer() { timer.start(); } - public static void outputTelemetry() { - partsNT.putBoolean("Hub Active", Hub.isHubActive()); - partsNT.putDouble("Time Left", timer.get() <=25 ? 25 - timer.get() : 0); - checkHubActivity(); - } - public static void checkHubActivity() { if (previousHubActive != Hub.isHubActive()) { timer.restart(); From ed0c5dfc68ebcbded0dae25fa1f558240cdd2cb1 Mon Sep 17 00:00:00 2001 From: team3492backup Date: Sat, 21 Feb 2026 18:21:48 -0500 Subject: [PATCH 08/16] superstructure works --- src/main/java/frc/robot/RobotContainer.java | 17 ++++++++++---- .../frc/robot/constants/FieldConstants.java | 11 ---------- .../frc/robot/constants/IntakeConstants.java | 4 ++-- .../frc/robot/constants/RobotConstants.java | 4 ++-- .../java/frc/robot/states/IntakeState.java | 7 +++--- .../frc/robot/subsystems/Intake/Intake.java | 17 +++++++++++++- .../frc/robot/subsystems/Shooter/Shooter.java | 6 +++-- .../frc/robot/subsystems/Superstructure.java | 22 +++++++++---------- 8 files changed, 51 insertions(+), 37 deletions(-) delete mode 100644 src/main/java/frc/robot/constants/FieldConstants.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 78b50b5..efd185c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,6 +36,7 @@ import frc.robot.states.CandleState; import frc.robot.subsystems.Candle; import frc.robot.subsystems.LimelightVision; +import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.LimelightVision.MegaTagMode; import frc.robot.subsystems.Drivetrain.CommandSwerveDrivetrain; import frc.robot.subsystems.Drivetrain.PARTsDrivetrain; @@ -103,13 +104,14 @@ public class RobotContainer { private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim(); + private final Superstructure superstructure = new Superstructure(hopper, intake, kicker, shooter, turret); + // private final ShooterSysid shooter = new ShooterSysid(); //for sysid // private final IntakeSysid intake = new IntakeSysid(); //for sysid - // private final TurretSysid turret = new - // TurretSysid(drivetrain.supplierGetPose()); + // private final TurretSysid turret = new TurretSysid(drivetrain.supplierGetPose()); private final ArrayList subsystems = new ArrayList( - Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake)); + Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake, superstructure)); // endregion End Subsystems @@ -121,6 +123,7 @@ public RobotContainer() { configureAutonomousCommands(); configureIntakeBindings(); configureHopperBindings(); + configureSuperstructureBindings(); partsNT.putSmartDashboardSendable("field", Field.FIELD2D); hubFieldObject2d = Field.FIELD2D.getObject("hub"); @@ -226,7 +229,9 @@ private void configureTurretBindings() { } private void configureIntakeBindings() { - driveController.a().onTrue(intake.intake()); + //driveController.x().onTrue(intake.intake()); + //driveController.b().onTrue(intake.intakeIdle()); + //driveController.x().onTrue(intake.home()); /* * operatorController.a().and(operatorController.rightBumper()) @@ -241,6 +246,10 @@ private void configureIntakeBindings() { } + private void configureSuperstructureBindings() { + //driveController.a().onTrue(superstructure.shoot(driveController.b()::getAsBoolean)); + } + public void configureAutonomousCommands() { autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java deleted file mode 100644 index c081603..0000000 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ /dev/null @@ -1,11 +0,0 @@ -package frc.robot.constants; - -import org.parts3492.partslib.PARTsUnit; -import org.parts3492.partslib.PARTsUnit.PARTsUnitType; - -public class FieldConstants { - public static final PARTsUnit ZONE1_RADIUS = new PARTsUnit(4, PARTsUnitType.Foot); - public static final PARTsUnit ZONE2_RADIUS = new PARTsUnit(6, PARTsUnitType.Foot); - public static final PARTsUnit ZONE3_RADIUS = new PARTsUnit(8, PARTsUnitType.Foot); - public static final PARTsUnit ZONE4_RADIUS = new PARTsUnit(10, PARTsUnitType.Foot); -} diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 0546ecb..7fbbc1b 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -9,10 +9,10 @@ public class IntakeConstants { public static final double PIVOT_GEAR_RATIO = (12.0/1.0)*(3.0/1.0); // PID Controller - public static final double P = 0; + public static final double P = 0.07; public static final double I = 0; public static final double D = 0; - public static final int PID_THRESHOLD = 5; + public static final int PID_THRESHOLD = 1; // Feedforward public static final double S = 0; diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 96afedb..59bb58c 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -12,6 +12,6 @@ public class RobotConstants { PARTsUnitType.Inch); public static boolean LOGGING = true; - public static boolean DEBUGGING = true; + public static boolean DEBUGGING = false; public static boolean ALLOW_AUTO_CONTROLLER_DETECTION = false; -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/states/IntakeState.java b/src/main/java/frc/robot/states/IntakeState.java index 3e87263..1d80d0b 100644 --- a/src/main/java/frc/robot/states/IntakeState.java +++ b/src/main/java/frc/robot/states/IntakeState.java @@ -4,9 +4,10 @@ public enum IntakeState { IDLE(0, 0), DISABLED(0, 0), - INTAKING(0.5, 90), - OUTTAKING(-0.5, 90), - SHOOTING(0, 15); + INTAKING(0.5, 190), + OUTTAKING(-0.5, 190), + SHOOTING(0, 45), + HOME(0, 0); private double speed; private double angle; diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index de60ecd..70c491d 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -65,12 +65,15 @@ public void periodic() { case INTAKING: case OUTTAKING: case SHOOTING: + case HOME: setIntakeSpeed(intakeState.getSpeed()); intakePIDController.setSetpoint(intakeState.getAngle()); double pidCalc = intakePIDController.calculate(getPivotAngle().to(PARTsUnitType.Angle), intakeState.getAngle()); double ffCalc = intakeFeedForward.calculate(intakePIDController.getSetpoint()); - //setPivotVoltage(pidCalc + ffCalc); + partsNT.putBoolean("At goal", intakePIDController.atSetpoint()); + + setPivotVoltage(pidCalc); break; default: setIntakeSpeed(0); @@ -112,5 +115,17 @@ public Command intakeShooting() { intakeState = IntakeState.SHOOTING; })); } + + public Command intakeIdle() { + return PARTsCommandUtils.setCommandName("Command IntakeIdle", this.runOnce(() -> { + intakeState = IntakeState.IDLE; + })); + } + + public Command home() { + return PARTsCommandUtils.setCommandName("Command IntakeHome", this.runOnce(() -> { + intakeState = IntakeState.HOME; + })); + } //endregion } diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java index 2ad5333..a49da28 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java @@ -77,9 +77,11 @@ public void periodic() { case SHOOTING: double voltage = 0; Targets zone = Hub.getZone(poseSupplier); - shooterPIDController.setSetpoint(shooterState.getZoneRPM(zone)); + double zoneRPM = shooterState.getZoneRPM(zone); + double shooterRPM = shooterState.getRPM(); + shooterPIDController.setSetpoint(shooterRPM); - double pidCalc = shooterPIDController.calculate(getRPM(), shooterState.getZoneRPM(zone)); + double pidCalc = shooterPIDController.calculate(getRPM(), shooterRPM); double ffCalc = shooterFeedforward.calculate((shooterPIDController.getSetpoint() * Math.PI * ShooterConstants.SHOOTER_WHEEL_RADIUS.to(PARTsUnitType.Meter) * 2) / 60); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 62a6414..2ad0536 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -7,6 +7,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import frc.robot.subsystems.Hopper.Hopper; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.Kicker.Kicker; @@ -29,26 +32,21 @@ public Superstructure(Hopper hopper, Intake intake, Kicker kicker, Shooter shoot } /** - * lift up pivot arm, roll hopper, roll kicker, shoot. Only happens if turret - * has valid angle + * lift up pivot arm, roll hopper, roll kicker, shoot. Only happens if turret has valid angle */ public Command shoot(BooleanSupplier end) { - return PARTsCommandUtils.setCommandName("Superstructure.shoot", Commands.run(() -> { - if (turret.isValidAngle()) { - turret.track(); - intake.intakeShooting(); - hopper.roll(); - kicker.roll(); - shooter.shoot(); - } - }, hopper, intake, kicker, shooter, turret).until(() -> !turret.isValidAngle() || end.getAsBoolean()) + return PARTsCommandUtils + .setCommandName("Superstructure.shoot", + Commands.parallel(turret.track(), intake.intakeShooting(), hopper.roll(), kicker.roll(), + shooter.shoot()).onlyIf(turret::isValidAngle)) + .andThen(new WaitUntilCommand(() -> !turret.isValidAngle() || end.getAsBoolean())) .andThen(Commands.runOnce(() -> { turret.reset(); intake.reset(); hopper.reset(); kicker.reset(); shooter.reset(); - }, hopper, intake, kicker, shooter, turret))); + }, hopper, intake, kicker, shooter, turret)); } @Override From 97d0c4c8369dd240d62398fdf3c158c6b4d6f05d Mon Sep 17 00:00:00 2001 From: team3492backup Date: Sat, 21 Feb 2026 19:25:09 -0500 Subject: [PATCH 09/16] sysid tests --- src/main/java/frc/robot/RobotContainer.java | 30 ++++++++++--------- .../robot/subsystems/Shooter/ShooterPhys.java | 2 +- .../subsystems/Shooter/ShooterSysid.java | 2 +- 3 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index efd185c..ef4911d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import java.util.function.BooleanSupplier; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -53,6 +54,7 @@ import frc.robot.subsystems.Shooter.Shooter; import frc.robot.subsystems.Shooter.ShooterPhys; import frc.robot.subsystems.Shooter.ShooterSim; +import frc.robot.subsystems.Shooter.ShooterSysid; import frc.robot.subsystems.Turret.Turret; import frc.robot.subsystems.Turret.TurretPhys; import frc.robot.subsystems.Turret.TurretSim; @@ -93,7 +95,7 @@ public class RobotContainer { public final Candle candle = new Candle(); - private final Shooter shooter = Robot.isReal() ? new ShooterPhys(drivetrain.supplierGetPose()) : new ShooterSim(drivetrain.supplierGetPose()); + //private final Shooter shooter = Robot.isReal() ? new ShooterPhys(drivetrain.supplierGetPose()) : new ShooterSim(drivetrain.supplierGetPose()); private final Turret turret = Robot.isReal() ? new TurretPhys(drivetrain.supplierGetPose()) : new TurretSim(drivetrain.supplierGetPose()); @@ -104,12 +106,11 @@ public class RobotContainer { private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim(); - private final Superstructure superstructure = new Superstructure(hopper, intake, kicker, shooter, turret); - - // private final ShooterSysid shooter = new ShooterSysid(); //for sysid + private final ShooterSysid shooter = new ShooterSysid(drivetrain.supplierGetPose()); //for sysid // private final IntakeSysid intake = new IntakeSysid(); //for sysid // private final TurretSysid turret = new TurretSysid(drivetrain.supplierGetPose()); + private final Superstructure superstructure = new Superstructure(hopper, intake, kicker, shooter, turret); private final ArrayList subsystems = new ArrayList( Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake, superstructure)); @@ -124,6 +125,8 @@ public RobotContainer() { configureIntakeBindings(); configureHopperBindings(); configureSuperstructureBindings(); + operatorController.povUp().onTrue(Commands.runOnce(() -> SignalLogger.start(), null)); + operatorController.povDown().onTrue(Commands.runOnce(() -> SignalLogger.stop(), null)); partsNT.putSmartDashboardSendable("field", Field.FIELD2D); hubFieldObject2d = Field.FIELD2D.getObject("hub"); @@ -192,16 +195,15 @@ private void configureShooterBindings() { // driveController.a().onTrue(shooter.shoot()); // driveController.b().onTrue(shooter.idle()); - /* - * operatorController.a().and(operatorController.rightBumper()) - * .whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - * operatorController.b().and(operatorController.rightBumper()) - * .whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - * operatorController.x().and(operatorController.rightBumper()) - * .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); - * operatorController.y().and(operatorController.rightBumper()) - * .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - */ + + operatorController.a().and(operatorController.rightBumper()) + .whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + operatorController.b().and(operatorController.rightBumper()) + .whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + operatorController.x().and(operatorController.rightBumper()) + .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); + operatorController.y().and(operatorController.rightBumper()) + .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } private void configureCandleBindings() { diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java index 28f564d..a3e5e96 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java @@ -62,7 +62,7 @@ protected void setSpeed(double speed) { @Override protected double getRPM() { - return leftMotor.getVelocity().getValueAsDouble() / 60; + return leftMotor.getVelocity().getValueAsDouble() * 60; } @Override diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java index a5f79e3..a3afbb7 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java @@ -43,7 +43,7 @@ public ShooterSysid(Supplier poseSupplier) { (log) -> { log.motor("shootermotor1") .voltage(appliedVoltage.mut_replace( - super.leftMotor.getStatorCurrent().getValueAsDouble(), Volts)) + super.leftMotor.getMotorVoltage().getValueAsDouble(), Volts)) .linearPosition(shooterPosition.mut_replace( super.leftMotor.getPosition().getValueAsDouble() * Math.PI * ShooterConstants.SHOOTER_WHEEL_RADIUS.to(PARTsUnitType.Inch) * 2, Inches)) .linearVelocity(shooterVelocity.mut_replace( From 2c496301d936f85778ced5f2e2319ba17b2c8e0a Mon Sep 17 00:00:00 2001 From: team3492backup Date: Sat, 21 Feb 2026 20:00:40 -0500 Subject: [PATCH 10/16] WE CAN FINALLY SHOOT --- src/main/java/frc/robot/RobotContainer.java | 20 +++++++++---------- .../frc/robot/constants/ShooterConstants.java | 6 +++--- .../java/frc/robot/states/HopperState.java | 4 ++-- .../java/frc/robot/states/IntakeState.java | 6 +++--- .../java/frc/robot/states/KickerState.java | 2 +- .../java/frc/robot/states/ShooterState.java | 4 ++-- 6 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ef4911d..b1675dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -95,7 +95,7 @@ public class RobotContainer { public final Candle candle = new Candle(); - //private final Shooter shooter = Robot.isReal() ? new ShooterPhys(drivetrain.supplierGetPose()) : new ShooterSim(drivetrain.supplierGetPose()); + private final Shooter shooter = Robot.isReal() ? new ShooterPhys(drivetrain.supplierGetPose()) : new ShooterSim(drivetrain.supplierGetPose()); private final Turret turret = Robot.isReal() ? new TurretPhys(drivetrain.supplierGetPose()) : new TurretSim(drivetrain.supplierGetPose()); @@ -106,7 +106,7 @@ public class RobotContainer { private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim(); - private final ShooterSysid shooter = new ShooterSysid(drivetrain.supplierGetPose()); //for sysid + //private final ShooterSysid shooter = new ShooterSysid(drivetrain.supplierGetPose()); //for sysid // private final IntakeSysid intake = new IntakeSysid(); //for sysid // private final TurretSysid turret = new TurretSysid(drivetrain.supplierGetPose()); @@ -125,8 +125,8 @@ public RobotContainer() { configureIntakeBindings(); configureHopperBindings(); configureSuperstructureBindings(); - operatorController.povUp().onTrue(Commands.runOnce(() -> SignalLogger.start(), null)); - operatorController.povDown().onTrue(Commands.runOnce(() -> SignalLogger.stop(), null)); + operatorController.povUp().onTrue(Commands.runOnce(() -> SignalLogger.start())); + operatorController.povDown().onTrue(Commands.runOnce(() -> SignalLogger.stop())); partsNT.putSmartDashboardSendable("field", Field.FIELD2D); hubFieldObject2d = Field.FIELD2D.getObject("hub"); @@ -192,18 +192,18 @@ private void configureDrivetrainBindings() { } private void configureShooterBindings() { - // driveController.a().onTrue(shooter.shoot()); - // driveController.b().onTrue(shooter.idle()); + //driveController.a().onTrue(shooter.shoot()); + //driveController.b().onTrue(shooter.idle()); - operatorController.a().and(operatorController.rightBumper()) + /*operatorController.a().and(operatorController.rightBumper()) .whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); operatorController.b().and(operatorController.rightBumper()) .whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); operatorController.x().and(operatorController.rightBumper()) .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); operatorController.y().and(operatorController.rightBumper()) - .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse));*/ } private void configureCandleBindings() { @@ -231,7 +231,7 @@ private void configureTurretBindings() { } private void configureIntakeBindings() { - //driveController.x().onTrue(intake.intake()); + driveController.x().onTrue(intake.intake()); //driveController.b().onTrue(intake.intakeIdle()); //driveController.x().onTrue(intake.home()); @@ -249,7 +249,7 @@ private void configureIntakeBindings() { } private void configureSuperstructureBindings() { - //driveController.a().onTrue(superstructure.shoot(driveController.b()::getAsBoolean)); + driveController.a().onTrue(superstructure.shoot(driveController.b()::getAsBoolean)); } public void configureAutonomousCommands() { diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index 92f170c..5a9b9a7 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -20,7 +20,7 @@ public class ShooterConstants { public static final int PID_THRESHOLD = 50; // Feedforward - public static final double S = 0.27506; - public static final double V = 0.53757; - public static final double A = 0.057542; + public static final double S = 0.041561; + public static final double V = 0.48125; + public static final double A = 0.019433; } diff --git a/src/main/java/frc/robot/states/HopperState.java b/src/main/java/frc/robot/states/HopperState.java index 76391d0..76b08e1 100644 --- a/src/main/java/frc/robot/states/HopperState.java +++ b/src/main/java/frc/robot/states/HopperState.java @@ -4,8 +4,8 @@ public enum HopperState { IDLE(0), DISABLED(0), - ROLLING(0.3), - BACKROLLING(-0.3); + ROLLING(1), + BACKROLLING(-1); private final double speed; diff --git a/src/main/java/frc/robot/states/IntakeState.java b/src/main/java/frc/robot/states/IntakeState.java index 1d80d0b..ca07a35 100644 --- a/src/main/java/frc/robot/states/IntakeState.java +++ b/src/main/java/frc/robot/states/IntakeState.java @@ -4,9 +4,9 @@ public enum IntakeState { IDLE(0, 0), DISABLED(0, 0), - INTAKING(0.5, 190), - OUTTAKING(-0.5, 190), - SHOOTING(0, 45), + INTAKING(0.75, 190), + OUTTAKING(-0.75, 190), + SHOOTING(0, 90), HOME(0, 0); private double speed; private double angle; diff --git a/src/main/java/frc/robot/states/KickerState.java b/src/main/java/frc/robot/states/KickerState.java index e0e1e1a..05a2948 100644 --- a/src/main/java/frc/robot/states/KickerState.java +++ b/src/main/java/frc/robot/states/KickerState.java @@ -4,7 +4,7 @@ public enum KickerState { IDLE(0), DISABLED(0), - ROLLING(0.1); + ROLLING(0.75); private final double speed; diff --git a/src/main/java/frc/robot/states/ShooterState.java b/src/main/java/frc/robot/states/ShooterState.java index eec61b8..b30e9a7 100644 --- a/src/main/java/frc/robot/states/ShooterState.java +++ b/src/main/java/frc/robot/states/ShooterState.java @@ -10,8 +10,8 @@ public enum ShooterState { IDLE(0), DISABLED(0), - CHARGING(3000), - SHOOTING(3000); + CHARGING(3500), + SHOOTING(3500); private final double rpm; ShooterState(double rpm) { From 4414bd5ad57c161883a88c85332f301210674627 Mon Sep 17 00:00:00 2001 From: Brandon Date: Sat, 21 Feb 2026 21:58:31 -0500 Subject: [PATCH 11/16] try to fix pipeline --- .github/workflows/ci.sh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/ci.sh b/.github/workflows/ci.sh index 4beca4c..c8c2ffe 100644 --- a/.github/workflows/ci.sh +++ b/.github/workflows/ci.sh @@ -3,6 +3,9 @@ # Make sure that Gradlew is executable. chmod +x gradlew +# Fix ant violations with lib +./gradlew :PARTsLib:spotlessApply + # Start simulation. ./gradlew simulateJavaRelease & sim_pid=$! From c8af02dbb96091df9976444829d7e4916dc19791 Mon Sep 17 00:00:00 2001 From: Brandon Date: Sat, 21 Feb 2026 22:09:59 -0500 Subject: [PATCH 12/16] another pipelien fix --- .github/workflows/ci.sh | 2 -- .github/workflows/ci.yml | 5 +++++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.sh b/.github/workflows/ci.sh index c8c2ffe..ba6f835 100644 --- a/.github/workflows/ci.sh +++ b/.github/workflows/ci.sh @@ -3,8 +3,6 @@ # Make sure that Gradlew is executable. chmod +x gradlew -# Fix ant violations with lib -./gradlew :PARTsLib:spotlessApply # Start simulation. ./gradlew simulateJavaRelease & diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ec68183..d43d585 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -27,6 +27,11 @@ jobs: - name: Grant execute permission for ci script run: chmod +x ./.github/workflows/ci.sh + + # Fix any violations with lib + - name: Grant execute permission for gradlew + run: ./gradlew :PARTsLib:spotlessApply + # Runs a single command using the runners shell - name: Compile robot code run: ./gradlew build From 23d06e78fe0ea5e45337b6103b2df1937b011426 Mon Sep 17 00:00:00 2001 From: Brandon Date: Sat, 21 Feb 2026 22:10:14 -0500 Subject: [PATCH 13/16] fix comment --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d43d585..b4cf72d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -29,7 +29,7 @@ jobs: run: chmod +x ./.github/workflows/ci.sh # Fix any violations with lib - - name: Grant execute permission for gradlew + - name: Fix any violations with lib run: ./gradlew :PARTsLib:spotlessApply # Runs a single command using the runners shell From 450033ddec0216b900030eb3c469b2f0b29db651 Mon Sep 17 00:00:00 2001 From: Brandon Date: Sun, 22 Feb 2026 19:51:48 -0500 Subject: [PATCH 14/16] update command names --- .../java/frc/robot/subsystems/Candle.java | 4 +-- .../Drivetrain/PARTsDrivetrain.java | 34 ++++++++----------- .../frc/robot/subsystems/Hopper/Hopper.java | 6 ++-- .../frc/robot/subsystems/Intake/Intake.java | 8 ++--- .../frc/robot/subsystems/Kicker/Kicker.java | 4 +-- .../frc/robot/subsystems/LimelightVision.java | 6 ++-- .../frc/robot/subsystems/Shooter/Shooter.java | 4 +-- .../frc/robot/subsystems/Turret/Turret.java | 4 +-- 8 files changed, 33 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Candle.java b/src/main/java/frc/robot/subsystems/Candle.java index dca0ed3..7e17d5e 100644 --- a/src/main/java/frc/robot/subsystems/Candle.java +++ b/src/main/java/frc/robot/subsystems/Candle.java @@ -37,12 +37,12 @@ public void removeState(CandleState state) { } public Command commandAddState(CandleState state) { - return PARTsCommandUtils.setCommandName("commandAddState", + return PARTsCommandUtils.setCommandName("Candle.commandAddState", Commands.runOnce(() -> addState(state)).ignoringDisable(true)); } public Command commandRemoveState(CandleState state) { - return PARTsCommandUtils.setCommandName("commandRemoveState", + return PARTsCommandUtils.setCommandName("Candle.commandRemoveState", Commands.runOnce(() -> removeState(state)).ignoringDisable(true)); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java index 202f492..f506b2e 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java @@ -225,7 +225,7 @@ public void toggleFineGrainDrive() { public Command commandJoystickDrive(PARTsCommandController controller) { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. - return PARTsCommandUtils.setCommandName("commandJoystickDrive", applyRequest(() -> { + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandJoystickDrive", applyRequest(() -> { double limit = MaxSpeed * frc.robot.constants.DrivetrainConstants.SPEED_PERCENT; if (fineGrainDrive) limit *= 0.25; @@ -249,18 +249,18 @@ public Command commandJoystickDrive(PARTsCommandController controller) { } public Command commandBrake() { - return PARTsCommandUtils.setCommandName("commandBrake", + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandBrake", applyRequest(() -> getBrakeDriveRequest())); } public Command commandPointWheels(PARTsCommandController controller) { - return PARTsCommandUtils.setCommandName("commandPointWheels", applyRequest(() -> getPointDriveRequest() + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandPointWheels", applyRequest(() -> getPointDriveRequest() .withModuleDirection(new Rotation2d(-controller.getLeftY(), -controller.getLeftX())))); } public Command commandSeedFieldCentric() { - return PARTsCommandUtils.setCommandName("commandSeedFieldCentric", + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandSeedFieldCentric", this.runOnce(() -> super.seedFieldCentric())); } @@ -357,8 +357,7 @@ public Command commandAlign(Supplier goalPose) { () -> ((xRangeController.atGoal() && yRangeController.atGoal() && thetaController.atGoal()) || timerElapsed)); - c.setName("align"); - return c; + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandAlign", c); } public Pose2d getPose() { @@ -387,7 +386,7 @@ public Command commandSnapToAngle(double angle) { .withVelocityY(0) .withRotationalRate(thetaOutput.getRadians())); }, (Boolean b) -> stop(), () -> thetaController.atGoal(), this); - return c.withName("drivetrain.commandSnapToAngle"); + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandSnapToAngle", c); } public void setChassisSpeeds(ChassisSpeeds robotSpeeds) { @@ -428,8 +427,7 @@ public Command commandPathFindToPath(String pathname) { Command pathfindingCommand = AutoBuilder.pathfindThenFollowPath( path, constraints); - pathfindingCommand.setName("pathFindToPathCommand"); - return pathfindingCommand; + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandPathFindToPath", pathfindingCommand); } catch (IOException e) { e.printStackTrace(); @@ -440,7 +438,7 @@ public Command commandPathFindToPath(String pathname) { // TODO Auto-generated catch block e.printStackTrace(); } - return new WaitCommand(0); + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandPathFindToPath.wait", new WaitCommand(0)); } public Command commandPathFindToPose(Pose2d pose) { @@ -456,14 +454,12 @@ public Command commandPathFindToPose(Pose2d pose) { Command pathfindingCommand = AutoBuilder.pathfindToPose( Field.conditionallyTransformToOppositeAlliance(pose), constraints, 0.0); // Goal end velocity in meters/sec - pathfindingCommand.setName("pathFindToPoseCommand"); - - return pathfindingCommand; + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandPathFindToPose", pathfindingCommand); } public Command commandPathOnTheFly(Pose2d pose) { - return PARTsCommandUtils.setCommandName("commandPathOnTheFly", Commands.defer(() -> { + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandPathOnTheFly", Commands.defer(() -> { PathConstraints constraints = new PathConstraints(2, 2, 2 * Math.PI, 4 * Math.PI); // The // constraints // for this path. @@ -509,7 +505,7 @@ public Command commandPathOnTheFly(Pose2d pose) { * @return the command */ public Command controlledRotateCommand(DoubleSupplier angle, BooleanSupplier condition) { - return Commands.run(() -> { + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.controlledRotateCommand", Commands.run(() -> { if (!isControlledRotationEnabled) { thetaController.reset(getPose().getRotation().getRadians()); } @@ -518,7 +514,7 @@ public Command controlledRotateCommand(DoubleSupplier angle, BooleanSupplier con thetaController.setGoal(angle.getAsDouble() + Math.PI); else thetaController.setGoal(angle.getAsDouble()); - }).until(condition).andThen(disableControlledRotation()).withName("drivetrain.controlledRotate"); + }).until(condition).andThen(disableControlledRotation())); } /** @@ -529,17 +525,17 @@ public Command controlledRotateCommand(DoubleSupplier angle, BooleanSupplier con * @return the command */ public Command targetPoseCommand(Supplier targetPose, BooleanSupplier condition) { - return controlledRotateCommand(() -> { + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.targetPoseCommand", controlledRotateCommand(() -> { Pose2d target = targetPose.get(); Transform2d diff = getPose().minus(target); Rotation2d rot = new Rotation2d(diff.getX(), diff.getY()); rot = rot.plus(Rotation2d.kPi); return rot.getRadians(); - }, condition).withName("drivetrain.targetPose"); + }, condition)); } public Command disableControlledRotation() { - return Commands.runOnce(() -> isControlledRotationEnabled = false); + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.disableControlledRotation", Commands.runOnce(() -> isControlledRotationEnabled = false)); } public Consumer> consumerSetVisionMeasurementStdDevs() { diff --git a/src/main/java/frc/robot/subsystems/Hopper/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper/Hopper.java index e68a052..129aea2 100644 --- a/src/main/java/frc/robot/subsystems/Hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper/Hopper.java @@ -69,19 +69,19 @@ public void periodic() { public HopperState getState() { return hopperstate; } public Command roll() { - return PARTsCommandUtils.setCommandName("Command Roll", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Hopper.roll", this.runOnce(() -> { hopperstate = HopperState.ROLLING; })); } public Command backRoll() { - return PARTsCommandUtils.setCommandName("Command BackRoll", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Hopper.backRoll", this.runOnce(() -> { hopperstate = HopperState.BACKROLLING; })); } public Command idle() { - return PARTsCommandUtils.setCommandName("Command Hopper Idle", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Hopper.idle", this.runOnce(() -> { hopperstate = HopperState.IDLE; })); } diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 70c491d..3a674b9 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -105,25 +105,25 @@ public void log() { public abstract double getPivotRotationSpeed(); public Command intake() { - return PARTsCommandUtils.setCommandName("Command Intake", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Intake.intake", this.runOnce(() -> { intakeState = IntakeState.INTAKING; })); } public Command intakeShooting() { - return PARTsCommandUtils.setCommandName("Command IntakeShooting", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Intake.intakeShooting", this.runOnce(() -> { intakeState = IntakeState.SHOOTING; })); } public Command intakeIdle() { - return PARTsCommandUtils.setCommandName("Command IntakeIdle", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Intake.intakeIdle", this.runOnce(() -> { intakeState = IntakeState.IDLE; })); } public Command home() { - return PARTsCommandUtils.setCommandName("Command IntakeHome", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Intake.home", this.runOnce(() -> { intakeState = IntakeState.HOME; })); } diff --git a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java index d4c0b30..f76f4b6 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java +++ b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java @@ -72,13 +72,13 @@ public KickerState getState() { } public Command roll() { - return PARTsCommandUtils.setCommandName("Command Roll", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Kicker.roll", this.runOnce(() -> { kickerState = KickerState.ROLLING; })); } public Command idle() { - return PARTsCommandUtils.setCommandName("Command Idle", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Kicker.idle", this.runOnce(() -> { kickerState = KickerState.IDLE; })); } diff --git a/src/main/java/frc/robot/subsystems/LimelightVision.java b/src/main/java/frc/robot/subsystems/LimelightVision.java index c30948f..abfa730 100644 --- a/src/main/java/frc/robot/subsystems/LimelightVision.java +++ b/src/main/java/frc/robot/subsystems/LimelightVision.java @@ -100,7 +100,7 @@ public void setMegaTagMode(MegaTagMode mode) { } public Command commandMegaTagMode(MegaTagMode mode) { - Command c = PARTsCommandUtils.setCommandName("commandMegaTagMode", this.runOnce(() -> setMegaTagMode(mode))); + Command c = PARTsCommandUtils.setCommandName("LimelightVision.commandMegaTagMode", this.runOnce(() -> setMegaTagMode(mode))); c = c.ignoringDisable(true); return c; } @@ -256,7 +256,7 @@ public void log() { // throw new UnsupportedOperationException("Unimplemented method 'log'"); } - public void resetRobotPose() { + /*public void resetRobotPose() { setMegaTagMode(MegaTagMode.MEGATAG1); for (Camera camera : CameraConstants.LimelightCameras) { LimelightHelpers.SetRobotOrientation( @@ -284,7 +284,7 @@ public void resetRobotPose() { } } setMegaTagMode(MegaTagMode.MEGATAG2); - } + }*/ public void setPipelineIndex(Pipelines pipeline) { partsNT.putString("Pipeline name", pipeline.name()); diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java index a49da28..6df7da8 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java @@ -116,13 +116,13 @@ public ShooterState getState() { } public Command shoot() { - return PARTsCommandUtils.setCommandName("Command Shoot", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Kicker.shoot", this.runOnce(() -> { shooterState = ShooterState.SHOOTING; })); } public Command idle() { - return PARTsCommandUtils.setCommandName("Command Shooter Idle", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Kicker.idle", this.runOnce(() -> { shooterState = ShooterState.IDLE; })); } diff --git a/src/main/java/frc/robot/subsystems/Turret/Turret.java b/src/main/java/frc/robot/subsystems/Turret/Turret.java index b67f6d0..ac7560f 100644 --- a/src/main/java/frc/robot/subsystems/Turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/Turret/Turret.java @@ -122,13 +122,13 @@ public TurretState getState() { } public Command track() { - return PARTsCommandUtils.setCommandName("Command Track", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Turret.track", this.runOnce(() -> { turretState = TurretState.TRACKING; })); } public Command idle() { - return PARTsCommandUtils.setCommandName("Command Idle", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Turret.idle", this.runOnce(() -> { turretState = TurretState.IDLE; })); } From 1c6159cda41566e7a61ecc4ee517245b0dfaa2a6 Mon Sep 17 00:00:00 2001 From: Brandon Date: Sun, 22 Feb 2026 19:52:13 -0500 Subject: [PATCH 15/16] update lib --- PARTsLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PARTsLib b/PARTsLib index d05cbf9..bbfcbd0 160000 --- a/PARTsLib +++ b/PARTsLib @@ -1 +1 @@ -Subproject commit d05cbf9e8880d79b23d4e14f8ef67b0cad44b475 +Subproject commit bbfcbd0e76b4ed0fa0565cbc3d3703619ffd6a9c From 90d7202e3ce5d4343f16901e6f545023c2730f94 Mon Sep 17 00:00:00 2001 From: Brandon Duke Date: Mon, 23 Feb 2026 10:12:52 -0500 Subject: [PATCH 16/16] add comment --- PARTsLib | 2 +- .../java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/PARTsLib b/PARTsLib index bbfcbd0..621fc4c 160000 --- a/PARTsLib +++ b/PARTsLib @@ -1 +1 @@ -Subproject commit bbfcbd0e76b4ed0fa0565cbc3d3703619ffd6a9c +Subproject commit 621fc4c5daf021b21d9b8d6ec42c3cb202b698c0 diff --git a/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java index f506b2e..cadb875 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java @@ -555,7 +555,9 @@ public Supplier supplierGetPose() { } - public boolean acceptVisionMeasurement(Pose2d measurement, double timestamp) { + public boolean acceptVisionMeasurement(Pose2d measurement, double timestamp) { + // accept values rotating less than + // 2*pi rad/s = 360 deg/s if (Math.max(Math.abs(getXAngularVelocity()), Math.abs(getYAngularVelocity())) < 2 * Math.PI) { super.addVisionMeasurement(measurement, timestamp); return true;