SHOOTER WORKS SEMI MANUAL

src/main/deploy/Robot Data - Distances.csv

@@ -1,4 +1,9 @@
 Distance (in),Hood Ext. (u),Drum Velocity (u/ds)
-21,3,10000
+74,20,8000
-262,30,15000
+144,23,10000
-492,30,15000
+162,28,10000
+208,29,10000
+296,32,12500
+418,33,16000
+430,31,16000
+450,31,16000

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

@@ -125,12 +125,15 @@ public final class Constants {
         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, 0.05, 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.05, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
         public static final double SHOOTER_TURRET_MIN = -1.0;
         public static final double ENCODER_TICKS_PER_REV = 2048;
         public static final double NEO_UNITS_PER_REV = 42;
         public static final double DEGREES_PER_ROT = 360;
 
+        public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
+            new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
+
         public static final int TURRET_RIGHT_SOFT_LIMIT = -2;
         public static final int TURRET_LEFT_SOFT_LIMIT = -55;
         public static final double TURRET_SPEED_MULTIPLIER = 0.3;

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

@@ -36,9 +36,12 @@ import frc4388.robot.commands.drive.DriveWithJoystick;
 import frc4388.robot.commands.drive.TurnDegrees;
 import frc4388.robot.commands.intake.RunIntakeWithTriggers;
 import frc4388.robot.commands.shooter.CalibrateShooter;
+import frc4388.robot.commands.shooter.ShootFireGroup;
 import frc4388.robot.commands.shooter.ShootFullGroup;
 import frc4388.robot.commands.shooter.ShootPrepGroup;
+import frc4388.robot.commands.shooter.TrackTarget;
 import frc4388.robot.commands.shooter.TrimShooter;
+import frc4388.robot.commands.storage.StorageIntake;
 import frc4388.robot.commands.storage.StoragePrepIntake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Climber;
@@ -119,6 +122,7 @@ 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));
         // runs the storage not
+        //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
         m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
         //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
     }
@@ -138,7 +142,6 @@ public class RobotContainer {
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
             .whenPressed(new TurnDegrees(m_robotDrive, 90));
-
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
             .whenPressed(new Wait(m_robotDrive, 0, 0));
@@ -163,15 +166,17 @@ public class RobotContainer {
         /* Operator Buttons */
         // shoots until released
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
+            //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
-            .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
+            //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
-            //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)));
+            .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage))
+            .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
 
         // shoots one ball
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
+            //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
-            .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
+            //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
-            //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)));
+            .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage))
+            .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
 
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -188,8 +193,10 @@ public class RobotContainer {
 
         // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage))
+            .whileHeld(new TrackTarget(m_robotShooterAim))
-            .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
+            .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle())))
+            //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage))
+            .whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
             //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
             //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
             //.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30)));
@@ -203,20 +210,20 @@ public class RobotContainer {
             .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
 
         //Prepares storage for intaking
-        new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER)
+        //new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER)
-            .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)))
+            //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)))
-            .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
+            //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
+            //.whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
 
         //Runs storage to outtake
-        new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER)
+       //new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER)
-            .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8)))
+            //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8)))
-            .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
+            //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
 
         //Run drum
