diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 763c6697..ae66163a 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -1,15 +1,17 @@ package frc.robot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; +import java.util.function.BooleanSupplier; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -36,20 +38,22 @@ import frc.robot.util.BranchHeight; import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; -import java.util.function.BooleanSupplier; +import frc.robot.util.SoloScoringMode; public class Controls { - private static final int DRIVER_CONTROLLER_PORT = 0; - private static final int OPERATOR_CONTROLLER_PORT = 1; - private static final int ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT = 2; - private static final int ELEVATOR_CONTROLLER_PORT = 3; - private static final int CLIMB_TEST_CONTROLLER_PORT = 4; + private static final int SOLO_CONTROLLER_PORT = 0; + private static final int DRIVER_CONTROLLER_PORT = 1; + private static final int OPERATOR_CONTROLLER_PORT = 2; + private static final int ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT = 3; + private static final int ELEVATOR_CONTROLLER_PORT = 4; + private static final int CLIMB_TEST_CONTROLLER_PORT = 5; private final CommandXboxController driverController; private final CommandXboxController operatorController; private final CommandXboxController armPivotSpinnyClawController; private final CommandXboxController elevatorTestController; private final CommandXboxController climbTestController; + private final CommandXboxController soloController; private final Subsystems s; private final Sensors sensors; @@ -57,6 +61,8 @@ public class Controls { private BranchHeight branchHeight = BranchHeight.CORAL_LEVEL_FOUR; private ScoringMode scoringMode = ScoringMode.CORAL; + private ScoringMode intakeMode = ScoringMode.CORAL; + private SoloScoringMode soloScoringMode = SoloScoringMode.NO_GAME_PIECE; private AlgaeIntakeHeight algaeIntakeHeight = AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR; // Swerve stuff @@ -90,6 +96,7 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { armPivotSpinnyClawController = new CommandXboxController(ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT); elevatorTestController = new CommandXboxController(ELEVATOR_CONTROLLER_PORT); climbTestController = new CommandXboxController(CLIMB_TEST_CONTROLLER_PORT); + soloController = new CommandXboxController(SOLO_CONTROLLER_PORT); this.s = s; this.sensors = sensors; this.superStructure = superStructure; @@ -104,13 +111,14 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { configureAutoAlignBindings(); configureGroundSpinnyBindings(); configureGroundArmBindings(); + configureSoloControllerBindings(); } private Trigger connected(CommandXboxController controller) { return new Trigger(() -> controller.isConnected()); } - // takes the X value from the joystic, and applies a deadband and input scaling + // takes the X value from the joystick, and applies a deadband and input scaling private double getDriveX() { // Joystick +Y is back // Robot +X is forward @@ -119,7 +127,7 @@ private double getDriveX() { return input * MaxSpeed * inputScale; } - // takes the Y value from the joystic, and applies a deadband and input scaling + // takes the Y value from the joystick, and applies a deadband and input scaling private double getDriveY() { // Joystick +X is right // Robot +Y is left @@ -128,7 +136,7 @@ private double getDriveY() { return input * MaxSpeed * inputScale; } - // takes the rotation value from the joystic, and applies a deadband and input scaling + // takes the rotation value from the joystick, and applies a deadband and input scaling private double getDriveRotate() { // Joystick +X is right // Robot +angle is CCW (left) @@ -143,7 +151,8 @@ private void configureDrivebaseBindings() { // Stop running this method return; } - + // if the solo controller is connected, use it for driving + boolean soloCC = soloController.isConnected(); // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. @@ -156,9 +165,9 @@ private void configureDrivebaseBindings() { .applyRequest( () -> drive - .withVelocityX(getDriveX()) - .withVelocityY(getDriveY()) - .withRotationalRate(getDriveRotate())) + .withVelocityX(soloCC ? getSoloDriveX() : getDriveX()) + .withVelocityY(soloCC ? getSoloDriveY() : getDriveY()) + .withRotationalRate(soloCC ? getSoloDriveRotate() : getDriveRotate())) .withName("Drive")); // various former controls that were previously used and could be referenced in the future @@ -370,6 +379,25 @@ private void configureSuperStructureBindings() { .asProxy() : Commands.none()) .withName("Automatic Intake")); + sensors + .armSensor + .inClaw() + .and(RobotModeTriggers.teleop()) + .onTrue( + Commands.runOnce( + () -> { + switch (intakeMode) { + case CORAL -> soloScoringMode = SoloScoringMode.CORAL_IN_CLAW; + case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW; + } + }) + .withName("Set solo scoring mode")) + .onFalse( + Commands.runOnce( + () -> { + soloScoringMode = SoloScoringMode.NO_GAME_PIECE; + }) + .withName("Clear solo scoring mode")); } if (s.climbPivotSubsystem.sensor != null) { @@ -421,6 +449,23 @@ private Command getCoralBranchHeightCommand() { }; } + private Command getSoloCoralBranchHeightCommand() { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure + .coralLevelFour(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_THREE -> superStructure + .coralLevelThree(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + }; + } + private void configureElevatorBindings() { if (s.elevatorSubsystem == null) { return; @@ -677,7 +722,7 @@ private void configureElevatorLEDBindings() { } s.elevatorLEDSubsystem.setDefaultCommand( - s.elevatorLEDSubsystem.showScoringMode(() -> scoringMode)); + s.elevatorLEDSubsystem.showScoringMode(() -> soloScoringMode)); if (s.elevatorSubsystem != null) { Trigger hasBeenZeroed = new Trigger(s.elevatorSubsystem::getHasBeenZeroed); @@ -791,7 +836,7 @@ private Command selectScoringHeight(BranchHeight b, AlgaeIntakeHeight a) { () -> { branchHeight = b; algaeIntakeHeight = a; - if (scoringMode == ScoringMode.ALGAE + if (intakeMode == ScoringMode.ALGAE && (sensors.armSensor == null || !sensors.armSensor.booleanInClaw())) { CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand()); } @@ -830,4 +875,239 @@ public void vibrateCoDriveController(double vibration) { operatorController.getHID().setRumble(RumbleType.kBothRumble, vibration); } } + + private double getSoloJoystickInput(double input) { + if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { + return 0; // stop driving if either stick is pressed + } + // Apply a deadband to the joystick input + double deadbandedInput = MathUtil.applyDeadband(input, 0.1); + return deadbandedInput; + } + + // Drive for Solo controller + // takes the X value from the joystick, and applies a deadband and input scaling + private double getSoloDriveX() { + // Joystick +Y is back + // Robot +X is forward + return getSoloJoystickInput(-soloController.getLeftY()) * MaxSpeed; + } + + // takes the Y value from the joystick, and applies a deadband and input scaling + private double getSoloDriveY() { + // Joystick +X is right + // Robot +Y is left + return getSoloJoystickInput(-soloController.getLeftX()) * MaxSpeed; + } + + // takes the rotation value from the joystick, and applies a deadband and input scaling + private double getSoloDriveRotate() { + // Joystick +X is right + // Robot +angle is CCW (left) + return getSoloJoystickInput(-soloController.getRightX()) * MaxSpeed; + } + + private void configureSoloControllerBindings() { + // Barge + Auto align left + Select scoring mode Algae + soloController + .leftTrigger() + .onTrue( + Commands.runOnce( + () -> { + Command scoreCommand; + switch (soloScoringMode) { + case CORAL_IN_CLAW -> { + scoreCommand = getSoloCoralBranchHeightCommand(); + } + + case ALGAE_IN_CLAW -> { + Command bargeScoreCommand = + BargeAlign.bargeScore( + s.drivebaseSubsystem, + superStructure, + () -> getSoloDriveX(), + () -> getSoloDriveY(), + () -> getSoloDriveRotate(), + soloController.rightBumper()); + + if (sensors.intakeSensor.booleanInIntake()) { + Command holdGroundIntakeOut = + superStructure.holdGroundIntakeOut(soloController.povUp()); + + Command quickHandoff = + Commands.sequence( + superStructure + .quickGroundIntake(soloController.povUp()) + .withName("Quick Gound intake"), + Commands.runOnce( + () -> soloScoringMode = soloScoringMode.CORAL_IN_CLAW)) + .withName("Quick Handoff After Algae Score"); + + scoreCommand = + bargeScoreCommand + .alongWith(holdGroundIntakeOut) + .andThen(quickHandoff) + .withName("Algae Score + Ground Handoff"); + } else { + scoreCommand = + Commands.sequence( + bargeScoreCommand, + Commands.runOnce( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) + .withName("Barge Algae Score"); + } + } + + case NO_GAME_PIECE -> { + scoreCommand = + Commands.parallel( + Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) + .alongWith(scoringModeSelectRumble()) + .withName("Algae Scoring Mode"), + Commands.runOnce( + () -> + CommandScheduler.getInstance() + .schedule(getAlgaeIntakeCommand())) + .withName("Run Algae Intake")); + } + + default -> { + scoreCommand = Commands.none(); + } + } + + CommandScheduler.getInstance().schedule(scoreCommand); + }) + .withName("Solo Left Trigger Action")); + + soloController + .leftTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); + // Processor + Auto align right + Select scoring mode Coral + soloController + .rightTrigger() + .onTrue( + Commands.runOnce( + () -> { + Command scoreCommand = + switch (soloScoringMode) { + case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand(); + case ALGAE_IN_CLAW -> Commands.sequence( + superStructure.algaeProcessorScore( + soloController.rightBumper()), + Commands.runOnce( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) + .withName("Processor score"); + case NO_GAME_PIECE -> Commands.parallel( + Commands.runOnce(() -> intakeMode = ScoringMode.CORAL) + .alongWith(scoringModeSelectRumble()) + .withName("Coral Scoring Mode"), + superStructure.coralPreIntake(), + s.climbPivotSubsystem.toStow()); + }; + CommandScheduler.getInstance().schedule(scoreCommand); + }) + .withName("score")); + soloController + .rightTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + // Ground Intake + soloController + .leftBumper() + .onTrue( + Commands.runOnce( + () -> { + Command intakeCommand; + switch (soloScoringMode) { + case CORAL_IN_CLAW, NO_GAME_PIECE -> intakeCommand = + superStructure + .quickGroundIntake(soloController.povUp()) + .withName("Quick Ground Intake") + .alongWith(Commands.runOnce(() -> intakeMode = ScoringMode.CORAL)); + case ALGAE_IN_CLAW -> intakeCommand = + superStructure + .holdGroundIntakeOut(soloController.povUp()) + .withName("Hold Ground Intake Out"); + default -> intakeCommand = Commands.none(); + } + CommandScheduler.getInstance().schedule(intakeCommand); + })); + // Scoring levels coral and algae intake heights + soloController + .y() + .onTrue( + selectScoringHeight( + BranchHeight.CORAL_LEVEL_FOUR, AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR) + .withName("coral level 4, algae level 3-4")); + soloController + .x() + .onTrue( + selectScoringHeight( + BranchHeight.CORAL_LEVEL_THREE, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) + .withName("coral level 3, algae level 2-3")); + soloController + .b() + .onTrue( + selectScoringHeight( + BranchHeight.CORAL_LEVEL_TWO, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) + .withName("coral level 2, algae level 2-3")); + soloController + .a() + .onTrue( + selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND) + .withName("coral level 1, algae ground level")); + // Zero Elevator + soloController + .back() + .onTrue( + Commands.parallel( + s.elevatorSubsystem.resetPosZero(), + rumble(soloController, 0.5, Seconds.of(0.3))) + .ignoringDisable(true) + .withName("Reset elevator zero")); + // Reset gyro + soloController + .start() + .onTrue( + s.drivebaseSubsystem + .runOnce(() -> s.drivebaseSubsystem.seedFieldCentric()) + .alongWith(rumble(soloController, 0.5, Seconds.of(0.3))) + .withName("Reset gyro")); + // Funnel Out + soloController.povLeft().onTrue(s.climbPivotSubsystem.toClimbOut()); + // Funnel Climbed + soloController.povRight().onTrue(s.climbPivotSubsystem.toClimbed()); + // Intake button + soloController + .povDown() + .onTrue( + superStructure + .coralIntake() + .alongWith( + s.elevatorLEDSubsystem != null + ? s.elevatorLEDSubsystem + .tripleBlink(255, 92, 0, "Orange - Manual Coral Intake") + .asProxy() + : Commands.none()) + .withName("Manual Coral Intake")); + // Arm manual + soloController + .rightStick() + .whileTrue( + s.armPivotSubsystem + .startMovingVoltage(() -> Volts.of(3 * soloController.getRightY())) + .withName("Arm Manual Control")); + // Elevator manual + soloController + .leftStick() + .whileTrue( + s.elevatorSubsystem + .startMovingVoltage( + () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY())) + .withName("Elevator Manual Control")); + } } diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index 978ac85d..f55ad969 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -55,7 +55,7 @@ public class CompTunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private static final Current kSlipCurrent = Amps.of(80.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. diff --git a/src/main/java/frc/robot/sensors/ElevatorLight.java b/src/main/java/frc/robot/sensors/ElevatorLight.java index bba94b64..f6139f74 100644 --- a/src/main/java/frc/robot/sensors/ElevatorLight.java +++ b/src/main/java/frc/robot/sensors/ElevatorLight.java @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Hardware; -import frc.robot.util.ScoringMode; +import frc.robot.util.SoloScoringMode; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -82,13 +82,15 @@ public Command animate(LEDPattern animation, String name) { .withName("Animate" + name); } - public Command showScoringMode(Supplier scoringMode) { + public Command showScoringMode(Supplier scoringMode) { return run(() -> { - ScoringMode currentMode = scoringMode.get(); - if (currentMode == ScoringMode.ALGAE) { + SoloScoringMode currentMode = scoringMode.get(); + if (currentMode == SoloScoringMode.ALGAE_IN_CLAW) { updateLEDs(LEDPattern.solid(Color.kTeal)); - } else { + } else if (currentMode == SoloScoringMode.CORAL_IN_CLAW) { updateLEDs(LEDPattern.solid(Color.kWhite)); + } else if (currentMode == SoloScoringMode.NO_GAME_PIECE) { + updateLEDs(LEDPattern.solid(Color.kDeepPink)); } }) .withName("Animate Scoring Mode"); diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 314c6035..f528043c 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -15,8 +15,8 @@ public class IntakeSensor { private final LaserCan intakeSensor; // VALUES ARE IN METERS - private static final double INTAKE_LOWER_LIMIT = 0.010; - private static final double INTAKE_UPPER_LIMIT = 0.1; + private static final double INTAKE_LOWER_LIMIT = 0.005; + private static final double INTAKE_UPPER_LIMIT = 0.05; public IntakeSensor() { intakeSensor = new LaserCan(Hardware.GROUND_INTAKE_SENSOR); @@ -29,7 +29,7 @@ public IntakeSensor() { private void ConfigureSensor(LaserCan Sensor) { try { Sensor.setRangingMode(LaserCan.RangingMode.SHORT); - Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(4, 4, 16, 16)); Sensor.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); } catch (ConfigurationFailedException e) { System.out.println("Configuration Failed! " + e); @@ -59,4 +59,9 @@ public Trigger inIntake() { }) .debounce(0.1); } + + public boolean booleanInIntake() { + double distance = getSensorDistance().in(Meters); + return distance > INTAKE_LOWER_LIMIT && distance < INTAKE_UPPER_LIMIT; + } } diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index 1b08d0b9..880a9f82 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -195,9 +195,9 @@ public void factoryDefaults() { talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; // enabling current limits - talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 80; // starting low for testing + talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 80; talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 40; // starting low for testing + talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 20; talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; // PID diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java index f4f82c82..80a4919d 100644 --- a/src/main/java/frc/robot/subsystems/ClimbPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimbPivot.java @@ -56,7 +56,7 @@ public enum TargetPositions { private final double CLIMB_HOLD_CLIMBED = -0.0705; private final double CLIMB_IN_SPEED = -0.75; - private final double climbInKp = 30; + private final double climbInKp = 50; private final double climbOutKp = 50; // positions for relation between motor encoder and WCP encoder diff --git a/src/main/java/frc/robot/subsystems/GroundSpinny.java b/src/main/java/frc/robot/subsystems/GroundSpinny.java index 93ce77e4..96182264 100644 --- a/src/main/java/frc/robot/subsystems/GroundSpinny.java +++ b/src/main/java/frc/robot/subsystems/GroundSpinny.java @@ -16,7 +16,7 @@ public class GroundSpinny extends SubsystemBase { public static final double GROUND_INTAKE_JITTER_SPEED = 1; public static final double FUNNEL_INTAKE_SPEED = -2; public static final double QUICK_HANDOFF_EXTAKE_SPEED = 1; - private static final double STATOR_CURRENT_STALL_THRESHOLD = 20; + private static final double STATOR_CURRENT_STALL_THRESHOLD = 50; // TalonFX private final TalonFX motor; diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d37b680b..baf47737 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -203,6 +203,21 @@ public Command groundIntake(BooleanSupplier retract) { } } + public Command holdGroundIntakeOut(BooleanSupplier retract) { + if (groundSpinny == null || groundArm == null) { + return Commands.none().withName("hold ground intake disabled"); + } + + return Commands.sequence( + Commands.parallel( + groundArm + .moveToPosition(GroundArm.GROUND_POSITION) + .andThen(groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE)) + .until(retract), + groundSpinny.setGroundIntakePower())) + .withName("Hold Ground Intake Out"); + } + // This is the actual version in use. It moves the coral directly into the claw. public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph if (groundSpinny == null || groundArm == null || intakeSensor == null) { diff --git a/src/main/java/frc/robot/util/SoloScoringMode.java b/src/main/java/frc/robot/util/SoloScoringMode.java new file mode 100644 index 00000000..529fc716 --- /dev/null +++ b/src/main/java/frc/robot/util/SoloScoringMode.java @@ -0,0 +1,7 @@ +package frc.robot.util; + +public enum SoloScoringMode { + CORAL_IN_CLAW, + ALGAE_IN_CLAW, + NO_GAME_PIECE; +}