Clean up robot container and a some of intake code

.gitignore

@@ -176,3 +176,5 @@ logs/
 
 # Folder that has CTRE Phoenix Sim device config storage
 ctre_sim/
+simgui.json
+simgui-ds.json

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

@@ -7,10 +7,7 @@
 
 package frc4388.robot;
 
-import java.time.Instant;
-
 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.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -23,7 +20,6 @@ import frc4388.robot.commands.Autos.PlaybackChooser;
 import frc4388.robot.commands.Swerve.JoystickPlayback;
 import frc4388.robot.commands.Swerve.JoystickRecorder;
 import frc4388.robot.commands.Intake.ArmIntakeIn;
-import frc4388.robot.commands.Intake.RotateIntakeToPosition;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
@@ -70,17 +66,6 @@ public class RobotContainer {
         new InstantCommand(() -> m_robotShooter.spin())
     );
 
-    private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotShooter.spin()),
-        new InstantCommand(() -> m_robotIntake.handoff())
-    );
- 
-    private SequentialCommandGroup intakeInToOut = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotIntake.rotateArmOut2(), m_robotIntake),
-        new RunCommand(() -> m_robotIntake.limitNote(), m_robotIntake).until(m_robotIntake.getArmFowardLimitState()),
-        new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)
-    );
-
     private SequentialCommandGroup i = new SequentialCommandGroup(
         intakeToShootStuff, intakeToShoot
     );
@@ -101,22 +86,22 @@ public class RobotContainer {
     private Command startRightMoveLeft = new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt");
 
     private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup (
-        ejectToShoot,
+        ejectToShoot.asProxy(),
-        taxi
+        taxi.asProxy()
     );
     private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup(
-        startLeftMoveRight,
+        startLeftMoveRight.asProxy(),
-        ejectToShoot,
+        ejectToShoot.asProxy(),
-        taxi
+        taxi.asProxy()
     );
     private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup(
-        startRightMoveLeft,
+        startRightMoveLeft.asProxy(),
-        ejectToShoot,
+        ejectToShoot.asProxy(),
-        taxi
+        taxi.asProxy()
     );
 
     private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
-            .addOption("Taxi Auto", taxi)
+            .addOption("Taxi Auto", taxi.asProxy())
             .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker)
             .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft)
             .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight)
@@ -141,10 +126,6 @@ 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));
 
-        
-       // SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity());
-
-       // m_robotIntake.resetPostion();
     }
 
     /**
@@ -185,13 +166,6 @@ public class RobotContainer {
         
         
         /* Operator Buttons */
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-        //      .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake))
-        //      .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
-             
-        // 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.pidIn()))
@@ -204,46 +178,17 @@ public class RobotContainer {
 
         // Override Intake Position encoder: out
         new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotIntake.setPosition(-53), m_robotIntake));
+            .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake));
 
-        // Override Intake Position encoder: out
+        // Override Intake Position encoder: in
         new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotIntake.setPosition(0), m_robotIntake));
+            .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake));
 
-        // //Pull arm in
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotIntake.pidIn(), m_robotIntake))
-        //     .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake));
-
-        // //Pull arm out
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotIntake.pidOut(), m_robotIntake))
-        //     .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake));
-        
-        // //Intake Note
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake))
-        //     .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
-        
-        // //Outtake Note
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake))
-        //     .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
-            
         //Spin Shooter Motors
         new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
              .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter))
              .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter));
 
-        // //Intake Note and ramp up shooter to 40%
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
-        //     .onTrue(intakeToShoot);
-        
-        // //Ramps up shooter to 100% to Shooter
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-        //     .onTrue(outtakeToShootFull);
-        
-
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(ejectToShoot)
             .onFalse(turnOffShoot);

src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java

@@ -1,38 +0,0 @@
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-package frc4388.robot.commands.Intake;
-
-
-import com.revrobotics.CANSparkMax;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import frc4388.robot.commands.PID;
-import frc4388.robot.subsystems.SwerveDrive;
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
-
-import frc4388.robot.subsystems.Intake;
-public class RotateIntakeToPosition extends PID {
-
-  Intake intake;
-  double targetAngle;
-
-  /** Creates a new PIDSparkMax. */
-  public RotateIntakeToPosition(Intake intake, double targetAngle) {
-    super(0.3, 0.0, 0.0, 0.0, 1);
-    
-    this.intake = intake;
-    this.targetAngle = targetAngle;
-
-    addRequirements(intake);
-  }
-
-  @Override
-  public double getError() {
-    return targetAngle - (((intake.getEncoder().getPosition()) * (360))%360);
-  }
-
-  @Override
-  public void runWithOutput(double output) {
-    intake.setVoltage(output / Math.abs(getError()));    
-  }
-}

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

@@ -36,10 +36,7 @@ public class Intake extends SubsystemBase {
   private BooleanSupplier sup = () -> true;
   private BooleanSupplier dup = () -> false;
 
-  private double val;
+  private double smartDashboardOuttakeValue;
-  
-
-
 
   /** Creates a new Intake. */
   public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
@@ -66,6 +63,8 @@ public class Intake extends SubsystemBase {
     m_spedController.setP(armGains.kP);
     m_spedController.setI(armGains.kI);
     m_spedController.setD(armGains.kD);
+
+    SmartDashboard.putNumber("Intake Speed", 0.5);
   }
 
   //hanoff
@@ -123,9 +122,9 @@ public class Intake extends SubsystemBase {
 
   public void handoff2() {
     if(intakeforwardLimit.isPressed()) {
-      intakeMotor.set(-val);
+      intakeMotor.set(-smartDashboardOuttakeValue);
     } else {
-      intakeMotor.set(-val);
+      intakeMotor.set(-smartDashboardOuttakeValue);
     }
   }
 
@@ -161,17 +160,13 @@ public class Intake extends SubsystemBase {
     return pivot.getEncoder().getVelocity();
   }
 
-  public void resetPostion() {
+  public void setPivotEncoderPosition(int val) {
-    setPosition(0);
-  }
-
-  public void setPosition(int val) {
     pivot.getEncoder().setPosition(val);
   }
 
-  public void resetPosition1() {
+  public void resetPosition() {
     if(forwardLimit.isPressed()) {
-      resetPostion();
+      setPivotEncoderPosition(0);
     }
   }
 
@@ -206,9 +201,9 @@ public class Intake extends SubsystemBase {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("Vel Output", getVelocity());
     SmartDashboard.putNumber("Position", getPos());
-    resetPosition1();
+    resetPosition();
     changeIntakeNeutralState();
 
-    val = SmartDashboard.getNumber("Intake Speed", 0.5);
+    smartDashboardOuttakeValue = SmartDashboard.getNumber("Intake Speed", 0.5);
   }
 }