Messing with Turret and storage

Not completly sure why the shooting actions arent happening in the right order

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

@@ -120,7 +120,7 @@ public final class Constants {
         public static final int SHOOTER_PID_LOOP_IDX = 1;
         public static final int SHOOTER_TIMEOUT_MS = 30;
         //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
-        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0, 0.0453, 0, 1.0);
+        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.2, 0.0, 0, 0.0453, 0, 1.0);
         public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
         public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
         public static final double SHOOTER_TURRET_MIN = -1.0;
@@ -130,7 +130,7 @@ public final class Constants {
     }
     
     public static final class ClimberConstants {
-        public static final int CLIMBER_SPARK_ID = 10;
+        public static final int CLIMBER_SPARK_ID = -10;
     }
   
     public static final class LevelerConstants {
@@ -138,7 +138,7 @@ public final class Constants {
     }
   
     public static final class StorageConstants {
-        public static final int STORAGE_CAN_ID = -1;
+        public static final int STORAGE_CAN_ID = 11;
         public static final double STORAGE_PARTIAL_BALL = 2;
         public static final double STORAGE_FULL_BALL = 7;
         public static final double STORAGE_SPEED = 0.5;
@@ -159,7 +159,7 @@ public final class Constants {
 
         /* PID Gains */
         public static final double STORAGE_MIN_OUTPUT = -1.0;
-        public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains STORAGE_GAINS = new Gains(1, 0.0, 0.0, 0.0, 0, 1.0);
     }
   
     public static final class LEDConstants {

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

@@ -41,7 +41,6 @@ import frc4388.robot.commands.StorageIntake;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
 import frc4388.robot.commands.RunIntakeWithTriggers;
-import frc4388.robot.commands.StorageIntakeGroup;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
@@ -105,7 +104,7 @@ public class RobotContainer {
         // drives intake with input from triggers on the opperator controller
         m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
         // runs the turret with joystick
-        m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
+        m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
         // moves the drum not
         m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter));
         // drives climber with input from triggers on the opperator controller
@@ -114,6 +113,8 @@ public class RobotContainer {
         m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+        //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
+        //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
     }
 
     /**
@@ -149,15 +150,18 @@ public class RobotContainer {
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
 
+
         /* Operator Buttons */
 
         // shoots until released
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false);
+            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
 
         // shoots one ball
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false);
+            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
 
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -171,7 +175,9 @@ public class RobotContainer {
         // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
+            //.whileHeld(new TrackTarget(m_robotShooter,m_robotShooterAim))
             .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
+            //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
 
         //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
@@ -182,10 +188,12 @@ public class RobotContainer {
             .whileHeld(new StorageOutake(m_robotStorage));
 
         new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
-            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage));
+            .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.2)))
+            .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
 
         new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity())));
+            .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.2)))
+            .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
     }
 
     /**

src/main/java/frc4388/robot/commands/HoldTarget.java

@@ -49,7 +49,8 @@ public class HoldTarget extends CommandBase {
   @Override
   public void initialize() {
       //Vision Processing Mode
-    LimeLight.limeOn();
+      NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+      NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
 
 
@@ -88,8 +89,7 @@ public class HoldTarget extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-      //Drive Camera Mode
-    LimeLight.limeOff();
+
   }
 
   // Returns true when the command should end.

src/main/java/frc4388/robot/commands/ShootFireGroup.java

@@ -28,7 +28,7 @@ public class ShootFireGroup extends ParallelRaceGroup {
       new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
       new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
       new HoldTarget(m_shooter, m_shooterAim),
-      new StorageRun(m_storage)
+      new StoragePositionPID(m_storage)
     );
   }
 }

src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.Shooter;
 
@@ -35,6 +36,8 @@ public class ShooterVelocityControlPID extends CommandBase {
   public void execute() {
     m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
     m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
+    SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
+    SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
   }
 
   // Called once the command ends or is interrupted.
@@ -49,9 +52,11 @@ public class ShooterVelocityControlPID extends CommandBase {
     double upperBound = m_targetVel + 300;
     double lowerBound = m_targetVel - 300;
     if (m_actualVel < upperBound && m_actualVel > lowerBound){
+      SmartDashboard.putBoolean("ShooterVelocityPID Finished", true);
       return true;
     }
     else{
+      SmartDashboard.putBoolean("ShooterVelocityPID Finished", false);
       return false;
     }
   }

src/main/java/frc4388/robot/commands/StorageIntake.java

@@ -37,7 +37,7 @@ public class StorageIntake extends CommandBase {
   @Override
   public void execute() {
 
-    if (m_storage.getBeam(0)){
+    if (!m_storage.getBeam(0)){
       m_storage.runStorage(StorageConstants.STORAGE_SPEED);
       intook = true;
     }
@@ -54,7 +54,7 @@ public class StorageIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeam(0) && intook){
+    if (m_storage.getBeam(0) && intook){
       return true;
     }
     return false;

src/main/java/frc4388/robot/commands/StoragePositionPID.java

@@ -8,30 +8,31 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
 
-
 public class StoragePositionPID extends CommandBase {
-  Storage m_storage;
+  public Storage m_storage;
+  double startPos;
   /**
    * Creates a new StoragePositionPID.
    */
