From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Thu, 26 Feb 2026 02:36:34 +0000 (-0800) Subject: a X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=3f424a11779d46566e758a5ff8b5cfd9ed03a726;p=FRC2026.git a --- diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 39367b7..a77b29b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import frc.robot.commands.vision.ShutdownAllPis; import frc.robot.constants.AutoConstants; import frc.robot.constants.Constants; import frc.robot.constants.IntakeConstants; +import frc.robot.constants.VisionConstants; import frc.robot.controls.BaseDriverConfig; import frc.robot.controls.Operator; import frc.robot.controls.PS5ControllerDriverConfig; @@ -104,18 +105,18 @@ public class RobotContainer { intake = new Intake(); case WaffleHouse: // AKA Betabot - // turret = new Turret(); + turret = new Turret(); shooter = new Shooter(); hood = new Hood(); case SwerveCompetition: // AKA "Vantage" case BetaBot: // AKA "Pancake" - // vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); + vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); // fall-through case Vivace: - // linearClimb = new LinearClimb(); + //linearClimb = new LinearClimb(); case Phil: // AKA "IHOP" diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java index 60cb84e..124e3ea 100644 --- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java +++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java @@ -23,14 +23,15 @@ public class ReverseMotors extends Command { @Override public void execute(){ + intake.extend(); intake.spinReverse(); - spindexer.reverseSpindexer(); + //spindexer.reverseSpindexer(); } @Override public void end(boolean interrupted){ - intake.spinStart(); - spindexer.maxSpindexer(); + intake.spinStop(); + //spindexer.maxSpindexer(); } } diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index e721a39..f5707b0 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -217,7 +217,7 @@ public class Superstructure extends Command { } else{ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity); } - + shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); if (phaseManager.shouldFeed()) { @@ -230,6 +230,8 @@ public class Superstructure extends Command { SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint); SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint); SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel()); + + SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString()); } @Override diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 578b8cb..e09f459 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.util.Units; public class IntakeConstants { /** Intake roller motor speed in range [-1, 1] */ - public static final double SPEED = 1.0; + public static final double SPEED = 0.8; /** 12 tooth pinion driving 36 tooth driven gear */ public static final double GEAR_RATIO = 36.0/12.0; /** radius (inches) of the rack gear which is a 10 tooth pinion at 10 DP */ @@ -22,9 +22,9 @@ public class IntakeConstants { /** max extension in inches */ - public static final double MAX_EXTENSION = 10.0 - 2.0; // inches + public static final double MAX_EXTENSION = 10.5; // inches - public static final double INTERMEDIATE_EXTENSION = 5.0 - 2.0; //inches + public static final double INTERMEDIATE_EXTENSION = 5.0; //inches public static final double STOW_EXTENSION = 0.2; // inches diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java index b4643d6..194b48b 100644 --- a/src/main/java/frc/robot/constants/ShotInterpolation.java +++ b/src/main/java/frc/robot/constants/ShotInterpolation.java @@ -21,7 +21,8 @@ public class ShotInterpolation { hoodAngleMap.put(1.0, Units.degreesToRadians(90)); //TODO: find actual values from video motion - exitVelocityMap.put(1.0, 2.0); - exitVelocityMap.put(2.0, 4.0); + exitVelocityMap.put(0.0, 0.0); + exitVelocityMap.put(1.0, 2.2); + exitVelocityMap.put(2.0, 4.4); } } diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java index 827817a..c9f6591 100644 --- a/src/main/java/frc/robot/constants/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java @@ -25,7 +25,7 @@ public class DriveConstants { */ public static final double ROBOT_WIDTH_WITH_BUMPERS = 0.832; - public static double ROBOT_MASS = Units.lbsToKilograms(108.3 + 13 + 13.4); + public static double ROBOT_MASS = Units.lbsToKilograms(108.3 + 13 + 13.4 + 5.0); /** Radius of the drive wheels [meters]. */ public static final double WHEEL_RADIUS = Units.inchesToMeters(1.95); diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index d85206f..ddc0e40 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -114,6 +114,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { controller.get(PS5Button.RIGHT_TRIGGER).debounce(3.0).onTrue(new InstantCommand(() -> { intake.retract(); intakeBoolean = true; + intake.spinStop(); })); // Calibration @@ -122,6 +123,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { })).onFalse(new InstantCommand(() -> { intake.stopCalibrating(); })); + + // Stop intake roller + controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{ + intake.spinStop(); + })); } // Spindexer @@ -140,7 +146,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { // Climb if (climb != null) { // Calibration - controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> { + controller.get(PS5Button.OPTIONS).onTrue(new InstantCommand(() -> { climb.hardstopCalibration(); })).onFalse(new InstantCommand(() -> { climb.stopCalibrating(); diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java index 784aaf6..9667a77 100644 --- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java +++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java @@ -36,7 +36,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{ private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising); // logging information - private LinearClimbIOInputs inputs = new LinearClimbIOInputs(); + private LinearClimbIOInputsAutoLogged inputs = new LinearClimbIOInputsAutoLogged(); private final PIDController pid = new PIDController( ClimbConstants.PID_P, @@ -73,6 +73,9 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{ * @param setpoint in rotations */ public void setSetpoint(double setpoint) { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + motor.getConfigurator().apply(config); pid.setSetpoint(setpoint); } @@ -145,6 +148,8 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{ motor.set(power); Logger.recordOutput("LinearClimb/setpointMeters", Units.rotationsToRadians(pid.getSetpoint()) * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO); + updateInputs(); + Logger.processInputs("LinearClimb", inputs); } /** * converts motor rotations to meters diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 4b033f7..75eaeca 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -69,7 +69,7 @@ public class Turret extends SubsystemBase implements TurretIO{ motor.setNeutralMode(NeutralModeValue.Brake); TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.Slot0.kP = 12.0; config.Slot0.kS = 0.1; // Static friction compensation @@ -130,6 +130,14 @@ public class Turret extends SubsystemBase implements TurretIO{ //Sets the initial motor position motor.setPosition(motorRotations); + motor.setPosition(0.0); + + SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);})); + SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);})); + SmartDashboard.putData("Turn to 90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0);})); + SmartDashboard.putData("Turn to 200", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0);})); + SmartDashboard.putData("Turn to -200", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0);})); + } /* ---------------- Public API ---------------- */ diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 83dd1c3..9b3dd6b 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -22,8 +22,8 @@ public class TurretConstants { public static int RIGHT_ENCODER_TEETH = 22; // read above public static int ENCODER_COUNT_TOTAL = 8192; // how many intervals it can have, like clicks on a clock chat gpt explained to me - public static double LEFT_ENCODER_OFFSET = 0.463379; // rot - public static double RIGHT_ENCODER_OFFSET = 0.197266; // rot + public static double LEFT_ENCODER_OFFSET = 0.364502; // rot + public static double RIGHT_ENCODER_OFFSET = 0.718506; // rot // Turret is in center of robot, but make use of the height in shooter physics public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters @@ -34,7 +34,7 @@ public class TurretConstants { public static final double FEEDFORWARD_KV = 0.185; - public static final double NORMAL_CURRENT_LIMIT = 50.0; // A + public static final double NORMAL_CURRENT_LIMIT = 20.0; // A public static final double CALIBRATION_CURRENT_LIMIT = 10.0; // A public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A