diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index abb21fd..290247d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; +import frc.robot.commands.IntakeCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; @@ -34,6 +35,9 @@ import frc.robot.subsystems.flywheel.FlywheelIO; import frc.robot.subsystems.flywheel.FlywheelIOSim; import frc.robot.subsystems.flywheel.FlywheelIOSparkMax; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeIO; +import frc.robot.subsystems.intake.IntakeIOReal; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -46,6 +50,7 @@ public class RobotContainer { // Subsystems private final Drive drive; + private final Intake intake; private final Flywheel flywheel; // Controller @@ -68,6 +73,7 @@ public RobotContainer() { new ModuleIOSparkMax(1), new ModuleIOSparkMax(2), new ModuleIOSparkMax(3)); + intake = new Intake(new IntakeIOReal()); flywheel = new Flywheel(new FlywheelIOSparkMax()); // drive = new Drive( // new GyroIOPigeon2(), @@ -88,6 +94,7 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim()); flywheel = new Flywheel(new FlywheelIOSim()); + intake = null; break; default: @@ -100,6 +107,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); flywheel = new Flywheel(new FlywheelIO() {}); + intake = new Intake(new IntakeIO() {}); break; } @@ -149,11 +157,17 @@ private void configureButtonBindings() { new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive) .ignoringDisable(true)); - controller - .a() - .whileTrue( - Commands.startEnd( - () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); + + controller.a() + .whileTrue(new IntakeCommands.IntakeCommand(intake)) + .onFalse(new IntakeCommands.RetractCommand(intake)); + + +// controller.a() +// .onTrue(new IntakeCommands.IntakeCommand(intake)); +// controller.b() +// .onTrue(new IntakeCommands.RetractCommand(intake)); + } /** diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java new file mode 100644 index 0000000..8f3d1c8 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -0,0 +1,51 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.Intake; + +public class IntakeCommands { + + public static class IntakeCommand extends Command{ + private final Intake intake; + private final Rotation2d intakeAngle = Rotation2d.fromRadians(0); + + public IntakeCommand(Intake intake) { + this.intake = intake; + } + + @Override + public void initialize() { + intake.rollerPercent(0.7); + intake.setPivotAngle(intakeAngle); + intake.setSpringMode(true); + } + } + + public static class RetractCommand extends Command{ + private final Intake intake; + private final Rotation2d retractAngle = Rotation2d.fromRadians(0); + + public RetractCommand(Intake intake) { + this.intake = intake; + } + + @Override + public void initialize() { + intake.rollerPercent(0); + intake.setPivotAngle(retractAngle); + } + + @Override + public boolean isFinished() { + return intake.atGoal(); + } + + @Override + public void end(boolean interrupted) { + if(!interrupted){ + intake.setSpringMode(false); + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 3b0c08c..169cacd 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,6 +1,9 @@ package frc.robot.subsystems.intake; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; @@ -15,12 +18,18 @@ public class Intake extends SubsystemBase { private final LoggedTunableNumber kP = new LoggedTunableNumber("Intake/kP", 0); private final LoggedTunableNumber kI = new LoggedTunableNumber("Intake/kI", 0); private final LoggedTunableNumber kD = new LoggedTunableNumber("Intake/kD", 0); + private final LoggedTunableNumber maxVelocity = new LoggedTunableNumber("Intake/kD", 0); + private final LoggedTunableNumber maxAcceleration = new LoggedTunableNumber("Intake/kD", 0); + private final ProfiledPIDController controller = new ProfiledPIDController(kP.get(), kI.get(), kD.get(), new TrapezoidProfile.Constraints(maxVelocity.get(),maxAcceleration.get())); + private final ArmFeedforward ffmodel = new ArmFeedforward(0,0,0); private Rotation2d setpoint = new Rotation2d(); - private Intake(IntakeIO io) { + public Intake(IntakeIO io) { this.io = io; io.setPivotBreakMode(true); + + controller.setTolerance(0.1); } @Override @@ -29,9 +38,16 @@ public void periodic() { Logger.processInputs("Intake", inputs); if (TUNING_MODE && (kP.hasChanged() || kI.hasChanged() || kD.hasChanged())) { - io.configurePID(kP.get(), kI.get(), kD.get()); + controller.setP(kP.get()); + controller.setI(kI.get()); + controller.setD(kD.get()); + } + if(TUNING_MODE && (maxAcceleration.hasChanged() || maxVelocity.hasChanged())){ + controller.setConstraints(new TrapezoidProfile.Constraints(maxVelocity.get(),maxAcceleration.get())); } + io.setPivotVoltage(controller.calculate(inputs.pivotPosition) + ffmodel.calculate(inputs.pivotPosition,inputs.pivotVelocity)); + Logger.recordOutput("Intake/setpoint", setpoint); } @@ -42,7 +58,19 @@ public void rollerPercent(double percent){ public void setPivotAngle(Rotation2d angle){ setpoint = angle; -// io.setPivotVoltage(); + controller.setGoal(angle.getRadians()); + } + + public double getPivotAngle(){ + return inputs.pivotPosition; + } + + public boolean atGoal(){ + return controller.atGoal(); + } + + public void setSpringMode(boolean springMode){ + io.springMode(springMode); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 97d33c5..17d176d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -21,5 +21,5 @@ public default void setRollerVoltage(double volts) {} public default void setPivotBreakMode(boolean breakMode) {} - public default void configurePID(double kP, double kI, double kD){} + public default void springMode(boolean springMode){} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index f5e434f..27acf7e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -23,7 +23,6 @@ public class IntakeIOReal implements IntakeIO { private final StatusSignal rollerCurrent = roller.getStatorCurrent(); public IntakeIOReal() { - // roller. leadSpark.restoreFactoryDefaults(); followSpark.restoreFactoryDefaults(); @@ -33,6 +32,8 @@ public IntakeIOReal() { leadSpark.enableVoltageCompensation(12.0); leadSpark.setSmartCurrentLimit(30); + followSpark.enableVoltageCompensation(12.0); + followSpark.setSmartCurrentLimit(30); leadSpark.burnFlash(); followSpark.burnFlash(); @@ -40,7 +41,7 @@ public IntakeIOReal() { var config = new TalonFXConfiguration(); config.CurrentLimits.StatorCurrentLimit = 30.0; config.CurrentLimits.StatorCurrentLimitEnable = true; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; roller.getConfigurator().apply(config); BaseStatusSignal.setUpdateFrequencyForAll(50.0, rollerVoltage, rollerCurrent); @@ -77,11 +78,15 @@ public void setPivotBreakMode(boolean breakMode) { followSpark.setIdleMode(idleMode); } + @Override - public void configurePID(double kP, double kI, double kD) { - leadSparkPID.setP(kP, 0); - leadSparkPID.setI(kI, 0); - leadSparkPID.setD(kD, 0); - leadSparkPID.setFF(0, 0); + public void springMode(boolean springMode) { + if (springMode){ + leadSpark.setSmartCurrentLimit(20); + followSpark.setSmartCurrentLimit(20); + } else { + leadSpark.setSmartCurrentLimit(30); + followSpark.setSmartCurrentLimit(30); + } } }