feat: finish intake, add commands

flamingchickens1540 · Dec 5, 2023 · 444414e · 444414e
1 parent 09421c5
commit 444414e
Show file tree

Hide file tree

Showing 5 changed files with 114 additions and 16 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -23,7 +23,6 @@ public class IntakeIOReal implements IntakeIO {
   private final StatusSignal<Double> rollerCurrent = roller.getStatorCurrent();
 
   public IntakeIOReal() {
-    //        roller.
 
     leadSpark.restoreFactoryDefaults();
     followSpark.restoreFactoryDefaults();
@@ -33,14 +32,16 @@ public IntakeIOReal() {
 
     leadSpark.enableVoltageCompensation(12.0);
     leadSpark.setSmartCurrentLimit(30);
+    followSpark.enableVoltageCompensation(12.0);
+    followSpark.setSmartCurrentLimit(30);
 
     leadSpark.burnFlash();
     followSpark.burnFlash();
 
     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);
+    }
   }
 }