From 6a88c391a9b573b745531ea2ea4b81c814c9a661 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Thu, 6 Feb 2025 19:45:33 -0800 Subject: [PATCH] actual testable mode --- src/main/java/frc/robot/RobotContainer.java | 305 +++++++++--------- .../subsystems/swerve/SwerveSubsystem.java | 64 ++-- 2 files changed, 185 insertions(+), 184 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 133891f2..2ceff059 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,11 +12,11 @@ import com.choreo.lib.ChoreoTrajectory; import com.fasterxml.jackson.databind.util.Named; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.PathPlannerLogging; +// import com.pathplanner.lib.auto.AutoBuilder; +// import com.pathplanner.lib.auto.NamedCommands; +// import com.pathplanner.lib.commands.PathPlannerAuto; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.cscore.MjpegServer; import edu.wpi.first.cscore.UsbCamera; @@ -101,15 +101,15 @@ public class RobotContainer { private final BaseDriveController driveController; private final SwerveSubsystem swerveSubsystem; - // private final IntakePivotSubsystem intakePivotSubsystem; - // private final IntakeRollerSubsystem intakeRollerSubsystem; + private final IntakePivotSubsystem intakePivotSubsystem; + private final IntakeRollerSubsystem intakeRollerSubsystem; - // private final ShooterFlywheelSubsystem shooterFlywheelSubsystem; - // private final ShooterPivotSubsystem shooterPivotSubsystem; + private final ShooterFlywheelSubsystem shooterFlywheelSubsystem; + private final ShooterPivotSubsystem shooterPivotSubsystem; // private final ClimbSubsystem climbSubsystem; - // private final ElevatorSubsystem elevatorSubsystem; + private final ElevatorSubsystem elevatorSubsystem; private final FieldManagementSubsystem fmsSubsystem; private final LightBarSubsystem lightBarSubsystem; @@ -185,7 +185,7 @@ public RobotContainer() { listEntry = autonTable.getEntry("AutonList"); selectedAutonEntry = autonTable.getEntry("Selected"); // System.out.print("Available Autons: " + AutoBuilder.getAllAutoNames().toArray(new String[0])); - listEntry.setStringArray(AutoBuilder.getAllAutoNames().toArray(new String[0])); + // listEntry.setStringArray(AutoBuilder.getAllAutoNames().toArray(new String[0])); selectedAutonEntry.setString(autonValue); autonTable.addListener("Auton", EnumSet.of(NetworkTableEvent.Kind.kValueAll), (table, key, event) -> { this.autonValue = event.valueData.value.getString(); @@ -199,19 +199,19 @@ public RobotContainer() { swerveSubsystem = new SwerveSubsystem(fmsSubsystem::isRedAlliance); swerveSubsystem.setVerbose(false); // SET THIS TO true FOR TUNING VALUES - // intakePivotSubsystem = new IntakePivotSubsystem(); - // intakeRollerSubsystem = new IntakeRollerSubsystem(lightBarSubsystem); + intakePivotSubsystem = new IntakePivotSubsystem(); + intakeRollerSubsystem = new IntakeRollerSubsystem(lightBarSubsystem); - // shooterPivotSubsystem = new ShooterPivotSubsystem( - // swerveSubsystem::getShootingDistance, - // fmsSubsystem::isRedAlliance - // ); - // shooterFlywheelSubsystem = new ShooterFlywheelSubsystem( - // swerveSubsystem::getShootingDistance, - // fmsSubsystem::isRedAlliance - // ); + shooterPivotSubsystem = new ShooterPivotSubsystem( + swerveSubsystem::getShootingDistance, + fmsSubsystem::isRedAlliance + ); + shooterFlywheelSubsystem = new ShooterFlywheelSubsystem( + swerveSubsystem::getShootingDistance, + fmsSubsystem::isRedAlliance + ); - // elevatorSubsystem = new ElevatorSubsystem(); + elevatorSubsystem = new ElevatorSubsystem(); // climbSubsystem = new ClimbSubsystem(); @@ -342,7 +342,7 @@ private void configureBindings() { xError.setValue(xPID.getPositionError()); yError.setValue(yPID.getPositionError()); - }, swerveSubsystem));} + }, swerveSubsystem)); // /* Pressing the button resets the field axes to the current robot axes. */ // driveController.getDriverHeadingResetButton().onTrue(new InstantCommand(() -> { @@ -490,8 +490,8 @@ private void configureBindings() { // elevatorToZero.onTrue(new ElevatorToLimitSwitchCommand(elevatorSubsystem)); /* INTAKE TEST */ - // xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3), - // intakePivotSubsystem)); + xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3), + intakePivotSubsystem)); // rightBumper.onTrue(new InstantCommand(() -> // intakePivotSubsystem.setPosition(0), intakePivotSubsystem)); @@ -562,138 +562,139 @@ private void configureBindings() { // ); - // // roll behaviors - // leftBumper.onTrue( - // new ConditionalCommand( - // new InstantCommand(), - // new ConditionalCommand( - // new InstantCommand(), - // new WaitCommand(.05).andThen( - // new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot - // new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor - // .until(intakeRollerSubsystem::getFrontSensorReached) - // ), - // intakeRollerSubsystem::getAmpSensor - // ), // raise the elevator - // () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos - // || elevatorSubsystem.getTargetState() == ElevatorState.TRAP) - // ); + // roll behaviors + leftBumper.onTrue( + new ConditionalCommand( + new InstantCommand(), + new ConditionalCommand( + new InstantCommand(), + new WaitCommand(.05).andThen( + new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot + new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor + .until(intakeRollerSubsystem::getFrontSensorReached) + ), + intakeRollerSubsystem::getAmpSensor + ), // raise the elevator + () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos + || elevatorSubsystem.getTargetState() == ElevatorState.TRAP) + ); + + + + // aButton runs the intake sequence + aButton.onTrue( + Commands.runOnce(() -> intakePivotSubsystem.setPosition(1), intakePivotSubsystem).alongWith( + new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)).andThen( + new ConditionalCommand( + new WaitCommand(.3).andThen( + new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem).andThen( + new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem) + ) + ), + + new InstantCommand( + () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem + ), + aButton + ) + ).until(() -> mechController.getLeftTriggerAxis() > .1) // cancel if try to outtake + + ); + + // bButton stops the rollers + bButton.onTrue(Commands.idle(intakeRollerSubsystem).withTimeout(0)); + + // xButton toggles the intake being stowed + xButton.onTrue(new InstantCommand(() -> { + double outPosition = 1; + double stowPosition = 0; + if (elevatorSubsystem.getExtensionPercent() > .5 + && elevatorSubsystem.getTargetState() == ElevatorState.TRAP) { + outPosition = .45; // push intake out + } else if (elevatorSubsystem.getExtensionPercent() > .5 + && elevatorSubsystem.getTargetState() == ElevatorState.AMP) { + stowPosition = .2; + } + intakePivotSubsystem.setPosition( + intakePivotSubsystem.getEncoderPosition() < (outPosition + stowPosition) / 2 + ? outPosition + : stowPosition + ); + }, intakePivotSubsystem)); - // // aButton runs the intake sequence - // aButton.onTrue( - // Commands.runOnce(() -> intakePivotSubsystem.setPosition(1), intakePivotSubsystem).alongWith( - // new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)).andThen( - // new ConditionalCommand( - // new WaitCommand(.3).andThen( - // new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem).andThen( - // new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem) - // ) - // ), - - // new InstantCommand( - // () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem - // ), - // aButton - // ) - // ).until(() -> mechController.getLeftTriggerAxis() > .1) // cancel if try to outtake - - // ); + // yButton runs the flywheels - // // bButton stops the rollers - // bButton.onTrue(Commands.idle(intakeRollerSubsystem).withTimeout(0)); - - // // xButton toggles the intake being stowed - // xButton.onTrue(new InstantCommand(() -> { - // double outPosition = 1; - // double stowPosition = 0; - // if (elevatorSubsystem.getExtensionPercent() > .5 - // && elevatorSubsystem.getTargetState() == ElevatorState.TRAP) { - // outPosition = .45; // push intake out - // } else if (elevatorSubsystem.getExtensionPercent() > .5 - // && elevatorSubsystem.getTargetState() == ElevatorState.AMP) { - // stowPosition = .2; - // } - - // intakePivotSubsystem.setPosition( - // intakePivotSubsystem.getEncoderPosition() < (outPosition + stowPosition) / 2 - // ? outPosition - // : stowPosition - // ); - - // }, intakePivotSubsystem)); - - // // yButton runs the flywheels - - // yButton.onTrue( - // new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen( - // new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem), - // new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition) - // ).unless(intakeRollerSubsystem::getRockwellSensorValue).andThen( - // new IntakePivotSetPositionCommand(intakePivotSubsystem, 0) - // ) - // ); + yButton.onTrue( + new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen( + new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem), + new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition) + ).unless(intakeRollerSubsystem::getRockwellSensorValue).andThen( + new IntakePivotSetPositionCommand(intakePivotSubsystem, 0) + ) + ); + - // shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> { - // if (yButton.getAsBoolean()) { - // lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2); - // // shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning + shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> { + if (yButton.getAsBoolean()) { + lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2); + // shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning - // shooterFlywheelSubsystem.setShooterMotorSpeed(); + shooterFlywheelSubsystem.setShooterMotorSpeed(); - // shooterPivotSubsystem.setAutoAimBoolean(true); - // if (shooterFlywheelSubsystem.atSpeed()) { - // mechController.setRumble(RumbleType.kBothRumble, .4); - // } else { - // mechController.setRumble(RumbleType.kBothRumble, 0); - // if (lightBarSubsystem.getLightBarMechStatus() == LightBarStatus.SHOOTER_SPIN_UP) { - // double top = shooterFlywheelSubsystem.getTopSpeed() - // / shooterFlywheelSubsystem.getTargetTopRPS(); - // double bottom = shooterFlywheelSubsystem.getBottomSpeed() - // / shooterFlywheelSubsystem.getTargetBottomRPS(); - // double avg = (top + bottom) / 2; // in case they're different, this just shows the average. - - // lightBarSubsystem.updateShooterSpeedPercentage(avg); - // } + shooterPivotSubsystem.setAutoAimBoolean(true); + if (shooterFlywheelSubsystem.atSpeed()) { + mechController.setRumble(RumbleType.kBothRumble, .4); + } else { + mechController.setRumble(RumbleType.kBothRumble, 0); + if (lightBarSubsystem.getLightBarMechStatus() == LightBarStatus.SHOOTER_SPIN_UP) { + double top = shooterFlywheelSubsystem.getTopSpeed() + / shooterFlywheelSubsystem.getTargetTopRPS(); + double bottom = shooterFlywheelSubsystem.getBottomSpeed() + / shooterFlywheelSubsystem.getTargetBottomRPS(); + double avg = (top + bottom) / 2; // in case they're different, this just shows the average. + + lightBarSubsystem.updateShooterSpeedPercentage(avg); + } - // } - // } else { - // // if(fmsSubsystem.getMatchStatus() != MatchStatus.AUTON){ - // // shooterPivotSubsystem.setAutoAimBoolean(false); - // // } - // if (mechController.getPOV() == 90) { - - // if (shooterFlywheelSubsystem.atSpeed()) { - // mechController.setRumble(RumbleType.kBothRumble, .4); - // } else { - // mechController.setRumble(RumbleType.kBothRumble, 0); - // } - // } else { - // shooterFlywheelSubsystem.stopShooter(); - // mechController.setRumble(RumbleType.kBothRumble, 0); - // } - // } - // if (shooterFlywheelSubsystem.atSpeed()) { - // mechController.setRumble(RumbleType.kBothRumble, .4); - // } else { - // mechController.setRumble(RumbleType.kBothRumble, 0); - // } - - // if (noteInBack - // && !intakeRollerSubsystem.getRockwellSensorValue() - // && intakeRollerSubsystem.getIntegrationSpeed() > 0 - // ) { - // System.out.println("Dist: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getShootingDistance()) - // + " Angle: " + GRTUtil.twoDecimals(Units.radiansToDegrees(shooterPivotSubsystem.getPosition())) - // + " Speed: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getSplineSpeed())); - // } - // noteInBack = intakeRollerSubsystem.getRockwellSensorValue(); - - // // if we are at speed, rumble the mech controller + } + } else { + // if(fmsSubsystem.getMatchStatus() != MatchStatus.AUTON){ + // shooterPivotSubsystem.setAutoAimBoolean(false); + // } + if (mechController.getPOV() == 90) { + + if (shooterFlywheelSubsystem.atSpeed()) { + mechController.setRumble(RumbleType.kBothRumble, .4); + } else { + mechController.setRumble(RumbleType.kBothRumble, 0); + } + } else { + shooterFlywheelSubsystem.stopShooter(); + mechController.setRumble(RumbleType.kBothRumble, 0); + } + } + if (shooterFlywheelSubsystem.atSpeed()) { + mechController.setRumble(RumbleType.kBothRumble, .4); + } else { + mechController.setRumble(RumbleType.kBothRumble, 0); + } + + if (noteInBack + && !intakeRollerSubsystem.getRockwellSensorValue() + && intakeRollerSubsystem.getIntegrationSpeed() > 0 + ) { + System.out.println("Dist: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getShootingDistance()) + + " Angle: " + GRTUtil.twoDecimals(Units.radiansToDegrees(shooterPivotSubsystem.getPosition())) + + " Speed: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getSplineSpeed())); + } + noteInBack = intakeRollerSubsystem.getRockwellSensorValue(); + + // if we are at speed, rumble the mech controller - // }, shooterFlywheelSubsystem - // )); + }, shooterFlywheelSubsystem + )); // dPadRight.onTrue(new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen( // new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem), @@ -712,12 +713,12 @@ private void configureBindings() { // // }, intakePivotSubsystem)); // // The triggers intake/outtake the rollers - // intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> { + intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> { - // double power = .7 * (mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()); + double power = .7 * (mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()); - // intakeRollerSubsystem.setRollSpeeds(power, power); - // }, intakeRollerSubsystem)); + intakeRollerSubsystem.setRollSpeeds(power, power); + }, intakeRollerSubsystem)); } // // Offset buttons to correct the shooter if needed // offsetUpButton.onTrue(new InstantCommand( diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 2e2ed208..2bb025d0 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -26,10 +26,10 @@ import static frc.robot.Constants.VisionConstants.FRONT_RIGHT_CAMERA_POSE; import com.kauailabs.navx.frc.AHRS; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; +// import com.pathplanner.lib.auto.AutoBuilder; +// import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +// import com.pathplanner.lib.util.PIDConstants; +// import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Nat; @@ -65,7 +65,7 @@ import java.util.Optional; import java.util.function.BooleanSupplier; -import org.photonvision.EstimatedRobotPose; +// import org.photonvision.EstimatedRobotPose; /** The subsystem that controls the swerve drivetrain. */ public class SwerveSubsystem extends SubsystemBase { @@ -187,21 +187,21 @@ public SwerveSubsystem(BooleanSupplier redSupplier) { ); // Configure AutoBuilder - AutoBuilder.configureHolonomic( - this::getRobotPosition, - this::resetPose, - this::getRobotRelativeChassisSpeeds, - this::setRobotRelativeDrivePowers, - new HolonomicPathFollowerConfig( - new PIDConstants(3.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(3.0, 0.0, 0.0), // Rotation PID constants - 4.5, // Max module speed, in m/s - FL_POS.getNorm(), // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig(true, true) - ), - redSupplier, - this - ); + // AutoBuilder.configureHolonomic( + // this::getRobotPosition, + // this::resetPose, + // this::getRobotRelativeChassisSpeeds, + // this::setRobotRelativeDrivePowers, + // new HolonomicPathFollowerConfig( + // new PIDConstants(3.0, 0.0, 0.0), // Translation PID constants + // new PIDConstants(3.0, 0.0, 0.0), // Rotation PID constants + // 4.5, // Max module speed, in m/s + // FL_POS.getNorm(), // Drive base radius in meters. Distance from robot center to furthest module. + // new ReplanningConfig(true, true) + // ), + // redSupplier, + // this + // ); velocityTimer.start(); } @@ -211,18 +211,18 @@ public void periodic() { robotPosEntry.setValue(GRTUtil.twoDecimals(getRobotPosition().getX())); - for (ApriltagWrapper apriltagWrapper : apriltagWrappers) { - Optional visionEstimate = apriltagWrapper.getRobotPose( - new Pose3d(getRobotPosition()) - ); - - if (visionEstimate.isPresent()) { - poseEstimator.addVisionMeasurement( - visionEstimate.get().estimatedPose.toPose2d(), - visionEstimate.get().timestampSeconds - ); - } - } + // for (ApriltagWrapper apriltagWrapper : apriltagWrappers) { + // Optional visionEstimate = apriltagWrapper.getRobotPose( + // new Pose3d(getRobotPosition()) + // ); + + // if (visionEstimate.isPresent()) { + // poseEstimator.addVisionMeasurement( + // visionEstimate.get().estimatedPose.toPose2d(), + // visionEstimate.get().timestampSeconds + // ); + // } + // } Rotation2d gyroAngle = getGyroHeading(); Pose2d estimate = poseEstimator.update(