testing

src/main/java/frc/robot/Constants.java

@@ -99,7 +99,7 @@ public static class IntakeConstants {
     public static final double encodermiddle = 1;
     public static final double rollersclockwise = 1;
     public static final double rollerscounterclockwise = 1; 
-    public static final double sensorreached = .3;
+    public static final double sensorreached = .2;
     public static final double pivotclockwise = 1;
     public static final double pivotcounterclockwise = -1;
     public static final double pastsensortime = 3;

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

@@ -173,8 +173,17 @@ public RobotContainer() {
     }
 
     private void configureBindings() {
+
+        // xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3), intakePivotSubsystem));
+
+        // rightBumper.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem));
+
+        // leftBumper.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.85), intakePivotSubsystem));
+
         aButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.85), intakePivotSubsystem).alongWith(
-            new IntakeRollerIntakeCommand(intakeRollerSubsystem, ledSubsystem)));
+            new IntakeRollerIntakeCommand(intakeRollerSubsystem, ledSubsystem)).andThen(
+                new IntakeRollerFeedCommand(intakeRollerSubsystem).withTimeout(.2)
+            ));
 
         bButton.onTrue(new IdleCommand(intakePivotSubsystem, intakeRollerSubsystem,
                 elevatorSubsystem,

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

@@ -29,7 +29,7 @@ public IdleCommand(IntakePivotSubsystem intakePivotSubsystem,
         // );
 
         addCommands(new InstantCommand(() -> intakeRollersSubsystem.setAllRollSpeed(0, 0), intakeRollersSubsystem),
-                    new ElevatorToGroundCommand(elevatorSubsystem),
+                    // new ElevatorToGroundCommand(elevatorSubsystem),
                     new ShooterFlywheelStopCommand(shooterFlywheelSubsystem),
                     new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem),
                     new InstantCommand(() -> {

src/main/java/frc/robot/commands/sequences/ShootModeSequence.java

@@ -42,7 +42,7 @@ public ShootModeSequence(
             ShooterPivotSubsystem shooterPivotSubsystem,
             LEDSubsystem ledSubsystem) {
         addCommands(new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem).alongWith(
-                new ElevatorToGroundCommand(elevatorSubsystem),
+                // new ElevatorToGroundCommand(elevatorSubsystem),
                 new ShooterPivotSetAngleCommand(shooterPivotSubsystem, Units.degreesToRadians(20)) // STUB FOR AUTOAIM
         ));
     }

src/main/java/frc/robot/subsystems/climb/ClimbArm.java

@@ -42,7 +42,7 @@ public void update() {
             resetEncoder();
 
         if (extensionEncoder.getPosition() == 0 && !zeroLimitSwitch.get())
-            System.out.println(this.toString() + "encoder uncalibrated!");
+            // System.out.println(this.toString() + "encoder uncalibrated!");
 
         if (isAtExtension())
             winchMotor.set(0);

src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java

@@ -13,21 +13,21 @@ public class IntakePivotSubsystem extends SubsystemBase {
     private final TalonFX pivotMotor;
     // private final DigitalInput extendedlimitswitch;
     // private final DigitalInput retractedlimitswitch;
-    private final Encoder intakeencoder;
+    // private final Encoder intakeencoder;
     private PositionVoltage request = new PositionVoltage(0).withSlot(0);
     private final double P = 1;
     private final double I = 0;
     private final double D = 0;
     private final double CONVERSION_FACTOR = 0.2142; // TODO tune
-    private final double OFFSET = 0.9495;
+    private final double OFFSET = 0;// 0.9495;
 
 
      /**
       * Subsystem for controlling the pivot on the intake.
       */
     public IntakePivotSubsystem() {
         pivotMotor = new TalonFX(PIVOT_MOTOR_ID);
-        intakeencoder = new Encoder(1, 2);
+        // intakeencoder = new Encoder(1, 2);
         // extendedlimitswitch = new DigitalInput(extendedlimitswitchID);
         // retractedlimitswitch = new DigitalInput(retractedlimitswitchID);
         Slot0Configs slot0Configs = new Slot0Configs();
@@ -37,6 +37,7 @@ public IntakePivotSubsystem() {
         slot0Configs.kD = D;
 
         pivotMotor.getConfigurator().apply(slot0Configs);
+        pivotMotor.setPosition(0);
     }
 
     /**
@@ -53,14 +54,14 @@ public void setPosition(double position) {
      * resets encoder to zero.
      */
     public void resetEncoder() {
-        intakeencoder.reset();
+        // pivotMotor.reset();
     }
 
     /**
      * returns the encoder position.
      */
     public double encoderPosition() {
-        return intakeencoder.get();
+        return pivotMotor.get();
     }
 
     /**
@@ -96,6 +97,6 @@ public void movePivot(double speed) {
 
     @Override
     public void periodic() {
-        System.out.println(pivotMotor.getClosedLoopReference());
+        // System.out.println(pivotMotor.getPosition().getValueAsDouble() * CONVERSION_FACTOR);
     }
 }

src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java

@@ -107,8 +107,6 @@ public void periodic() {
 
         OpacityColor noteColor = pieceSeen ? crossFadeWithTime(NOTE_COLOR, TRANSPARENT_COLOR, 1) : NOTE_COLOR;
 
-        System.out.println(notePosition);
-
         switch (notePosition) {
             case NONE:
                 noteLayer.fillColor(pieceSeen ? noteColor : new OpacityColor());

src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java

@@ -172,7 +172,7 @@ public void periodic() {
         if (timer.advanceIfElapsed(.2)) { 
             //printCurrentAngle();
             //System.out.println(Util.twoDecimals(Units.radiansToDegrees(getAutoAimAngle())));
-            System.out.println(rotationEncoder.getPosition());
+            // System.out.println(rotationEncoder.getPosition());
         }
 
         // System.out.println("current pos" + rotationEncoder.getPosition());