it not shooting properly (im tweakin)

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

@@ -155,15 +155,17 @@ public final class Constants {
     public static final class ShooterConstants {
         public static final int LEFT_SHOOTER_ID = 15; // final
         public static final int RIGHT_SHOOTER_ID = 16; // final
-        public static final double SHOOTER_SPEED = 1; // final
+        public static final double SHOOTER_SPEED = 1.0; // final
+        public static final double SHOOTER_IDLE = 0.4; // final
+
     }
 
     public static final class IntakeConstants {
         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.5;
-        public static final double HANDOFF_SPEED = 0.2; //TODO:
+        public static final double INTAKE_OUT_SPEED = 0.7;
+        public static final double HANDOFF_SPEED = 0.4; //TODO:
         public static final double PIVOT_SPEED = 0.2; //TODO:
 
         public static final class ArmPID {

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

@@ -8,12 +8,13 @@
 package frc4388.robot;
 
 import edu.wpi.first.cameraserver.CameraServer;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.utility.DeferredBlock;
 import frc4388.utility.RobotTime;
-
+//import frc4388.robot.subsystems.LED;
 /**
  * The VM is configured to automatically run this class, and to call the
  * functions corresponding to each mode, as described in the TimedRobot
@@ -23,16 +24,17 @@ import frc4388.utility.RobotTime;
  */
 public class Robot extends TimedRobot {
   Command m_autonomousCommand;
-  
+
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
-
+  //private LED mled = new LED();
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
    */
   @Override
   public void robotInit() {
+
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
@@ -50,6 +52,7 @@ public class Robot extends TimedRobot {
   @Override
   public void robotPeriodic() {
     m_robotTime.updateTimes();
+    //mled.updateLED();
     // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
     // commands, running already-scheduled commands, removing finished or interrupted commands,
     // and running subsystem periodic() methods.  This must be called from the robot's periodic

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

@@ -22,6 +22,7 @@ import frc4388.robot.Constants.OIConstants;
 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.SwerveDrive;
@@ -42,7 +43,7 @@ public class RobotContainer {
     private final RobotMap m_robotMap = new RobotMap();
 
     /* Subsystems */
-    private final LED m_robotLED = new LED(m_robotMap.LEDController);
+    private final LED m_robotLED = new LED();
 
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
                                                                   m_robotMap.rightFront,
@@ -61,12 +62,13 @@ public class RobotContainer {
     private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
 
     private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
+    private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
 
    
 
     private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
         new InstantCommand(() -> m_robotIntake.pidIn()),
-        new InstantCommand(() -> m_robotShooter.spin(0.4))
+        new InstantCommand(() -> m_robotShooter.spin())
     );
 
     private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup(
@@ -75,12 +77,24 @@ public class RobotContainer {
     );
  
     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))
+        new InstantCommand(() -> m_robotIntake.rotateArmOut2(), m_robotIntake),
+        new RunCommand(() -> m_robotIntake.limitNote(), m_robotIntake).until(m_robotIntake.getArmFowardLimitState()),
+        new InstantCommand(() -> m_robotShooter.idle(), m_robotShooter)
     );
 
+    private SequentialCommandGroup i = new SequentialCommandGroup(
+        intakeToShootStuff, intakeToShoot
+    );
 
+    private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
+        new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake)
+    );
+
+    private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
+        new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)
+    );
 
     
 
@@ -136,9 +150,9 @@ public class RobotContainer {
         //                                    "IntenseTaxi.txt"))
         //    .onFalse(new InstantCommand());
 
-        //new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
-        //    .onTrue(new JoystickPlayback(m_robotSwerveDrive, "IntenseTaxi.txt"))
-        //    .onFalse(new InstantCommand()); 
+        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+           .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
+           .onFalse(new InstantCommand()); 
         
         /* Speed */
         new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
@@ -159,13 +173,13 @@ public class RobotContainer {
         //      .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.Y_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotIntake.pidIn()))
+            .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.X_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotIntake.pidOut()))
+            .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
 
         // //Pull arm in
         // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
@@ -187,10 +201,10 @@ public class RobotContainer {
         //     .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));
+        //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)
@@ -202,8 +216,13 @@ public class RobotContainer {
         
 
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
-            .whileTrue(intakeInToOut);
-        
+            .onTrue(ejectToShoot)
+            .onFalse(turnOffShoot);
+
+
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
+            .onTrue(i)
+            .onFalse(new InstantCommand(() -> m_robotIntake.pidIn()));
         
         
     }
@@ -213,10 +232,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/commands/Intake/ArmIntakeIn.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// 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 edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Shooter;
+
+public class ArmIntakeIn extends Command {
+  /** Creates a new ArmIntakeIn. */
+  private Intake robotIntake;
+  private Shooter robotShooter;
+
+  public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.robotIntake = robotIntake;
+    this.robotShooter = robotShooter;
+
+    addRequirements(this.robotIntake, this.robotShooter);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    robotIntake.pidOut();
+    robotIntake.spinIntakeMotor();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if(robotIntake.getIntakeLimitSwtichState() == true) {
+      return true;
+    } else {
+      return false;
+    }
+
+  }
+}

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

@@ -32,6 +32,10 @@ public class Intake extends SubsystemBase {
 
   private Shooter shooter;
 
+  private BooleanSupplier sup = () -> true;
+  private BooleanSupplier dup = () -> false;
+  
+
 
 
   /** Creates a new Intake. */
@@ -111,14 +115,11 @@ public class Intake extends SubsystemBase {
       pidIn();
     }
   }
-
   
   public void handoff() {
     intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED);
   }
 
- 
-
   public void stopIntakeMotors() {
     intakeMotor.set(0);
   }
@@ -130,6 +131,19 @@ public class Intake extends SubsystemBase {
   public RelativeEncoder getEncoder() {
     return pivot.getEncoder();
   }
+
+  public boolean getForwardLimitSwitchState() {
+    return forwardLimit.isPressed();
+  }
+
+  public boolean getReverseLimitSwitchState() {
+    return reverseLimit.isPressed();
+  }
+
+  public boolean getIntakeLimitSwtichState() {
+    return intakeforwardLimit.isPressed();
+  }
+
   public void setVoltage(double voltage) {
    pivot.setVoltage(voltage);
   }
@@ -143,7 +157,7 @@ public class Intake extends SubsystemBase {
   }
 
   public void resetPosition1() {
-    if(forwardLimit.isPressed() == true) {
+    if(forwardLimit.isPressed()) {
       resetPostion();
     }
   }
@@ -157,7 +171,11 @@ public class Intake extends SubsystemBase {
   }
 
   public BooleanSupplier getArmFowardLimitState() {
-    return forwardLimit::isPressed;
+    if(forwardLimit.isPressed()) {
+      return sup;
+    } else {
+      return dup;
+    }
   }
 
   @Override

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

@@ -7,6 +7,8 @@
 
 package frc4388.robot.subsystems;
 
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -20,44 +22,69 @@ import frc4388.utility.LEDPatterns;
  */
 public class LED extends SubsystemBase {
 
-  private LEDPatterns m_currentPattern;
-  private Spark m_LEDController;
-
+  static AddressableLED m_led;
+  static AddressableLEDBuffer m_ledBuffer;
+  static LED m_self;
   /**
    * Add your docs here.
    */
-  public LED(Spark LEDController){
-    m_LEDController = LEDController;
-    setPattern(LEDConstants.DEFAULT_PATTERN);
-    updateLED();
+
+  public LED(){
+    if(m_self != null)
+      return;
+    m_led = new AddressableLED(9);
+    m_ledBuffer = new AddressableLEDBuffer(130);
+    m_led.setLength(m_ledBuffer.getLength());
+    m_led.setData(m_ledBuffer);
+    m_led.start();
     System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
   }
-
+  public static LED getInstance() {
+    if(m_self == null)
+      m_self = new LED();
+    return m_self;
+  }
   @Override
   public void periodic(){
-    SmartDashboard.putNumber("LED", m_currentPattern.getValue());
+    //gamermode();
+    //SmartDashboard.putNumber("LED", m_currentPattern.getValue());
+    return;
+  }
+  static int firstcolor = 0;
+  static void gamermode() {
+      for(int i = 0; i < m_ledBuffer.getLength(); i++) {
+          final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
+          setLEDHSV(i, hue, 255, 128);
+      }
+      firstcolor +=3;
+      firstcolor %= 180;
+  }
+  /**
+   * Add your docs here.
+   */
+  public static void updateLED (){
+    gamermode();
+   // m_LEDController.set(m_currentPattern.getValue());
   }
 
   /**
    * Add your docs here.
    */
-  public void updateLED(){
-    m_LEDController.set(m_currentPattern.getValue());
+  public static void setLEDRGB(int lednum, int r, int g, int b){
+    m_ledBuffer.setRGB(lednum, r, g, b);
+    //m_currentPattern = pattern;
+   // m_LEDController.set(m_currentPattern.getValue());
   }
-
-  /**
-   * Add your docs here.
-   */
-  public void setPattern(LEDPatterns pattern){
-    m_currentPattern = pattern;
-    m_LEDController.set(m_currentPattern.getValue());
+  public static void setLEDHSV(int lednum, int hue, int sat, int val){
+    m_ledBuffer.setRGB(lednum, hue, sat, val);
+    //m_currentPattern = pattern;
+   // m_LEDController.set(m_currentPattern.getValue());
   }
-
   /**
    * Add your docs here.
    * @return
    */
-  public LEDPatterns getPattern() {
-    return m_currentPattern;
+  public AddressableLEDBuffer getBuffer() {
+    return m_ledBuffer;
   }
 }

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

@@ -4,6 +4,8 @@
 
 package frc4388.robot.subsystems;
 
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ShooterConstants;
 
@@ -23,8 +25,8 @@ public class Shooter extends SubsystemBase {
     leftShooter  = leftTalonFX;
     rightShooter = rightTalonFX;
 
-    leftShooter.setNeutralMode(NeutralModeValue.Coast);
-    rightShooter.setNeutralMode(NeutralModeValue.Coast);
+    leftShooter.setNeutralMode(NeutralModeValue.Brake);
+    rightShooter.setNeutralMode(NeutralModeValue.Brake);
   }
 
   public void spin() {
@@ -40,9 +42,15 @@ public class Shooter extends SubsystemBase {
     spin(0.d);
   }
 
+  public void idle() {
+    spin(ShooterConstants.SHOOTER_IDLE);
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
+    SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
     
   }
 }