From ac2a7e5a864fc613000ea99c216260cbcc97bd9c Mon Sep 17 00:00:00 2001 From: iefomit Date: Tue, 17 Feb 2026 12:41:11 -0800 Subject: [PATCH] more cleanup --- .vscode/settings.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 1 - .../robot/commands/gpm/AutoShootCommand.java | 3 +- .../robot/commands/gpm/SimpleAutoShoot.java | 57 ++++-------- .../commands/vision/AcquireGamePiece.java | 7 +- .../java/frc/robot/constants/Constants.java | 10 +- .../frc/robot/constants/FieldConstants.java | 41 ++++----- .../controls/PS5ControllerDriverConfig.java | 50 +++++----- .../frc/robot/subsystems/climb/Climb.java | 46 +++++----- .../java/frc/robot/subsystems/hood/Hood.java | 92 +++++++++---------- .../robot/subsystems/hood/HoodConstants.java | 10 +- .../frc/robot/subsystems/hood/HoodIO.java | 2 +- 12 files changed, 139 insertions(+), 182 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 2af50e3..5e6ede8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,5 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.dependency.enableDependencyCheckup": false, + "java.dependency.enableDependencyCheckup": false } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 54e3ec4..04f7687 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -98,7 +98,6 @@ public class RobotContainer { case WaffleHouse: // AKA Betabot turret = new Turret(); shooter = new Shooter(); - // hood = new Hood(); break; case SwerveCompetition: // AKA "Vantage" diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index 7aa4cd5..0d55269 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -196,8 +196,7 @@ public class AutoShootCommand extends Command { // Add 180 degrees to the rotation bc robot is backwards drivepose = new Pose2d( currentPose.getTranslation(), - currentPose.getRotation() - ); + currentPose.getRotation()); ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); drivepose.exp( new Twist2d( diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java index f260c0b..64d94c7 100644 --- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java +++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java @@ -14,25 +14,19 @@ import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.turret.Turret; -import frc.robot.util.FieldZone; -import frc.robot.util.Vision.TurretVision; public class SimpleAutoShoot extends Command { private Turret turret; private Drivetrain drivetrain; - private TurretVision turretVision; private Shooter shooter; private double fieldAngleRad; private double turretSetpoint; private double adjustedSetpoint; - private double yawToTagCamera; private double yawToTag; - private boolean turretVisionEnabled = false; private boolean SOTM = true; private Translation2d drivepose; - private double lastPos = 0; private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); private double lastTurretAngle = 0; @@ -44,27 +38,30 @@ public class SimpleAutoShoot extends Command { this.turret = turret; this.drivetrain = drivetrain; this.shooter = shooter; - drivepose = drivetrain.getPose().getTranslation(); - + drivepose = drivetrain.getPose().getTranslation(); + addRequirements(turret); } public void updateTurretSetpoint(Translation2d drivepose) { - - //FieldZone currentZone = getZone(drivepose); + + // FieldZone currentZone = getZone(drivepose); Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); double D_y; double D_x; - double timeToGoal = 0.0; - + // If the robot is moving, adjust the target position based on velocity if (SOTM) { ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); double xVel = fieldRelVel.vxMetersPerSecond; double yVel = fieldRelVel.vyMetersPerSecond; - + + double distance = target.getDistance(drivepose); + double speed = Math.hypot(xVel, yVel); + double timeToGoal = speed > 0.1 ? distance / speed : 0.0; + D_y = target.getY() - drivepose.getY() - timeToGoal * yVel; D_x = target.getX() - drivepose.getX() - timeToGoal * xVel; } else { @@ -76,7 +73,8 @@ public class SimpleAutoShoot extends Command { fieldAngleRad = Math.atan2(D_y, D_x); // Calculate robot heading and adjust for reverse drive - double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive adjustment + double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive + // adjustment // Calculate turret setpoint (angle relative to robot heading) turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0); @@ -84,7 +82,7 @@ public class SimpleAutoShoot extends Command { // System.out.println("Aligning the turn to degree angle: " + turretSetpoint); } - public void updateYawToTag(){ + public void updateYawToTag() { // Calculate the yaw offset to the tag double D_y = FieldConstants.getHubTranslation().getY() - drivetrain.getPose().getY(); double D_x = FieldConstants.getHubTranslation().getX() - drivetrain.getPose().getX(); @@ -96,33 +94,25 @@ public class SimpleAutoShoot extends Command { public void initialize() { // Initialize setpoint calculation and set the initial goal for the turret updateTurretSetpoint(drivepose); - // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), drivetrain.getAngularRate(2)); turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), 0); } @Override public void execute() { - //shooter.setShooterPower(ShotInterpolation.shooterPowerMap.get(FieldConstants.getHubTranslation().toTranslation2d().getDistance(drivetrain.getPose().getTranslation()))); // Continuously update setpoints and adjust based on vision if available drivepose = drivetrain.getPose().getTranslation(); updateTurretSetpoint(drivepose); updateYawToTag(); - // double turretVelocity = - // turretAngleFilter.calculate( - // new Rotation2d(Units.degreesToRadians(turretSetpoint)).minus(new Rotation2d(Units.degreesToRadians(lastTurretAngle))).getRadians() / Constants.LOOP_TIME); - double velocityAdjustment = 0; - double turretAcceleration = ((-drivetrain.getAngularRate(2)) - (lastFrameVelocity)) / Constants.LOOP_TIME; if (Math.abs(lastTurretAngle - turretSetpoint) > 90) { velocityAdjustment = -drivetrain.getAngularRate(2) * 1.4; } Logger.recordOutput("Spinny accel", drivetrain.getAngularRate(2)); Logger.recordOutput("Original Turret Setpoint", turretSetpoint); - - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) - velocityAdjustment); - // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), (-drivetrain.getAngularRate(2)) + turretAcceleration * 0.3); - //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity); + + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), + -drivetrain.getAngularRate(2) - velocityAdjustment); lastTurretAngle = turretSetpoint; lastFrameVelocity = drivetrain.getAngularRate(2); @@ -133,17 +123,4 @@ public class SimpleAutoShoot extends Command { // Set the turret to a safe position when the command ends turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); } - - public boolean leftSide(Translation2d drivepose) { - if (drivepose.getY() > (FieldConstants.FIELD_WIDTH / 2)) { - return true; - } else { - return false; - } - } - - public FieldZone getZone(Translation2d drivepose) { - return FieldConstants.getZone(drivepose); - } -} - +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java b/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java index d42bd60..a5adbdd 100644 --- a/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java +++ b/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java @@ -10,11 +10,12 @@ import frc.robot.util.Vision.DetectedObject; public class AcquireGamePiece extends SequentialCommandGroup { /** * Intakes a game piece - * + * * @param gamePiece The supplier for the game piece to intake - * @param drive The drivetrain + * @param drive The drivetrain */ - public AcquireGamePiece(Supplier gamePiece, Drivetrain drive){ + public AcquireGamePiece(Supplier gamePiece, Drivetrain drive) { + // TODO: Replace DoNothing with next year's intake command addCommands(new DoNothing().deadlineFor(new DriveToGamePiece(gamePiece, drive))); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index f07a800..8614999 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj.RobotBase; public class Constants { - // constants: + // constants: public static final double GRAVITY_ACCELERATION = 9.8; public static final double ROBOT_VOLTAGE = 12.0; @@ -17,7 +17,7 @@ public class Constants { public static final CANBus RIO_CAN = new CANBus("rio"); public static final CANBus SUBSYSTEM_CANIVORE_CAN = new CANBus("CANivoreSub"); - // Logging + // Logging public static final boolean USE_TELEMETRY = true; public static enum Mode { @@ -67,13 +67,13 @@ public class Constants { public static final double DEFAULT_DEADBAND = 0.00005; public static final double TRANSLATIONAL_DEADBAND = 0.01; - + public static final double ROTATION_DEADBAND = 0.01; - + public static final double HEADING_DEADBAND = 0.05; public static final double HEADING_SLEWRATE = 10; - //Modes + // Modes public static final Mode SIM_MODE = Mode.SIM; public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE; diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 3ba98bc..34b8d96 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -11,11 +11,11 @@ import frc.robot.util.FieldZone; public class FieldConstants { /** Width of the field [meters] */ - public static final double FIELD_LENGTH = Units.inchesToMeters(57*12 + 6+7.0/8.0); + public static final double FIELD_LENGTH = Units.inchesToMeters(57 * 12 + 6 + 7.0 / 8.0); /** Height of the field [meters] */ - public static final double FIELD_WIDTH = Units.inchesToMeters(26*12 + 5); + public static final double FIELD_WIDTH = Units.inchesToMeters(26 * 12 + 5); - /**Apriltag layout for 2026 REBUILT */ + /** Apriltag layout for 2026 REBUILT */ public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); public static final double RED_BORDER = Units.inchesToMeters(180); @@ -24,33 +24,28 @@ public class FieldConstants { public static final double RIGHT_SIDE_TARGET = FIELD_WIDTH * 0.75; /** Location of hub target */ - public static final Translation3d HUB_BLUE = - new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035, Units.inchesToMeters(72)); - - public static final Translation3d HUB_RED = - new Translation3d(FIELD_LENGTH - Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72)); - - public static final Translation3d NEUTRAL_LEFT = - new Translation3d(FIELD_LENGTH/2, LEFT_SIDE_TARGET, 0); + public static final Translation3d HUB_BLUE = new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035, + Units.inchesToMeters(72)); - public static final Translation3d NEUTRAL_RIGHT = - new Translation3d(FIELD_LENGTH/2, RIGHT_SIDE_TARGET, 0); + public static final Translation3d HUB_RED = new Translation3d(FIELD_LENGTH - Units.inchesToMeters(156.8 + 20), + 4.035 + .67, Units.inchesToMeters(72)); - public static final Translation3d ALLIANCE_LEFT_BLUE = - new Translation3d(BLUE_BORDER + 5, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back + public static final Translation3d NEUTRAL_LEFT = new Translation3d(FIELD_LENGTH / 2, LEFT_SIDE_TARGET, 0); - public static final Translation3d ALLIANCE_RIGHT_BLUE = - new Translation3d(BLUE_BORDER + 5, RIGHT_SIDE_TARGET, 0); + public static final Translation3d NEUTRAL_RIGHT = new Translation3d(FIELD_LENGTH / 2, RIGHT_SIDE_TARGET, 0); + // previous hub + a few feet further back + public static final Translation3d ALLIANCE_LEFT_BLUE = new Translation3d(BLUE_BORDER + 5, LEFT_SIDE_TARGET, 0); - public static final Translation3d ALLIANCE_LEFT_RED = - new Translation3d(RED_BORDER + 5, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back + public static final Translation3d ALLIANCE_RIGHT_BLUE = new Translation3d(BLUE_BORDER + 5, RIGHT_SIDE_TARGET, 0); - public static final Translation3d ALLIANCE_RIGHT_RED = - new Translation3d(RED_BORDER + 5, RIGHT_SIDE_TARGET, 0); + // previous hub + a few feet further back + public static final Translation3d ALLIANCE_LEFT_RED = new Translation3d(RED_BORDER + 5, LEFT_SIDE_TARGET, 0); + + public static final Translation3d ALLIANCE_RIGHT_RED = new Translation3d(RED_BORDER + 5, RIGHT_SIDE_TARGET, 0); public static final double BlueAllianceLine = BLUE_BORDER; // That's the distance from one side to the blue bump - public static final double RedAllianceLine = RED_BORDER; // + public static final double RedAllianceLine = RED_BORDER; // That's the distance from one side to the red bump public static Translation3d getHubTranslation() { if (Robot.getAlliance() == Alliance.Blue) { @@ -87,7 +82,7 @@ public class FieldConstants { public static FieldZone getZone(Translation2d drivepose) { double x = drivepose.getX(); double y = drivepose.getY(); - if(x < FieldConstants.RedAllianceLine) { // inside red + if (x < FieldConstants.RedAllianceLine) { // inside red if (Robot.getAlliance() == Alliance.Red) { return FieldZone.ALLIANCE; } else { diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 3779017..81c30b0 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -2,7 +2,6 @@ package frc.robot.controls; import java.util.function.BooleanSupplier; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; @@ -28,20 +27,19 @@ import lib.controllers.PS5Controller.PS5Button; */ public class PS5ControllerDriverConfig extends BaseDriverConfig { private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY); - private final BooleanSupplier slowModeSupplier = ()->false; + private final BooleanSupplier slowModeSupplier = () -> false; private Shooter shooter; private Turret turret; private Hood hood; private Intake intake; private Spindexer spindexer; - private Pose2d alignmentPose = null; - private Command turretAutoShoot; private Command autoShoot; private boolean intakeBoolean = true; - public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake, Spindexer spindexer) { + public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake, + Spindexer spindexer) { super(drive); this.shooter = shooter; this.turret = turret; @@ -50,35 +48,33 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { this.spindexer = spindexer; } - public void configureControls() { + public void configureControls() { // Reset the yaw. Mainly useful for testing/driver practice driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( - new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI) - ))); + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); // Cancel commands - driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(()->{ + driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> { getDrivetrain().setIsAlign(false); - getDrivetrain().setDesiredPose(()->null); + getDrivetrain().setDesiredPose(() -> null); CommandScheduler.getInstance().cancelAll(); })); // Align wheels driver.get(PS5Button.MUTE).onTrue(new FunctionalCommand( - ()->getDrivetrain().setStateDeadband(false), - getDrivetrain()::alignWheels, - interrupted->getDrivetrain().setStateDeadband(true), - ()->false, getDrivetrain()).withTimeout(2)); + () -> getDrivetrain().setStateDeadband(false), + getDrivetrain()::alignWheels, + interrupted -> getDrivetrain().setStateDeadband(true), + () -> false, getDrivetrain()).withTimeout(2)); // Intake - if(intake != null){ - driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{ - if(intakeBoolean){ + if (intake != null) { + driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> { + if (intakeBoolean) { intake.setSetpoint(IntakeConstants.INTAKE_ANGLE); intake.setFlyWheel(); intakeBoolean = false; - } - else{ + } else { intake.setSetpoint(IntakeConstants.STOW_ANGLE); intake.stopFlyWheel(); } @@ -86,20 +82,18 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { } // Auto shoot - if(turret != null){ + if (turret != null) { driver.get(PS5Button.SQUARE).onTrue( - new InstantCommand(()->{ - if (autoShoot != null && autoShoot.isScheduled()){ + new InstantCommand(() -> { + if (autoShoot != null && autoShoot.isScheduled()) { autoShoot.cancel(); - } else{ + } else { autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer); CommandScheduler.getInstance().schedule(autoShoot); } - }) - ); + })); } - } @Override @@ -137,11 +131,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { return false; } - public void startRumble(){ + public void startRumble() { driver.rumbleOn(); } - public void endRumble(){ + public void endRumble() { driver.rumbleOff(); } } diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index cd22dfc..fccfd20 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -21,10 +21,10 @@ import frc.robot.constants.IdConstants; import frc.robot.util.ClimbArmSim; public class Climb extends SubsystemBase { - + private double startingPosition = 0; - //Motors + // Motors // TODO: tune better once design is finalized private final PIDController pid = new PIDController(0.4, 4, 0.04); @@ -34,12 +34,11 @@ public class Climb extends SubsystemBase { private final DCMotor climbGearBox = DCMotor.getKrakenX60(1); private TalonFXSimState encoderSim; - //Mechism2d display + // Mechism2d display private final Mechanism2d simulationMechanism = new Mechanism2d(3, 3); private final MechanismRoot2d mechanismRoot = simulationMechanism.getRoot("Climb", 1.5, 1.5); private final MechanismLigament2d simLigament = mechanismRoot.append( - new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite)) - ); + new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite))); private final double climbGearRatio = 54 / 10 * 60 / 18; // gear ratio of 18 private ClimbArmSim climbSim; @@ -48,19 +47,18 @@ public class Climb extends SubsystemBase { public Climb() { if (isSimulation()) { encoderSim = motorLeft.getSimState(); - encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition)*climbGearRatio); + encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition) * climbGearRatio); climbSim = new ClimbArmSim( - climbGearBox, - climbGearRatio, - 0.1, - 0.127, - 0, //min angle - Units.degreesToRadians(90), //max angle - true, - Units.degreesToRadians(startingPosition), - 60 - ); + climbGearBox, + climbGearRatio, + 0.1, + 0.127, + 0, // min angle + Units.degreesToRadians(90), // max angle + true, + Units.degreesToRadians(startingPosition), + 60); climbSim.setIsClimbing(true); } @@ -68,8 +66,8 @@ public class Climb extends SubsystemBase { pid.setIZone(1); pid.setSetpoint(Units.degreesToRadians(startingPosition)); - motorLeft.setPosition(Units.degreesToRotations(startingPosition)*climbGearRatio); - motorRight.setPosition(Units.degreesToRotations(startingPosition)*climbGearRatio); + motorLeft.setPosition(Units.degreesToRotations(startingPosition) * climbGearRatio); + motorRight.setPosition(Units.degreesToRotations(startingPosition) * climbGearRatio); SmartDashboard.putData("PID", pid); SmartDashboard.putData("Climb Display", simulationMechanism); @@ -78,9 +76,9 @@ public class Climb extends SubsystemBase { } @Override - public void periodic() { + public void periodic() { double motorPosition = motorLeft.getPosition().getValueAsDouble(); - double currentPosition = Units.rotationsToRadians(motorPosition/climbGearRatio); + double currentPosition = Units.rotationsToRadians(motorPosition / climbGearRatio); power = pid.calculate(currentPosition); @@ -106,6 +104,7 @@ public class Climb extends SubsystemBase { /** * Sets the motor to an angle from 0-90 deg + * * @param angle in degrees */ public void setAngle(double angle) { @@ -115,6 +114,7 @@ public class Climb extends SubsystemBase { /** * Gets the current position of the motor in degrees + * * @return The angle in degrees */ public double getAngle() { @@ -124,7 +124,7 @@ public class Climb extends SubsystemBase { /** * Turns the motor to 90 degrees (extended positiion) */ - public void extend(){ + public void extend() { double extendAngle = 90; setAngle(extendAngle); } @@ -132,11 +132,11 @@ public class Climb extends SubsystemBase { /** * Turns the motor to 0 degrees (climb position) */ - public void climb(){ + public void climb() { setAngle(startingPosition); } - public boolean isSimulation(){ + public boolean isSimulation() { return RobotBase.isSimulation(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index d6095b0..3948af5 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -22,74 +22,72 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; -public class Hood extends SubsystemBase implements HoodIO{ - private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN); +public class Hood extends SubsystemBase implements HoodIO { + private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN); - private double MIN_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MIN_ANGLE); + private double MIN_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MIN_ANGLE); private double MAX_ANGLE_RAD = Units.degreesToRadians(HoodConstants.MAX_ANGLE); private double MAX_VEL_RAD_PER_SEC = 25; private double MAX_ACCEL_RAD_PER_SEC2 = 160.0; - private double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO; + private double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO; - private static final PIDController positionPID = new PIDController(6, 0, 0.2); + private static final PIDController positionPID = new PIDController(6.0, 0, 0.2); - private final TrapezoidProfile profile = new TrapezoidProfile( - new TrapezoidProfile.Constraints( - MAX_VEL_RAD_PER_SEC, - MAX_ACCEL_RAD_PER_SEC2)); - private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0); + private final TrapezoidProfile profile = new TrapezoidProfile( + new TrapezoidProfile.Constraints( + MAX_VEL_RAD_PER_SEC, + MAX_ACCEL_RAD_PER_SEC2)); + private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, + 1. / DCMotor.getFalcon500(1).KvRadPerSecPerVolt, 0); private State setpoint = new State(); private Rotation2d goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)); private double goalVelocityRadPerSec = 0.0; - private static double kP = 2.0; - private static double kD = 0.2; + private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); - private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); - - public Hood(){ + public Hood() { motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * GEAR_RATIO); - motor.setNeutralMode(NeutralModeValue.Coast); - - motor.getConfigurator().apply( - new Slot0Configs() - .withKP(kP) - .withKD(kD)); + try { + Thread.sleep(100); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + motor.setNeutralMode(NeutralModeValue.Coast); TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + motor.getConfigurator().apply(config); + + setpoint = new State(getPositionRad() / GEAR_RATIO, 0.0); + + SmartDashboard.putData("max", new InstantCommand( + () -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0))); + SmartDashboard.putData("medium", + new InstantCommand(() -> setFieldRelativeTarget( + new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), + 0))); + SmartDashboard.putData("min", new InstantCommand( + () -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0))); + } - motor.getConfigurator().apply( - new com.ctre.phoenix6.configs.TalonFXConfiguration() { - { - MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - } - }); - - setpoint = new State(getPositionRad() / GEAR_RATIO, 0.0); - - SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0))); - SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0))); - SmartDashboard.putData("min", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0))); - } - - public double getPositionRad(){ - return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()); - } + public double getPositionRad() { + return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()); + } - public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) { + public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) { goalAngle = angle; goalVelocityRadPerSec = velocityRadPerSec; } - @Override - public void periodic() { + @Override + public void periodic() { updateInputs(); - State goalState = new State( + State goalState = new State( MathUtil.clamp(goalAngle.getRadians(), MIN_ANGLE_RAD, MAX_ANGLE_RAD), goalVelocityRadPerSec); @@ -102,29 +100,29 @@ public class Hood extends SubsystemBase implements HoodIO{ double targetVelocity; - double motorSetpointPosition = (setpoint.position) * GEAR_RATIO; + double motorSetpointPosition = (setpoint.position) * GEAR_RATIO; targetVelocity = positionPID.calculate( motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians), motorSetpointPosition); - + targetVelocity += Units.rotationsToRadians(motorVelRotPerSec); double voltage = feedForward.calculate(targetVelocity); motor.setVoltage(voltage); - Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); + Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); Logger.recordOutput("Hood/velocitySetpoint", targetVelocity / GEAR_RATIO); Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(setpoint.position) / GEAR_RATIO); SmartDashboard.putData("Hood PID", positionPID); - SmartDashboard.putNumber("Turret Position Deg", Units.radiansToDegrees(getPositionRad()) / GEAR_RATIO); + SmartDashboard.putNumber("Hood Position Deg", Units.radiansToDegrees(getPositionRad()) / GEAR_RATIO); Logger.processInputs("Hood", inputs); } - @Override + @Override public void updateInputs() { inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / GEAR_RATIO; diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java index 4f5044e..92ad6f4 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java @@ -12,14 +12,11 @@ public class HoodConstants { public static final double CENTER_OF_MASS_LENGTH = 0.138; // meters - // public static final double MAX_VELOCITY = 5; // rad/s public static final double MAX_VELOCITY = 0.30; // rad/s public static final double MAX_ACCELERATION = 30; // rad/s^2 public static final double MAX_ANGLE = 82; // degrees - public static final double MIN_ANGLE = 58.5; // degrees - - public static final double START_ANGLE = 90-22.9; // degrees + public static final double MIN_ANGLE = 58.5; // degrees // Arena dimensions public static final double TARGET_HEIGHT = 2.44; // meters @@ -28,10 +25,7 @@ public class HoodConstants { public static final Translation2d TRANSLATION_TARGET = new Translation2d(0, 0); public static final Rotation2d ROTATION_TARGET_ANGLE = new Rotation2d(); // Other - public static final double INITIAL_VELOCTIY = 14.9; // meters per second - - // Testing purposes - public static final double START_DISTANCE = 2; // meters + public static final double INITIAL_VELOCITY = 14.9; // meters per second // Calibration Purposes public static final double CURRENT_SPIKE_THRESHHOLD = 10.0; // amps diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/hood/HoodIO.java index 42886b5..12632b8 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodIO.java @@ -4,7 +4,7 @@ import org.littletonrobotics.junction.AutoLog; public interface HoodIO { @AutoLog - public static class HoodIOInputs{ + public static class HoodIOInputs { public double positionDeg = HoodConstants.MAX_ANGLE; public double velocityRadPerSec = 0.0; public double motorCurrent = 0.0; -- 2.39.5