made PID, commented out all of the auto stuff cuz it broke the code (im tweakin)

src/main/java/frc4388/robot/Constants.java

@@ -166,7 +166,7 @@ public final class Constants {
         public static final double PIVOT_SPEED = 0.2; //TODO:
 
         public static final class ArmPID {
-            public static final Gains INTAKE_GAINS = new Gains(0, 0, 0, 0, 0, 1.0);
+            public static final Gains INTAKE_GAINS = new Gains(0.00005, 0, 0, 0, 0, 1.0);
         }
     }
 

src/main/java/frc4388/robot/Robot.java

@@ -81,20 +81,20 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void autonomousInit() {
-    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+    // m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
-    /*
-     * String autoSelected = SmartDashboard.getString("Auto Selector",
-     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
-     * = new MyAutoCommand(); break; case "Default Auto": default:
-     * autonomousCommand = new ExampleCommand(); break; }
-     */
+    // /*
+    //  * String autoSelected = SmartDashboard.getString("Auto Selector",
+    //  * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+    //  * = new MyAutoCommand(); break; case "Default Auto": default:
+    //  * autonomousCommand = new ExampleCommand(); break; }
+    //  */
 
-    // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.schedule();
-    }
-    m_robotTime.startMatchTime();
+    // // schedule the autonomous command (example)
+    // if (m_autonomousCommand != null) {
+    //   m_autonomousCommand.schedule();
+    // }
+    // m_robotTime.startMatchTime();
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -8,6 +8,7 @@
 package frc4388.robot;
 
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -56,7 +57,9 @@ public class RobotContainer {
 
     private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
 
-    private PlaybackChooser playbackChooser;
+    
+
+  //  private PlaybackChooser playbackChooser;
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -75,9 +78,12 @@ public class RobotContainer {
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
 
-        playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
-            .addOption("Taxi Auto", new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
-            .buildDisplay();
+        // playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
+        //     .addOption("Taxi Auto", new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
+        //     .buildDisplay();
+        //SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity());
+
+       // m_robotIntake.resetPostion();
     }
 
     /**
@@ -123,6 +129,18 @@ public class RobotContainer {
         // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
         //      .onTrue(new InstantCommand(() ->  new RotateIntakeToPosition(m_robotIntake, 360).execute(), m_robotIntake))
         //      .onFalse(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 0).execute(), m_robotShooter));
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotIntake.rotateArmIn()))
+            .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotIntake.rotateArmOut()))
+            .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotIntake.pidIn()))
+            .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
              
         // new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
         //     .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor()))
@@ -135,10 +153,10 @@ public class RobotContainer {
      *
      * @return the command to run in autonomous
      */
-    public Command getAutonomousCommand() {
-        // no auto
-        return playbackChooser.getCommand();
-    }
+    // public Command getAutonomousCommand() {
+         // no auto
+    //     return playbackChooser.getCommand();
+    // }
 
     /**
      * Add your docs here.

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

@@ -10,6 +10,7 @@ import com.revrobotics.SparkPIDController;
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.wpilibj.CAN;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants;
 import frc4388.robot.Constants.IntakeConstants;
@@ -31,15 +32,16 @@ public class Intake extends SubsystemBase {
     this.pivot = pivot;
 
     pivot.restoreFactoryDefaults();
+    //pivot.setInverted(true);
 
-    forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed);
-    reverseLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed);
+    forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
+    reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
     forwardLimit.enableLimitSwitch(true);
     reverseLimit.enableLimitSwitch(true);
     
 
     //Arm PID
-    m_spedController = intakeMotor.getPIDController();
+    m_spedController = pivot.getPIDController();
     m_spedController.setP(armGains.kP);
     m_spedController.setI(armGains.kI);
     m_spedController.setD(armGains.kD);
@@ -52,6 +54,7 @@ public class Intake extends SubsystemBase {
 
   //Rotate robot in for handoff
   public void rotateArmIn() {
+    //pivot.set(IntakeConstants.PIVOT_SPEED);
     pivot.set(IntakeConstants.PIVOT_SPEED);
   }
 
@@ -61,6 +64,15 @@ public class Intake extends SubsystemBase {
 
   }
 
+  public void pidIn() {
+    m_spedController.setReference(8000, CANSparkMax.ControlType.kVelocity);
+    //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity());
+  }
+
+  public void pidOut() {
+
+  }
+
   public void handoff() {
     intakeMotor.set(-IntakeConstants.INTAKE_SPEED);
   }
@@ -68,12 +80,36 @@ public class Intake extends SubsystemBase {
   public void stopIntakeMotors() {
     intakeMotor.set(0);
   }
+
+  public void stopArmMotor() {
+    pivot.set(0);
+  }
+
   public RelativeEncoder getEncoder() {
     return pivot.getEncoder();
   }
   public void setVoltage(double voltage) {
    pivot.setVoltage(voltage);
   }
+
+  public double getVelocity() {
+    return pivot.getEncoder().getVelocity();
+  }
+
+  public void resetPostion() {
+    pivot.getEncoder().setPosition(0);
+  }
+
+  public void resetPosition1() {
+    if(forwardLimit.isPressed() == true) {
+      resetPostion();
+    }
+  }
+
+  public double getPos() {
+    return pivot.getEncoder().getPosition();
+  }
+
   public void rotateArm() {
 
   }
@@ -81,5 +117,8 @@ public class Intake extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    SmartDashboard.putNumber("Vel Output", getVelocity());
+    SmartDashboard.putNumber("Position", getPos());
+    resetPosition1();
   }
 }