-        //new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
+        new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
-            //.whenPressed(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage));
+            .whileHeld(new ShootFireGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
-            //.whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
+            .whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
-            //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0)));
     }
 
     /**

src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java

@@ -40,8 +40,6 @@ public class CalibrateShooter extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
-    m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
     m_shooterHood.m_angleEncoder.setPosition(0);
     m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
 
@@ -54,9 +52,6 @@ public class CalibrateShooter extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
-    m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
-
     m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
     m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
   }

src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java

@@ -31,9 +31,10 @@ public class HoodPositionPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
+    //double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
-    double b = ShooterConstants.HOOD_CONVERT_B;
+    //double b = ShooterConstants.HOOD_CONVERT_B;
-    firingAngle = (-slope*m_shooterHood.addFireAngle())+b;
+    //firingAngle = (-slope*m_shooterHood.addFireAngle())+b;
+    firingAngle = m_shooterHood.addFireAngle();
     //SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
     //SmartDashboard.putNumber("Fire Angle", firingAngle);
     m_shooterHood.runAngleAdjustPID(firingAngle);

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

@@ -29,8 +29,8 @@ public class ShootFireGroup extends ParallelRaceGroup {
     addCommands(
       new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter),
       new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood),
-      new TrackTarget(m_shooterAim),
+      new TrackTarget(m_shooterAim)
-      new StorageRun(m_storage)
+      //new StorageRun(m_storage)
     );
   }
 }

src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java

@@ -31,7 +31,6 @@ public class ShootPrepGroup extends ParallelDeadlineGroup {
       new ShooterVelocityControlPID(m_shooter),
       new HoodPositionPID(m_shooterHood),
       new StoragePrepAim(m_storage)
-      //new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())),
     );
   }
 }

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

@@ -88,15 +88,18 @@ public class TrackTarget extends CommandBase {
       double xVel = (distance * VisionConstants.GRAV) / (yVel);
 
       //fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
-      fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
+      //fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
         //END Equation Code
 
         //START CSV Code
       fireVel = m_shooter.m_shooterTable.getVelocity(distance);
-      //fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
+      fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
       //fireAngle = 33;
         //END CSV Code
 
+      //fireVel = SmartDashboard.getNumber("Velocity Target", 0);
+      //fireAngle = SmartDashboard.getNumber("Angle Target", 3);
+
       
       m_shooter.m_fireVel = fireVel;
       m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;

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

@@ -38,7 +38,7 @@ public class StorageIntake extends CommandBase {
   @Override
   public void execute() {
 
-    if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
+    if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
       m_storage.runStorage(StorageConstants.STORAGE_SPEED);
       intook = true;
     }

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

@@ -34,7 +34,7 @@ public class StoragePrepAim extends CommandBase {
   @Override
   public void execute() {
     if (m_storage.getBeam(1)){
-      m_storage.runStorage(StorageConstants.STORAGE_SPEED);
+      //m_storage.runStorage(StorageConstants.STORAGE_SPEED);
     }
     else{
       m_storage.runStorage(0);
@@ -49,12 +49,13 @@ public class StoragePrepAim extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
+    /*if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
       m_storage.m_isStorageReadyToFire = true;
       return true;
     } else {
       m_storage.m_isStorageReadyToFire = false;
       return false;
-    }
+    }*/
+    return true;
   }
 }

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

@@ -55,7 +55,7 @@ public class StoragePrepIntake extends CommandBase {
   @Override
   public boolean isFinished() {
     if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
-      return true;
+      return false;
     }
     return false;
   }

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

@@ -47,12 +47,14 @@ public class Shooter extends SubsystemBase {
     //Testing purposes reseting gyros
     //resetGyroAngleAdj();
     shooterTrims = new Trims(0, 0);
+    //SmartDashboard.putNumber("Velocity Target", 10000);
+    //SmartDashboard.putNumber("Angle Target", 3);
 
     m_shooterFalcon.configFactoryDefault();
     m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
     m_shooterFalcon.setInverted(true);
     m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.configClosedloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.configClosedloopRamp(1.0, ShooterConstants.SHOOTER_TIMEOUT_MS);
     setShooterGains();
 
     m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -64,6 +66,8 @@ public class Shooter extends SubsystemBase {
 
     m_shooterTable = new ShooterTables();
 
+    m_shooterFalcon.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
+
     //SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10));
     //SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200));
     //SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250));
@@ -83,6 +87,10 @@ public class Shooter extends SubsystemBase {
 
     SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
 
+    SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature());
+
+    SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
+
     //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition());
     }
 
@@ -129,7 +137,7 @@ public class Shooter extends SubsystemBase {
    * @param targetVel Target velocity to run motor at
    */
   public void runDrumShooterVelocityPID(double targetVel) {
-    System.out.println("Target Velocity" + targetVel);
+    //System.out.println("Target Velocity" + targetVel);
     m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
   }
 }

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

@@ -50,6 +50,8 @@ public class ShooterAim extends SubsystemBase {
     m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
     m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT);
     m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT);
+
+    m_shooterRotateMotor.setInverted(false);
   }
 
   @Override

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

@@ -48,10 +48,10 @@ public class ShooterHood extends SubsystemBase {
     m_hoodUpLimit.enableLimitSwitch(true);
     m_hoodDownLimit.enableLimitSwitch(true);
 
-    m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
+    //m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
-    m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+    //m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
-    m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
+    //m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
-    m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
+    //m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
   }
 
   @Override