Dcm pfixing auto intaking (#174)

* changed command structure off intake and raising arm command

* changed auto to start after intake command during rase arm to insert it into preveous cycle. FIxed issue where the intake was "too fast" and reflective sensor was triggering seemingly earlier in auto

* spot :/

src/main/deploy/pathplanner/autos/B-ThreePieceAutoTame.auto

@@ -65,7 +65,7 @@
                     {
                       "type": "named",
                       "data": {
-                        "name": "raiseArmAndWaitWhileAdvancingPiece"
+                        "name": "raiseArmAndWait"
                       }
                     }
                   ]

src/main/deploy/pathplanner/autos/B-TwoPieceFar1.auto

@@ -70,7 +70,7 @@
         {
           "type": "named",
           "data": {
-            "name": "raiseArmAndWaitWhileAdvancingPiece"
+            "name": "raiseArmAndWait"
           }
         },
         {

src/main/deploy/pathplanner/autos/R-TheTwoPieceNear.auto

@@ -65,7 +65,7 @@
                     {
                       "type": "named",
                       "data": {
-                        "name": "raiseArmAndWaitWhileAdvancingPiece"
+                        "name": "raiseArmAndWait"
                       }
                     }
                   ]

src/main/deploy/pathplanner/autos/R-ThreePieceAutoTame.auto

@@ -65,7 +65,7 @@
                     {
                       "type": "named",
                       "data": {
-                        "name": "raiseArmAndWaitWhileAdvancingPiece"
+                        "name": "raiseArmAndWait"
                       }
                     }
                   ]

src/main/deploy/pathplanner/autos/R-TwoPieceFar1.auto

@@ -70,7 +70,7 @@
         {
           "type": "named",
           "data": {
-            "name": "raiseArmAndWaitWhileAdvancingPiece"
+            "name": "raiseArmAndWait"
           }
         },
         {

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

@@ -245,10 +245,6 @@ private void createNamedCommands() {
     NamedCommands.registerCommand("raiseArmAndWait", 
       m_arm.createDeployCommand()
         .andThen(new WaitCommand(1.8)));
-
-    NamedCommands.registerCommand("raiseArmAndWaitWhileAdvancingPiece", 
-      m_arm.createDeployCommand().alongWith(m_intake.createSetVelocityCommand(0.5))
-        .andThen(new WaitCommand(1.8)).andThen(m_intake.createStopIntakeCommand()));
 
     NamedCommands.registerCommand("resetArmAndIntake", 
       m_arm.createStowCommand()
@@ -259,8 +255,11 @@ private void createNamedCommands() {
         .withTimeout(0.7));
 
     NamedCommands.registerCommand("intakePiece", 
-      createAutoIntakeCommandSequence()
-      );
+      m_arm.createStowCommand()
+        .andThen(m_intake.createIntakeCommand()).until(m_intake.hasNote)
+        .andThen(m_arm.createCarryCommand()
+        .alongWith(m_intake.createAdvanceAfterIntakingCommand()))
+    );
 
     NamedCommands.registerCommand("stopIntake", 
       m_intake.createStopIntakeCommand());
@@ -288,16 +287,18 @@ private void configureDriverButtonBindings() {
     new JoystickButton(m_driver,OIConstants.kZorroAIn)
     .whileTrue((new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematicsDriveFromArm, m_driver)));
 
-
     Trigger armDeployed = new Trigger(m_arm.stateChecker(ArmState.DEPLOYED));
     JoystickButton D_Button = new JoystickButton(m_driver, OIConstants.kZorroDIn);
 
     // Reverse intake to outake or reject intaking Note
     D_Button.and(armDeployed.negate())
-            .whileTrue(m_arm.createStowCommand().andThen(new WaitCommand(0.2).andThen(m_intake.createOuttakeToFloorCommand())));
-        // Shoot Note into Amp
+      .whileTrue(m_arm.createStowCommand()
+      .andThen(new WaitCommand(0.2)
+      .andThen(m_intake.createOuttakeToFloorCommand())));
+
+    // Shoot Note into Amp
     D_Button.and(armDeployed) 
-            .whileTrue(m_intake.createOuttakeToAmpCommand());
+      .whileTrue(m_intake.createOuttakeToAmpCommand());
   }
   // spotless:on
 
@@ -307,7 +308,6 @@ private void configureOperatorButtonBindings() {
     JoystickButton leftBumper = new JoystickButton(m_operator, Button.kLeftBumper.value);
     JoystickButton leftJoystickDown = new JoystickButton(m_operator, Button.kLeftStick.value);
 
-    Trigger hasNote = new Trigger(m_intake.eitherSensorSupplier());
     Trigger armDeployed = new Trigger(m_arm.stateChecker(ArmState.DEPLOYED));
 
     // CLIMBER
@@ -333,16 +333,19 @@ private void configureOperatorButtonBindings() {
     // Control position of Note in intake
     Trigger leftStick = new Trigger(() -> Math.abs(m_operator.getLeftY()) > 0.2);
     //While arm is down
-    leftStick.and(armDeployed.negate()).whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmDown));
+    leftStick.and(armDeployed.negate())
+      .whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmDown));
+
     //While arm is up
