Merge pull request #13 from Coconuts2486-FRC/Connor_Manipulators

Intake code is the main thrust here

src/main/java/frc/robot/RobotContainer.java

@@ -47,6 +47,7 @@
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.ElevatorCommand;
+import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.LED.LEDCommand;
 import frc.robot.subsystems.Controls.CoralControl;
 import frc.robot.subsystems.Intake.Intake;
@@ -116,6 +117,9 @@ public class RobotContainer {
   // Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed
   final CommandXboxController driverController = new CommandXboxController(0); // Main Driver
 
+  private Trigger leftBumper = driverController.leftBumper();
+  private Trigger rightBumper = driverController.rightBumper();
+
   final CommandXboxController operatorController = new CommandXboxController(1); // Second Operator
   final OverrideSwitches overrides = new OverrideSwitches(2); // Console toggle switches
 
@@ -131,7 +135,7 @@ public class RobotContainer {
   private final PowerMonitoring m_power;
   private final Intake m_intake = new Intake(new IntakeIOKraken());
   private final LED m_led = new LED(new LEDIOCandle());
-  private final DigitalInput lightStop = new DigitalInput(1);
+  private final DigitalInput lightStop = new DigitalInput(5);
 
   /** Dashboard inputs ***************************************************** */
   // AutoChoosers for both supported path planning types
@@ -332,9 +336,39 @@ private void configureBindings() {
     // Press X button --> Stop with wheels in X-Lock position
     driverController.x().onTrue(Commands.runOnce(m_drivebase::stopWithX, m_drivebase));
 
-    driverController.leftBumper().whileTrue(Commands.run(() -> getAutonomousCommandPathPlanner()));
+    // driverController.a().whileTrue(new IntakeCommand(m_intake, 0));
+
+    // m_elevator.setDefaultCommand(
+    //     Commands.run(
+    //         () -> m_elevator.runVolts(driverController.getRightTriggerAxis()), m_elevator));
+
+    // the two driver controller bumpers below make it so when you let go of either button the
+    // intake pivot will go to a resting posistion
+
+    m_intake.setDefaultCommand(
+        Commands.run(
+            () ->
+                m_intake.runPivotVolts(
+                    driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis()),
+            m_intake));
+
+    driverController
+        .rightBumper()
+        .whileTrue(new IntakeCommand(m_intake, 0.25, -0.35, 0))
+        .whileFalse(new IntakeCommand(m_intake, 0.9, 0, 0).until(leftBumper));
+
+    driverController
+        .leftBumper()
+        .whileTrue(
+            new IntakeCommand(m_intake, 0.75, 0, 0)
+                .withTimeout(0.075)
+                .andThen(new IntakeCommand(m_intake, 0.75, 0.7, 0)))
+        .whileFalse(new IntakeCommand(m_intake, 0.9, 0, 0).until(rightBumper));
+
+    driverController.a().whileTrue(new IntakeCommand(m_intake, 0.9, 0, 1));
 
     // Press Y button --> Manually Re-Zero the Gyro
+
     driverController
         .y()
         .onTrue(
@@ -461,6 +495,7 @@ private void definesysIdRoutines() {
       //   autoChooserPathPlanner.addOption(
       //       "Flywheel SysId (Dynamic Reverse)",
       //       m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
     }
   }
 

src/main/java/frc/robot/commands/IntakeCommand.java

@@ -6,19 +6,30 @@
 public class IntakeCommand extends Command {
 
   private final Intake intake;
-  private final double direction;
+  private final double wantedPosistion;
+  private final double rollerSpeed;
+  private final double test; // get rid of this variable in future
 
-  public IntakeCommand(Intake intake, double direction) {
+  public IntakeCommand(Intake intake, double wantedPosistion, double rollerSpeed, double test) {
     this.intake = intake;
-    this.direction = direction;
+    this.wantedPosistion = wantedPosistion;
+    this.rollerSpeed = rollerSpeed;
+    this.test = test;
   }
 
   @Override
-  public void initialize() {}
+  public void initialize() {
+    intake.configure(1, 0, 0);
+  }
 
   @Override
   public void execute() {
-    intake.setRollerVolts(-direction);
+    if (test == 0) {
+      intake.setPivotPosition(wantedPosistion);
+      intake.rollerSpeed(rollerSpeed);
+    } else if (test == 1) {
+      System.out.println(intake.getEncoder());
+    }
   }
 
   @Override

src/main/java/frc/robot/subsystems/Intake/Intake.java

@@ -4,7 +4,6 @@
 import static edu.wpi.first.units.Units.Volts;
 
 import edu.wpi.first.units.Units;
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.util.RBSISubsystem;
 import org.littletonrobotics.junction.Logger;
@@ -38,6 +37,10 @@ public void setPivotPosition(double position) {
     io.setPivotPosition(position);
   }
 
+  public void rollerSpeed(double speed) {
+    io.rollerSpeed(speed);
+  }
+
   public void runPivotVolts(double volts) {
     io.setPivotVolts(volts);
   }
@@ -50,30 +53,17 @@ public void stop() {
     io.stop();
   }
 
-  public void configure(
-      double kG,
-      double kS,
-      double kV,
-      double kA,
-      double kP,
-      double kI,
-      double kD,
-      double velocity,
-      double acceleration,
-      double jerk) {
-    io.configure(kG, kS, kV, kA, kP, kI, kD, velocity, acceleration, jerk);
+  public void configure(double kP, double kI, double kD) {
+    io.configure(kP, kI, kD);
+  }
+  ;
+
+  public double getEncoder() {
+    return io.getEncoder();
   }
 
   @Override
   public int[] getPowerPorts() {
     return io.powerPorts;
   }
-
-  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
-    return sysId.quasistatic(direction);
-  }
-
-  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
-    return sysId.dynamic(direction);
-  }
 }

src/main/java/frc/robot/subsystems/Intake/IntakeIO.java

@@ -18,21 +18,17 @@ public default void updateInputs(IntakeIOInputs inputs) {}
 
   public default void setPivotPosition(double position) {}
 
+  public default void rollerSpeed(double speed) {}
+
   public default void stop() {}
 
   public default void setPivotVolts(double volts) {}
 
+  public default void configure(double kP, double kI, double kD) {}
+
   public default void setRollerVolts(double volts) {}
 
-  public default void configure(
-      double kG,
-      double kS,
-      double kV,
-      double kA,
-      double kP,
-      double kI,
-      double kD,
-      double velocity,
-      double acceleration,
-      double jerk) {}
+  public default double getEncoder() {
+    return 0.0;
+  }
 }

src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java

@@ -2,9 +2,9 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -13,16 +13,20 @@
 import frc.robot.Constants.CANandPowerPorts;
 
 public class IntakeIOKraken implements IntakeIO {
+
   private final TalonFX intakeRoller =
       new TalonFX(CANandPowerPorts.INTAKE_ROLLER.getDeviceNumber());
-
   private final TalonFX intakePivot = new TalonFX(CANandPowerPorts.INTAKE_PIVOT.getDeviceNumber());
 
+  private final CANcoder encoderActual = new CANcoder(23);
+
+  PIDController pivotPID = new PIDController(1.5, 0, 0);
+
   public final int[] powerPorts = {
     CANandPowerPorts.INTAKE_PIVOT.getPowerPort(), CANandPowerPorts.INTAKE_ROLLER.getPowerPort()
   };
 
-  private final StatusSignal<Angle> pivotPosition = intakePivot.getPosition();
+  private final StatusSignal<Angle> pivotPosition = encoderActual.getAbsolutePosition();
   private final StatusSignal<AngularVelocity> pivotVelocity = intakePivot.getVelocity();
   private final StatusSignal<Voltage> pivotAppliedVolts = intakePivot.getMotorVoltage();
   private final StatusSignal<Current> pivotCurrent = intakePivot.getSupplyCurrent();
@@ -31,11 +35,13 @@ public IntakeIOKraken() {}
 
   @Override
   public void updateInputs(IntakeIOInputs inputs) {
-    BaseStatusSignal.refreshAll(pivotPosition, pivotVelocity, pivotAppliedVolts, pivotCurrent);
+    BaseStatusSignal.refreshAll(
+        encoderActual.getAbsolutePosition(), pivotVelocity, pivotAppliedVolts, pivotCurrent);
     inputs.positionRad =
-        Units.rotationsToRadians(pivotPosition.getValueAsDouble()) / 21.42857; // gear ratio
+        Units.rotationsToRadians(
+            encoderActual.getAbsolutePosition().getValueAsDouble()); // 21.42857; // gear ratio
     inputs.velocityRadPerSec =
-        Units.rotationsToRadians(pivotVelocity.getValueAsDouble()) / 21.42857; // gear ratio
+        Units.rotationsToRadians(pivotVelocity.getValueAsDouble()); // 21.42857; // gear ratio
     inputs.appliedVolts = pivotAppliedVolts.getValueAsDouble();
     inputs.currentAmps = new double[] {pivotCurrent.getValueAsDouble()};
   }
@@ -58,39 +64,24 @@ public void stop() {
 
   @Override
   public void setPivotPosition(double position) {
-    final MotionMagicVoltage m_request = new MotionMagicVoltage(0);
+    intakePivot.set(
+        -pivotPID.calculate(encoderActual.getAbsolutePosition().getValueAsDouble(), position));
+  }
 
-    intakePivot.setControl(m_request.withPosition(position));
+  @Override
+  public void rollerSpeed(double speed) {
+    intakeRoller.set(speed);
+  }
+
+  @Override
+  public void configure(double kP, double kI, double kD) {
+    pivotPID.setP(kP);
+    pivotPID.setI(kI);
+    pivotPID.setD(kD);
   }
 
   @Override
-  public void configure(
-      double kG,
-      double kS,
-      double kV,
-      double kA,
-      double kP,
-      double kI,
-      double kD,
-      double velocity,
-      double acceleration,
-      double jerk) {
-
-    var talonFXConfigs = new TalonFXConfiguration();
-    var slot0Configs = talonFXConfigs.Slot0;
-
-    slot0Configs.kG = kG;
-    slot0Configs.kS = kS;
-    slot0Configs.kV = kV;
-    slot0Configs.kA = kA;
-    slot0Configs.kP = kP;
-    slot0Configs.kI = kI;
-    slot0Configs.kD = kD;
-
-    var motionMagicConfigs = talonFXConfigs.MotionMagic;
-
-    motionMagicConfigs.MotionMagicCruiseVelocity = velocity;
-    motionMagicConfigs.MotionMagicAcceleration = acceleration;
-    motionMagicConfigs.MotionMagicJerk = jerk;
+  public double getEncoder() {
+    return encoderActual.getAbsolutePosition().getValueAsDouble();
   }
 }