diff --git a/src/main/java/frc/robot/auto/Autonomous.java b/src/main/java/frc/robot/auto/Autonomous.java index 0c425275..bce20a50 100644 --- a/src/main/java/frc/robot/auto/Autonomous.java +++ b/src/main/java/frc/robot/auto/Autonomous.java @@ -75,13 +75,11 @@ private Command singleNeutralCycle(boolean rightSide) { robot .getSwerveDrive() .followPath("left_neutral.0", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.1", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral.2", rightSide), shootFuel.shoot()); } @@ -104,30 +102,25 @@ private Command doubleNeutralCycle(boolean rightSide) { robot .getSwerveDrive() .followPath("left_neutral.0", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.1", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral.2", rightSide), shootFuel.shootAllFuelStationary().withTimeout(20.0 - 13.2), robot .getSwerveDrive() .followPath("left_neutral.3", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.4", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.5", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), shootFuel.shoot()); } @@ -150,24 +143,24 @@ public Command bump(boolean rightSide) { .getSwerveDrive() .followPath("left_neutral_bump.0", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral_bump.1", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral_bump.2", rightSide), shootFuel.shootAllFuelStationary().withTimeout(5), robot .getSwerveDrive() .followPath("left_neutral_bump.3", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral_bump.4", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral_bump.5", rightSide), shootFuel.shoot()) .withTimeout(20); @@ -218,9 +211,11 @@ public Command center(boolean fromHub) { 3.616589307785034, 5.050553798675537, Rotation2d.k180deg))), Commands.parallel( Commands.sequence( - robot - .getSwerveDrive() - .followPath(pathName + ".0"), // Drive away from hub to where shooting can start + Commands.parallel( + robot.getSwerveDrive().followPath(pathName + ".0"), + robot + .getIntakeExtension() + .extend()), // Drive away from hub to where shooting can start robot .getSwerveDrive() .followPath(pathName + ".1") @@ -228,8 +223,7 @@ public Command center(boolean fromHub) { shootFuel.shootOnTheMove()), // Drive while shooting preload to depot setup // position shootFuel.shootAllFuelStationary().withTimeout(2) // Shoot any remaining fuel - ), - robot.getIntakeExtension().extend()), + )), robot .getSwerveDrive() .followPath(pathName + ".2") diff --git a/src/main/java/frc/robot/auto/ShootFuel.java b/src/main/java/frc/robot/auto/ShootFuel.java index 49f8d80d..ccaf81a9 100644 --- a/src/main/java/frc/robot/auto/ShootFuel.java +++ b/src/main/java/frc/robot/auto/ShootFuel.java @@ -23,7 +23,9 @@ public Command shootAllFuel() { .feed() .onlyWhile( () -> autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation()) - .repeatedly()) + .repeatedly(), + robot.getIntakeRollers().intake(), + robot.getIntakeExtension().agitate()) .until(() -> robot.getHopper().isEmpty()); } @@ -40,7 +42,8 @@ public Command shootAllFuelStationary() { Commands.waitUntil( () -> autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation()) .andThen(robot.getHopper().feed().repeatedly()), - robot.getIntakeRollers().intake()) + robot.getIntakeRollers().intake(), + robot.getIntakeExtension().agitate()) .until(() -> robot.getHopper().isEmpty()); } @@ -91,8 +94,7 @@ public Command shootAllFuelOnTheMove() { autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation()) .repeatedly()), - robot.getIntakeExtension().retract(), - robot.getIntakeExtension().extend().repeatedly(), + robot.getIntakeExtension().agitate(), robot.getIntakeRollers().intake()) .until(() -> robot.getHopper().isEmpty()); } diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index e0be2d8e..3a2e7af9 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -292,14 +292,6 @@ public void configureBindings() { .getIntakeExtension() .moveAtVoltage(IntakeExtensionConstants.FINE_CONTROL_VOLTAGE.unaryMinus()))); - // Intake extension and retraction - WORKS - Trigger intakeRetract = operator.rightStick().or(driver.start()); - Trigger intakeExtend = - intakeRetract.negate().and(RobotState::isTeleop).and(RobotState::isEnabled); - - intakeRetract.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().retract()); - intakeExtend.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().extend()); - // Climb retraction // Command autodescend = robot.getClimb().descend(); // Trigger climbRetract = @@ -379,7 +371,16 @@ public void configureBindings() { RotationsPerSecondPerSecond.of(hubMaxAngularAcceleration))), Set.of())) .and(autoShoot.isReadyToShoot()) - .whileTrue(robot.getHopper().feed()); + .whileTrue( + robot + .getHopper() + .feed() + .alongWith( + robot + .getIntakeExtension() + .agitate() + .alongWith(robot.getIntakeRollers().intake()) + .onlyWhile(driver.rightStick().negate()))); shootButtonsTrigger .and(inAllianceZone.negate()) @@ -434,6 +435,18 @@ public void configureBindings() { // Degrees.of(1))), // robot.getHopper().feed()))); + // Intake extension and retraction - WORKS + Trigger intakeRetract = operator.rightStick().or(driver.start()); + Trigger intakeExtend = + intakeRetract + .negate() + .and(RobotState::isTeleop) + .and(RobotState::isEnabled) + .and(shootButtonsTrigger.and(inAllianceZone).and(autoShoot.isReadyToShoot()).negate()); + + intakeRetract.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().retract()); + intakeExtend.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().extend()); + new ShiftFeedback(List.of(driverRumble, operatorRumble)); } diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java index b5b70320..15d84daa 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; 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 com.ctre.phoenix6.BaseStatusSignal; @@ -261,6 +262,33 @@ public Command retract() { IntakeExtensionConstants.POSITION_TOLERANCE)); } + public Command agitate() { + return Commands.repeatingSequence( + startEnd( + () -> { + motor.setControl( + new MotionMagicVoltage( + IntakeExtensionConstants.AGITATED_POSITION.in(Meters))); + }, + () -> { + if (!getPosition() + .isNear( + IntakeExtensionConstants.AGITATED_POSITION, + IntakeExtensionConstants.POSITION_TOLERANCE)) { + motor.setControl(new MotionMagicVoltage(getPosition().in(Meters))); + } + }) + .until( + () -> + getPosition() + .isNear( + IntakeExtensionConstants.AGITATED_POSITION, + IntakeExtensionConstants.POSITION_TOLERANCE)) + .withTimeout(0.25), + Commands.waitTime(Seconds.of(0.5)), + extend().withTimeout(0.25)); + } + public Command extendSlow() { return startEnd( () -> { diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java index 1092cc12..9ffceef1 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java @@ -26,17 +26,18 @@ public final class IntakeExtensionConstants { public static final int MOTOR_CAN_ID = 40; - public static final DCMotor MOTOR_PHYSICS = DCMotor.getKrakenX44Foc(1); - public static final Mass MOVING_MASS = Pounds.of(12); + public static final DCMotor MOTOR_PHYSICS = DCMotor.getKrakenX44(1); + public static final Mass MOVING_MASS = Pounds.of(13.25); public static final Distance MAX_POSITION = Inches.of(10.1); public static final Distance MIN_POSITION = Inches.of(0); public static final Distance RETRACT_POSITION = Inches.of(3); - public static final Angle ANGLE = Degrees.of(-18.5); - public static final Distance PINION_RADIUS = Inches.of(0.75); + public static final Distance AGITATED_POSITION = Inches.of(8); + public static final Angle ANGLE = Degrees.of(-18.6196233); + public static final Distance PINION_RADIUS = Inches.of(1.5); public static final int CANDI_DEVICE_ID = 20; public static final Distance POSITION_TOLERANCE = Inches.of(0.125); public static final Voltage FINE_CONTROL_VOLTAGE = Volts.of(1.0); - public static final double GEAR_RATIO = 7.2; + public static final double GEAR_RATIO = 17.5; public static final TalonFXConfiguration MOTOR_CONFIGURATION = new TalonFXConfiguration() @@ -52,12 +53,12 @@ public final class IntakeExtensionConstants { .withMotionMagicJerk(0)) .withSlot0( new Slot0Configs() - .withKA(0.035) + .withKA(0.0393) .withKD(0.0) .withKP(100.0) - .withKG(-0.045) - .withKV(5.878) - .withKS(0.315) + .withKG(-0.1196) + .withKV(6.737) + .withKS(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign)) .withCurrentLimits( new CurrentLimitsConfigs() @@ -67,7 +68,7 @@ public final class IntakeExtensionConstants { .withStatorCurrentLimitEnable(true)) .withMotorOutput( new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) + .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast)); public static final CANdiConfiguration CANDI_CONFIGURATION = new CANdiConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index 0a43bf0a..8d2adbf0 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -4,12 +4,14 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.phoenix.StatusUtil; import dev.doglog.DogLog; @@ -23,29 +25,41 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeRollers extends SubsystemBase { - private TalonFX intakeMotor; + private TalonFX leaderMotor; + private TalonFX followerMotor; private StatusSignal velocitySignal; private StatusSignal statorCurrentSignal; private StatusSignal supplyCurrentSignal; private StatusSignal appliedVoltageSignal; private IntakeRollerSim simulation; - private double intakeVoltage = 4.0; + private double intakeVoltage = 12.0; private double intakeStallVoltage = 12.0; private Debouncer stallDebouncer = new Debouncer(0.25, DebounceType.kRising); private boolean stalling = false; /** Intializes motor and status signals Class for Intake Rollers */ public IntakeRollers() { - this.intakeMotor = - new TalonFX(IntakeRollersConstants.DEVICE_ID, new CANBus("subsystems")); // temporary - - intakeMotor.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); - this.velocitySignal = intakeMotor.getVelocity(); - this.statorCurrentSignal = intakeMotor.getStatorCurrent(); - this.supplyCurrentSignal = intakeMotor.getSupplyCurrent(); - this.appliedVoltageSignal = intakeMotor.getMotorVoltage(); + leaderMotor = new TalonFX(IntakeRollersConstants.LEADER_ID_1, IntakeRollersConstants.CANBUS); + + leaderMotor.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); + + followerMotor = + new TalonFX(IntakeRollersConstants.FOLLOWER_ID_2, IntakeRollersConstants.CANBUS); + + IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted = + IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted + == InvertedValue.Clockwise_Positive + ? InvertedValue.CounterClockwise_Positive + : InvertedValue.Clockwise_Positive; + + followerMotor.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); + + this.velocitySignal = leaderMotor.getVelocity(); + this.statorCurrentSignal = leaderMotor.getStatorCurrent(); + this.supplyCurrentSignal = followerMotor.getSupplyCurrent(); + this.appliedVoltageSignal = leaderMotor.getMotorVoltage(); if (RobotBase.isSimulation()) { - simulation = new IntakeRollerSim(intakeMotor); + simulation = new IntakeRollerSim(leaderMotor); } DogLog.tunable( @@ -63,6 +77,11 @@ public IntakeRollers() { }); CurrentDrawLogger.add("Intake Rollers", this::getSupplyCurrent); + + followerMotor.setControl(new Follower(leaderMotor.getDeviceID(), MotorAlignmentValue.Opposed)); + if (RobotBase.isSimulation()) { + simulation = new IntakeRollerSim(leaderMotor); + } } /** Returns command to make the motor move and stop */ @@ -70,10 +89,10 @@ public IntakeRollers() { private Command move(Voltage voltage) { return startEnd( () -> { - intakeMotor.setControl(new VoltageOut(voltage).withEnableFOC(false)); + leaderMotor.setControl(new VoltageOut(voltage).withEnableFOC(false)); }, () -> { - intakeMotor.setControl(new CoastOut()); + leaderMotor.setControl(new CoastOut()); }); } @@ -85,27 +104,11 @@ private Command move(Voltage voltage) { public Command intake() { return runEnd( () -> { - intakeMotor.setControl( + leaderMotor.setControl( new VoltageOut(stalling ? intakeStallVoltage : intakeVoltage).withEnableFOC(false)); }, () -> { - intakeMotor.setControl(new CoastOut()); - }); - } - - /** - * Returns command where motor intakes fuel at full speed. This should only be used when shooting - * fuel out of the robot with the intake is not a concern. - * - * @return The command to intake fuel at full speed. - */ - public Command intakeFast() { - return startEnd( - () -> { - intakeMotor.setControl(new VoltageOut(7).withEnableFOC(false)); - }, - () -> { - intakeMotor.setControl(new CoastOut()); + leaderMotor.setControl(new CoastOut()); }); } @@ -117,10 +120,10 @@ public Command intakeFast() { public Command outtake() { return startEnd( () -> { - intakeMotor.setControl(new DutyCycleOut(-1).withEnableFOC(false)); + leaderMotor.setControl(new DutyCycleOut(-1).withEnableFOC(false)); }, () -> { - intakeMotor.setControl(new CoastOut()); + leaderMotor.setControl(new CoastOut()); }); } @@ -143,12 +146,13 @@ public Current getStatorCurrent() { } /** - * Takes status signal Supply Current and returns it as a Current + * Returns the total supply current draw of the mechanism. It is multiplied by 2 to account for + * the second intake roller motor. * * @return Current */ public Current getSupplyCurrent() { - return supplyCurrentSignal.getValue(); + return Amps.of((2 * supplyCurrentSignal.getValueAsDouble())); } /** diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java index 625f1155..263509a3 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Amps; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -10,9 +11,11 @@ import com.ctre.phoenix6.signals.InvertedValue; public class IntakeRollersConstants { - public static final double GEAR_RATIO = 32 / 18; - public static final double MOMENT_OF_INERTIA = 0.00074271944; - public static final int DEVICE_ID = 41; + public static final double GEAR_RATIO = 15 / 27; + public static final double MOMENT_OF_INERTIA = 0.00022826; + public static final int LEADER_ID_1 = 41; + public static final int FOLLOWER_ID_2 = 42; + public static final CANBus CANBUS = new CANBus("subsystems"); public static final TalonFXConfiguration MOTOR_CONFIGURATION = new TalonFXConfiguration()