-    leftStick.and(armDeployed).whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmUp));
+    leftStick.and(armDeployed)
+      .whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmUp));
 
     // Intake Note from floor
-    rightBumper.and(hasNote.negate())
+    rightBumper.and(m_intake.hasNote.negate())
       .whileTrue(m_arm.createStowCommand()
       .andThen(m_intake.createIntakeCommand()));
 
-    hasNote.and(m_intake.stateChecker(IntakeState.INTAKING)).and(() -> RobotState.isTeleop())
+    m_intake.hasNote.and(m_intake.stateChecker(IntakeState.INTAKING)).and(() -> RobotState.isTeleop())
       .onTrue(m_arm.createCarryCommand()
       .andThen(m_intake.createAdvanceAfterIntakingCommand().withInterruptBehavior(InterruptionBehavior.kCancelIncoming)));
 

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

@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.IntakeConstants.IntakeState;
 import java.util.function.BooleanSupplier;
@@ -61,6 +62,7 @@ public Intake() {
     m_velocityController.setD(IntakeConstants.kVelocityD);
 
     m_positionController.setTolerance(IntakeConstants.kPositionTolerance);
+    m_positionController.setGoal(17);
 
     m_relativeEncoder = m_motor.getEncoder();
     m_relativeEncoder.setPosition(0.0);
@@ -88,23 +90,30 @@ public Command createStopIntakeCommand() {
   }
 
   private void configurePositionController(double targetPosition) {
+    SmartDashboard.putNumber("ResetBeam", 1);
     m_positionController.reset(m_relativeEncoder.getPosition(), m_relativeEncoder.getVelocity());
     m_positionController.setGoal(m_relativeEncoder.getPosition() + targetPosition);
+    m_positionController.calculate(m_relativeEncoder.getPosition());
   }
 
   // spotless:off
   private void advanceAfterIntaking(double targetPosition) {
     m_secondSensorTriggered.rising().ifHigh(
             () -> {
+              SmartDashboard.putNumber("RealativeEncoderReset", 0);
               m_positionController.reset(
                   m_relativeEncoder.getPosition(), m_relativeEncoder.getVelocity());
+                  SmartDashboard.putNumber("RealativeEncoderReset", 1);
               m_positionController.setGoal(m_relativeEncoder.getPosition() + targetPosition);
+                SmartDashboard.putNumber("RealativeEncoderReset", 2);
             });
     setState(IntakeState.PROCESSING);
     m_motor.set(m_positionController.calculate(m_relativeEncoder.getPosition()));
   }
   // spotless:on
 
+  public Trigger hasNote = new Trigger(this.eitherSensorSupplier());
+
   public Command createIntakeCommand() {
     return this.run(
         () -> {
@@ -182,6 +191,8 @@ public void periodic() {
     SmartDashboard.putNumber(
         "IntakeSensorRetroReflective", !m_noteSensorRetroReflective.get() ? 1d : 0d);
     SmartDashboard.putNumber("IntakeSensorBeamBreak", !m_noteSensorBeamBreak.get() ? 1d : 0d);
+    SmartDashboard.putNumber("RealativeEncoderReset", -1);
+    SmartDashboard.putNumber("ResetBeam", -1);
 
     if (m_state != null) SmartDashboard.putString("Intake State", m_state.name());
   }