diff --git a/.vscode/settings.json b/.vscode/settings.json index 2cc7f2e5..a992d31a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,6 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "editor.tabSize": 2 + "editor.tabSize": 2, + "wpilib.autoStartRioLog": false } diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index b7097488..9eb88b68 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -272,6 +272,8 @@ private void configureSuperStructureBindings() { .onTrue( selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND) .withName("coral level 1, algae ground level")); + + // Processor scoring and left coral pre-score driverController .leftTrigger() .onTrue( @@ -283,11 +285,24 @@ private void configureSuperStructureBindings() { superStructure.algaeProcessorScore( driverController.rightBumper()), Commands.waitSeconds(0.7), - getAlgaeIntakeCommand()) + Commands.deferredProxy( + () -> + switch (sensors.intakeSensor.inIntakeSwitchable()) { + case 1 -> Commands.sequence( + Commands.runOnce( + () -> scoringMode = ScoringMode.CORAL) + .alongWith(scoringModeSelectRumble()) + .withName("Coral Scoring Mode"), + superStructure + .superCycleCoralHandoff() + .withName("supercycle handoff")); + default -> getAlgaeIntakeCommand(); + })) .withName("Processor score"); }) .withName("Schedule processor score")); + // Algae mode operatorController .leftBumper() .onTrue( @@ -297,6 +312,8 @@ private void configureSuperStructureBindings() { .onTrue( Commands.runOnce(() -> CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand())) .withName("run algae intake")); + + // Coral Mode operatorController // should work??? .leftTrigger() .onTrue( @@ -304,7 +321,13 @@ private void configureSuperStructureBindings() { .alongWith(scoringModeSelectRumble()) .withName("Coral Scoring Mode")) .onTrue(superStructure.coralPreIntake()) - .onTrue(s.climbPivotSubsystem.toStow()); + .onTrue(s.climbPivotSubsystem.toStow()) + .onTrue( + s.groundArm + .moveToPosition(GroundArm.STOWED_POSITION) + .withName("Ground stowed position")); + + // Stow Mode operatorController .povLeft() .onTrue( @@ -371,6 +394,8 @@ private void configureSuperStructureBindings() { : Commands.none()) .withName("Automatic Intake")); } + + // Net scoring and right coral pre-score driverController .rightTrigger() .onTrue( @@ -387,7 +412,21 @@ private void configureSuperStructureBindings() { () -> getDriveY(), () -> getDriveRotate(), driverController.rightBumper()), - getAlgaeIntakeCommand()) + // If we are supercycling (have a coral), hand it off and switch + // to coral mode + Commands.deferredProxy( + () -> + switch (sensors.intakeSensor.inIntakeSwitchable()) { + case 1 -> Commands.sequence( + Commands.runOnce( + () -> scoringMode = ScoringMode.CORAL) + .alongWith(scoringModeSelectRumble()) + .withName("Coral Scoring Mode"), + superStructure + .superCycleCoralHandoff() + .withName("supercycle handoff")); + default -> getAlgaeIntakeCommand(); + })) .withName("Algae score then intake"); }; CommandScheduler.getInstance().schedule(scoreCommand); diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 314c6035..b21a604c 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -59,4 +59,9 @@ public Trigger inIntake() { }) .debounce(0.1); } + + public int inIntakeSwitchable() { + double distance = getSensorDistance().in(Meters); + return (distance > INTAKE_LOWER_LIMIT && distance < INTAKE_UPPER_LIMIT) ? 1 : 0; + } } diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java index 5b10c18d..01fce3e2 100644 --- a/src/main/java/frc/robot/subsystems/ClimbPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimbPivot.java @@ -56,6 +56,9 @@ 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 climbOutKp = 50; + // positions for relation between motor encoder and WCP encoder // relative to eachother, likely not accurately zero'ed when obtained. private static final double MIN_ROTOR_POSITION = -50.45; @@ -83,6 +86,8 @@ public enum TargetPositions { // the target speed private double setSpeed = 0; + private double speedToSet = 0; + private double pError = 0; // alerts for checking if either motor is or isnt connected private final Alert NotConnectedErrorOne = @@ -390,12 +395,15 @@ public Command advanceClimbCheck() { setSpeed = 0; } else { if (!moveComplete) { - if (minTargetPos > getClimbPosition()) { - motorLeft.set(CLIMB_OUT_SPEED); - setSpeed = CLIMB_OUT_SPEED; + pError = minTargetPos - getClimbPosition(); + if (pError > 0) { + speedToSet = CLIMB_OUT_SPEED * pError * climbOutKp; + motorLeft.set(speedToSet); + setSpeed = speedToSet; } else { - motorLeft.set(CLIMB_IN_SPEED); - setSpeed = CLIMB_IN_SPEED; + speedToSet = CLIMB_IN_SPEED * -pError * climbInKp; + motorLeft.set(speedToSet); + setSpeed = speedToSet; } } } diff --git a/src/main/java/frc/robot/subsystems/GroundArm.java b/src/main/java/frc/robot/subsystems/GroundArm.java index 1e69da24..a1eafbb1 100644 --- a/src/main/java/frc/robot/subsystems/GroundArm.java +++ b/src/main/java/frc/robot/subsystems/GroundArm.java @@ -26,8 +26,9 @@ public class GroundArm extends SubsystemBase { private final double ARMPIVOT_KG = 0.048; private final double ARMPIVOT_KA = 0; public static final double STOWED_POSITION = 0.45; - public static final double UP_POSITION = - 0.27; // untested - should be somewhere in between stowed and ground + public static final double UP_POSITION = 0.25; // Tested and working for supercycling + public static final double MIN_OUT_QUICK_HANDOFF = + 0.15; // Needs to be tested, used by supercycling public static final double GROUND_POSITION = -0.050; public static final double QUICK_INTAKE_POSITION = 0.31; public static final double POS_TOLERANCE = Units.degreesToRotations(5); diff --git a/src/main/java/frc/robot/subsystems/GroundSpinny.java b/src/main/java/frc/robot/subsystems/GroundSpinny.java index 93ce77e4..6f67f220 100644 --- a/src/main/java/frc/robot/subsystems/GroundSpinny.java +++ b/src/main/java/frc/robot/subsystems/GroundSpinny.java @@ -17,6 +17,7 @@ public class GroundSpinny extends SubsystemBase { 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; + public static final double GROUND_INTAKE_HOLD_SPEED = -1; // TalonFX private final TalonFX motor; @@ -80,6 +81,10 @@ public Command holdFunnelIntakePower() { return holdPower(FUNNEL_INTAKE_SPEED).withName("hold funnel intake power"); } + public Command holdCoralPower() { + return holdPower(GROUND_INTAKE_HOLD_SPEED).withName("hold coral holding power"); + } + public void imperativeSetGroundIntakePower() { motor.setVoltage(GROUND_INTAKE_SPEED); lastSetPower = GROUND_INTAKE_SPEED; diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d37b680b..c7d71428 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -203,6 +203,60 @@ public Command groundIntake(BooleanSupplier retract) { } } + public Command supercycleGroundIntake(BooleanSupplier retract) { + return // Core intake sequence + Commands.sequence( + // Deploy the ground arm (and wait until it reaches the position). + groundArm.moveToPosition(GroundArm.GROUND_POSITION), + // Once it's out, set the ground spinny speed + groundSpinny.setGroundIntakePower(), + // After it's deployed, apply a constant voltage to press it into the bumper + // and continue. + groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE)) + + // Move on from the intake being down when stuff is triggered + .withDeadline(Commands.waitUntil(intakeSensor.inIntake().or(retract))) + // And bring it back inside the robot + .andThen( + Commands.sequence( + // Move ground intake to in positiong and hold it there + groundArm.moveToPosition(GroundArm.UP_POSITION).andThen(Commands.idle()), + // Set ground intake power to just hold the coral tightly + groundSpinny.holdCoralPower())); + } + + // This is called when a coral is in the ground intake and the robot has just scored an algae + public Command superCycleCoralHandoff() { + return Commands.sequence( + // Initial setup- Move elevator high enough for ground arm to be clear, start moving + // arm pivot, stop the spinny claw, and start spinning the ground intake + Commands.parallel( + elevator.setLevel(ElevatorSubsystem.MIN_FULL_GROUND_INTAKE), + armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + spinnyClaw.stop(), // just as a backup in case things are silly + groundSpinny.setGroundIntakePower()) + // Move on even if arm isn't in position yet as long as elevator is high enough + .until(elevator.above(ElevatorSubsystem.MIN_FULL_GROUND_INTAKE)), + // Move the ground intake out + groundArm.moveToPosition(GroundArm.MIN_OUT_QUICK_HANDOFF), + // Prep is done, now do the handoff + Commands.parallel( + // These three are the initial setup: Move elevator down to the handoff + // height, make sure armPivot finishes moving to the right height, and + // spin claw + elevator.setLevel(ElevatorSubsystem.CORAL_QUICK_INTAKE), + armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + spinnyClaw.coralIntakePower()), + // Start the sequence of handing off the coral + groundArm + .moveToPosition(GroundArm.STOWED_POSITION) + .until(groundArm.atPosition(GroundArm.QUICK_INTAKE_POSITION)), + // Spin groundSpinny out, but skip if we lost the coral. + groundSpinny.setQuickHandoffExtakeSpeed().onlyIf(armSensor.inClaw()), + // Go back to stow, but skip if we lost the coral. + coralStow().onlyIf(armSensor.inClaw())); + } + // 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) { @@ -370,7 +424,8 @@ public Command algaeLevelThreeFourIntake() { // removes algae from upper reef Commands.parallel( spinnyClaw.algaeIntakePower(), armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR))) + groundArm.moveToPosition(GroundArm.UP_POSITION).andThen(Commands.idle())), + elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR)) .withName("Algae L3-L4 Intake"); } @@ -379,7 +434,8 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef Commands.parallel( spinnyClaw.algaeIntakePower(), armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE))) + elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE)), + groundArm.moveToPosition(GroundArm.STOWED_POSITION).andThen(Commands.idle())) .withName("Algae L2-L3 Intake"); } @@ -388,7 +444,9 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef return Commands.parallel( spinnyClaw.algaeIntakePower(), Commands.sequence( - armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE), + Commands.parallel( + armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE), + groundArm.moveToPosition(GroundArm.STOWED_POSITION).andThen(Commands.idle())), elevator.setLevel(ElevatorSubsystem.ALGAE_GROUND_INTAKE))) .withName("Algae Ground Intake"); } @@ -417,7 +475,7 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p public Command algaeNetScore(BooleanSupplier score) { return Commands.sequence( Commands.parallel( - elevator.setLevel(ElevatorSubsystem.ALGAE_NET_SCORE), + groundArm.moveToPosition(GroundArm.UP_POSITION).andThen(Commands.idle()), armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE), spinnyClaw.algaeIntakePower()), Commands.waitUntil(score),