-  public StoragePositionPID(Storage storage) {
-    m_storage = storage;
+  public StoragePositionPID(Storage subsystem) {
+    m_storage = subsystem;
     addRequirements(m_storage);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    startPos = m_storage.getEncoderPos();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    new RunCommand(() -> m_storage.runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL));
+    System.err.println("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooiujgxzxfghjkiujsdasdgioiedsdjkl");
+    m_storage.runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL);
   }
 
   // Called once the command ends or is interrupted.
@@ -42,6 +43,10 @@ public class StoragePositionPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
+    if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos())
+    {
+      return true;
+    }
     return false;
   }
 }

src/main/java/frc4388/robot/commands/StoragePrepAim.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
@@ -29,7 +30,7 @@ public class StoragePrepAim extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (m_storage.getBeam(1) == false){
+    if (m_storage.getBeam(1)){
       m_storage.runStorage(StorageConstants.STORAGE_SPEED);
     }
     else{
@@ -45,9 +46,11 @@ public class StoragePrepAim extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(1)){
+    if (!m_storage.getBeam(1)){
+      SmartDashboard.putBoolean("StoragePrepAim Finished", true);
       return true;
     }
+    SmartDashboard.putBoolean("StoragePrepAim Finished", false);
     return false;
   }
 }

src/main/java/frc4388/robot/commands/StoragePrepIntake.java

@@ -36,7 +36,8 @@ public class StoragePrepIntake extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (!m_storage.getBeam(0) && System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed
+    System.err.println(m_storage.getBeam(0));
+    if (m_storage.getBeam(0)){//&& System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed
       m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
     }
     else{
@@ -52,7 +53,7 @@ public class StoragePrepIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(0)){
+    if (!m_storage.getBeam(0)){
       return true;
     }
     return false;

src/main/java/frc4388/robot/commands/TrackTarget.java

@@ -51,7 +51,8 @@ public class TrackTarget extends CommandBase {
   @Override
   public void initialize() {
     // Vision Processing Mode
-    LimeLight.limeOn();
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -93,17 +94,18 @@ public class TrackTarget extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    // Drive Camera Mode
-    LimeLight.limeOff();
+
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (xAngle < 1 && xAngle > -1)
+    if (xAngle < 1 && xAngle > -1 && target == 1)
     {
+      SmartDashboard.putBoolean("TrackTarget Finished", true);
       return true;
     }
+    SmartDashboard.putBoolean("TrackTarget Finished", false);
     return false;
   }
 }

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

@@ -110,7 +110,7 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
     m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
     m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
-    m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
+   // m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
 
     /* Config Open Loop Ramp so we don't make sudden output changes */
     m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);

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

@@ -18,12 +18,12 @@ public class LimeLight extends SubsystemBase {
 
   }
 
-  public static void limeOff(){
+  public void limeOff(){
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
   }
   
-  public static void limeOn(){
+  public void limeOn(){
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }

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

@@ -13,9 +13,11 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANDigitalInput;
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANPIDController;
 import com.revrobotics.ControlType;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.Joystick;
@@ -48,6 +50,7 @@ public class Shooter extends SubsystemBase {
   public boolean velReached;
   public double m_fireVel;
   public double m_fireAngle;
+  CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
   
   /*
    * Creates a new Shooter subsystem.
@@ -81,6 +84,13 @@ public class Shooter extends SubsystemBase {
     SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10));
     SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
     SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));
+
+
+
+    m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_hoodUpLimit.enableLimitSwitch(true);
+    m_hoodDownLimit.enableLimitSwitch(true);
   }
 
   @Override

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

@@ -7,10 +7,12 @@
 
 package frc4388.robot.subsystems;
 
+import com.revrobotics.CANDigitalInput;
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANPIDController;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.ControlType;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
@@ -22,6 +24,7 @@ import frc4388.robot.Constants.ShooterConstants;
 public class ShooterAim extends SubsystemBase {
   public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
   public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
+  CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit;
 
     // Configure PID Controllers
     CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
@@ -32,10 +35,15 @@ public class ShooterAim extends SubsystemBase {
   public ShooterAim() {
     resetGyroShooterRotate();
     m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
+
+    m_shooterRightLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_shooterLeftLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_shooterRightLimit.enableLimitSwitch(true);
+    m_shooterLeftLimit.enableLimitSwitch(true);
   }
 
   public void runShooterWithInput(double input) {
-    m_shooterRotateMotor.set(input);
+    m_shooterRotateMotor.set(input/3);
   }
 
 

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

@@ -20,12 +20,13 @@ import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.command.WaitUntilCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
 import frc4388.robot.Constants.StorageConstants;
 
 public class Storage extends SubsystemBase {
-  private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
+  public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
   private DigitalInput[] m_beamSensors = new DigitalInput[6];
 
   CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
@@ -69,6 +70,11 @@ public class Storage extends SubsystemBase {
     m_encoder.setPosition(0);
   }
 
+  public void testBeams(){
+    SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
+    SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
+  }
+
   /* Storage PID Control */
   public void runStoragePositionPID(double targetPos){
     // Set PID Coefficients
@@ -79,6 +85,8 @@ public class Storage extends SubsystemBase {
     m_storagePIDController.setFF(storageGains.m_kF);
     m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput);
 
+    SmartDashboard.putNumber("Storage Position PID Target", targetPos);
+    SmartDashboard.putNumber("Storage Position Pos", getEncoderPos());
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }