added commands for intaking notes (im tweakin)

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

@@ -162,12 +162,12 @@ public final class Constants {
         public static final int INTAKE_MOTOR_ID = 18; //TODO:
         public static final int PIVOT_MOTOR_ID = 17; //TODO:
         public static final double INTAKE_SPEED = 0.75; //TODO:
-        public static final double INTAKE_OUT_SPEED = 0.2;
+        public static final double INTAKE_OUT_SPEED = 0.5;
         public static final double HANDOFF_SPEED = 0.2; //TODO:
         public static final double PIVOT_SPEED = 0.2; //TODO:
 
         public static final class ArmPID {
-            public static final Gains INTAKE_GAINS = new Gains(0.00005, 0, 0, 0, 0, 1.0);
+            public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0);
         }
     }
 

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

@@ -7,11 +7,16 @@
 
 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;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.commands.Autos.PlaybackChooser;
@@ -57,6 +62,28 @@ public class RobotContainer {
 
     private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
 
+   
+
+    private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotIntake.pidIn()),
+        new InstantCommand(() -> m_robotShooter.spin(0.4))
+    );
+
+    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()),
+        new RunCommand(() -> m_robotIntake.limitNote()).until(m_robotIntake.getArmFowardLimitState()),
+        new InstantCommand(() -> m_robotShooter.spin(0.4))
+    );
+
+
+
+    
+
     
 
     private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
@@ -140,25 +167,43 @@ public class RobotContainer {
         //     .onTrue(new InstantCommand(() -> m_robotIntake.rotateArmOut()))
         //     .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
 
-        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 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.A_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(new InstantCommand(() -> m_robotIntake.pidOut(), m_robotIntake))
-            .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake));
+            .whileTrue(intakeInToOut);
         
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake))
-            .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
-        
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_TRIGGER_AXIS)
-            .onTrue(new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake))
-            .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
-            
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_TRIGGER_AXIS)
-             .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter))
-             .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter));
         
         
     }

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

@@ -4,6 +4,8 @@
 
 package frc4388.robot.subsystems;
 
+import java.util.function.BooleanSupplier;
+
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.SparkLimitSwitch;
 import com.revrobotics.SparkPIDController;
@@ -78,13 +80,36 @@ public class Intake extends SubsystemBase {
   }
 
   public void pidIn() {
-    m_spedController.setReference(8000, CANSparkMax.ControlType.kVelocity);
+    m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition);
     //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity());
   }
 
-  public void pidOut() {
-    m_spedController.setReference(-8000, CANSparkMax.ControlType.kVelocity);
+  public void limitNote() {
+    if (intakeforwardLimit.isPressed()) {
+      rotateArmIn2();
+    } else {
+      spinIntakeMotor();
+    }
+  }
 
+  public void pidOut() {
+    m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
+  }
+
+  public void rotateArmOut2() {
+    if(reverseLimit.isPressed()){
+      stopArmMotor();
+    } else {
+      pidOut();
+    }
+  }
+
+  public void rotateArmIn2() {
+    if(forwardLimit.isPressed()){
+      stopArmMotor();
+    } else {
+      pidIn();
+    }
   }
 
   
@@ -131,6 +156,10 @@ public class Intake extends SubsystemBase {
 
   }
 
+  public BooleanSupplier getArmFowardLimitState() {
+    return forwardLimit::isPressed;
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run