2-ball Internals

src/main/java/frc/robot/RobotContainer.java

@@ -141,7 +141,7 @@ private void controllerBindings() {
         mechAButton.whenPressed(new RequestShotCommand(internalSubsystem));
         mechXButton.whenPressed(new InstantCommand(() -> internalSubsystem.setPower(0)));
 
-        mechYButton.whenPressed(new InstantCommand(() -> turretSubsystem.setFlywheelPower(0.8)));
+        mechYButton.whenPressed(new InstantCommand(() -> turretSubsystem.setFlywheelPower(0.2)));
         mechBButton.whenPressed(new InstantCommand(() -> turretSubsystem.setFlywheelPower(0)));
 
         if (tankSubsystem != null) {

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -27,7 +27,7 @@ public class IntakeSubsystem extends GRTSubsystem {
      * counterclockwise angle from straight upwards. In degrees.
      */
     public enum IntakePosition {
-        RAISED(0), DEPLOYED(465469);
+        RAISED(0), DEPLOYED(200000);
 
         public final double value;
 
@@ -83,6 +83,14 @@ public IntakeSubsystem(InternalSubsystem internalSubsystem /* , JetsonConnection
         deploy.config_kI(0, kI);
         deploy.config_kD(0, kD);
 
+        // Soft limit deploy between RAISED and DEPLOYED
+        /*
+        deploy.configForwardSoftLimitEnable(true);
+        deploy.configReverseSoftLimitEnable(true);
+        deploy.configForwardSoftLimitThreshold(IntakePosition.RAISED.value);
+        deploy.configReverseSoftLimitThreshold(IntakePosition.DEPLOYED.value);
+        */
+
         // Initialize Shuffleboard entries
         shuffleboardTab = Shuffleboard.getTab("Intake");
         shuffleboardPEntry = shuffleboardTab.add("kP", kP).getEntry();
@@ -98,11 +106,9 @@ public IntakeSubsystem(InternalSubsystem internalSubsystem /* , JetsonConnection
     @Override
     public void periodic() {
         // Get PID constants from Shuffleboard for testing
-
         deploy.config_kP(0, shuffleboardPEntry.getDouble(kP));
         deploy.config_kI(0, shuffleboardIEntry.getDouble(kI));
         deploy.config_kD(0, shuffleboardDEntry.getDouble(kD));
-
 
         // If the jetson detects a ball or the driver is running the intake, the intake
         // is deployed,

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -44,13 +44,15 @@ public enum TurretMode {
 
     /**
      * Represents the state of a turret module (flywheel, turntable, hood).
-     * If a module is READY, it is completely ready to shoot.
-     * If a module is ALMOST, it is nearly ready and will become ready after a robot stop.
+     * If a module is in HIGH_TOLERANCE, it is completely ready to shoot.
+     * If a module is in LOW_TOLERANCE, it is nearly ready and will become ready after a robot stop.
+     * This state is for rejecting, which doesn't need a perfectly lined up shot but needs better than completely
+     * unaligned. It is also for drivers to know when the turret is almost ready
      * If a module is UNALIGNED, it is not ready; it will require more than a second to get it to READY status. 
      * For turntable, this includes being in the blind spot.
      */
     public enum ModuleState {
-        READY, ALMOST, UNALIGNED
+        HIGH_TOLERANCE, LOW_TOLERANCE, UNALIGNED
     }
 
     public boolean spin = false;
@@ -262,7 +264,7 @@ public void periodic() {
         // hood.set(ControlMode.Position, desiredHoodAngle);
         // turntable.set(ControlMode.Position, Math.max(Math.min(desiredTurntablePosition, TURNTABLE_MAX_POS), TURNTABLE_MIN_POS));
 
-        System.out.println("Turret RPM: " + flywheelEncoder.getVelocity());
+        //System.out.println("Turret RPM: " + flywheelEncoder.getVelocity());
     }
 
     /**
@@ -283,8 +285,8 @@ public void setReject(boolean reject) {
     private ModuleState flywheelReady() {
         // TODO: test thresholding values
         double diff = Math.abs(flywheelEncoder.getVelocity() - desiredFlywheelSpeed);
-        return diff < 10 ? ModuleState.READY
-            : diff < 20 ? ModuleState.ALMOST
+        return diff < 10 ? ModuleState.HIGH_TOLERANCE
+            : diff < 20 ? ModuleState.LOW_TOLERANCE
             : ModuleState.UNALIGNED;
     }
 
@@ -293,14 +295,14 @@ private ModuleState flywheelReady() {
      * @return The state of the turntable.
      */
     private ModuleState turntableAligned() {
-        // If the calculated theta is in the blind spot, return the RED state
+        // If the calculated theta is in the blind spot, return UNALIGNED
         if (desiredTurntablePosition < TURNTABLE_MIN_POS || desiredTurntablePosition > TURNTABLE_MAX_POS)
             return ModuleState.UNALIGNED;
 
         // TODO: test thresholding values
         double diff = Math.abs(turntable.getSelectedSensorPosition() - desiredTurntablePosition);
-        return diff < 10 ? ModuleState.READY
-            : diff < 20 ? ModuleState.ALMOST
+        return diff < 10 ? ModuleState.HIGH_TOLERANCE
+            : diff < 20 ? ModuleState.LOW_TOLERANCE
             : ModuleState.UNALIGNED;
     }
 
@@ -311,15 +313,15 @@ private ModuleState turntableAligned() {
     private ModuleState hoodReady() {
         // TODO: test thresholding values
         double diff = Math.abs(hood.getSelectedSensorPosition() - desiredHoodAngle);
-        return diff < 5 ? ModuleState.READY
-            : diff < 10 ? ModuleState.ALMOST
+        return diff < 5 ? ModuleState.HIGH_TOLERANCE
+            : diff < 10 ? ModuleState.LOW_TOLERANCE
             : ModuleState.UNALIGNED;
     }
 
     /**
      * Gets the current state of the turret by taking the lowest state of all of its modules.
-     * Ex: UNALIGNED, ALMOST, READY -> UNALIGNED
-     * Ex. ALMOST, ALMOST, READY -> ALMOST
+     * Ex: UNALIGNED, LOW_TOLERANCE, HIGH_TOLERANCE -> UNALIGNED
+     * Ex. LOW_TOLERANCE, LOW_TOLERANCE, HIGH_TOLERANCE -> LOW_TOLERANCE
      * @return The state of the turret.
      */
     public ModuleState getState() {
@@ -329,8 +331,8 @@ public ModuleState getState() {
 
         // TODO: is there a better way to implement this?
         return flywheelState == ModuleState.UNALIGNED || turntableState == ModuleState.UNALIGNED || hoodState == ModuleState.UNALIGNED ? ModuleState.UNALIGNED
-            : flywheelState == ModuleState.ALMOST || turntableState == ModuleState.ALMOST || hoodState == ModuleState.ALMOST ? ModuleState.ALMOST
-            : ModuleState.READY;
+            : flywheelState == ModuleState.LOW_TOLERANCE || turntableState == ModuleState.LOW_TOLERANCE || hoodState == ModuleState.LOW_TOLERANCE ? ModuleState.LOW_TOLERANCE
+            : ModuleState.HIGH_TOLERANCE;
     }
 
     /**

src/main/java/frc/robot/subsystems/internals/InternalSubsystem.java

@@ -40,13 +40,23 @@ public class InternalSubsystem extends GRTSubsystem {
 
     private final ColorMatch colorMatcher;
     private final Timer exitTimer;
+    private final Timer entranceTimer;
+    private final Timer storageTimer;
+
+    // Subsystem ball states
+    private int entranceStorageBallCount = 0;
+    private int storageStagingBallCount = 0;
+    private int stagingExitBallCount = 0;
+    private int totalBallCount = 0;
+
+    private boolean prevEntranceDetected = false;
+   // private boolean prevStorageDetected = false;
+   // private boolean prevStagingDetected = false;
 
     private boolean shotRequested = false;
     private boolean rejecting = false;
     private boolean rejectingChecked = false;
-    private int ballCount = 0;
 
-    private boolean prevEntranceDetected = false;
     private boolean driverOverride = false;
 
     public InternalSubsystem(TurretSubsystem turretSubsystem) {
@@ -81,93 +91,131 @@ public InternalSubsystem(TurretSubsystem turretSubsystem) {
         colorMatcher.setConfidenceThreshold(0.8);
 
         exitTimer = new Timer();
+        entranceTimer = new Timer();
+        storageTimer = new Timer();
 
         // Set alliance color from FMS
         switch (DriverStation.getAlliance()) {
             case Red: ALLIANCE_COLOR = RED; break;
             case Blue: ALLIANCE_COLOR = BLUE; break;
             default: ALLIANCE_COLOR = RED; break;
         }
+
+        // if we are in auton we start with 1 ball
+        if (DriverStation.isAutonomous()) {
+            totalBallCount = 1;
+        }
     }
 
     @Override
     public void periodic() {
-        // Get last detected storage color from the sensor thread and check IR sensors for balls.
+
+        boolean entranceDetected = entrance.get() >= 0.4;
+        if (!prevEntranceDetected && entranceDetected) totalBallCount++;
+        prevEntranceDetected = entranceDetected;
+
         Color storageColor = matchColor(colorSensorThread.getLastStorage());
-        //boolean entranceDetected = entrance.get() >= 0.4;
-        //boolean stagingDetected = staging.get() >= 0.2;
+        boolean storageDetected = isBall(storageColor);
 
-        double e = entrance.get();
-        double s = staging.get();
-        boolean entranceDetected = e >= 0.4;
-        boolean stagingDetected = s >= 0.2;
+        boolean stagingDetected = staging.get() >= 0.13;
 
-        /*
-        System.out.println("Entrance: " + e);
-        System.out.println("Staging: " + s);
+        // Testing prints, Y = ball detected, X = no ball
+        System.out.println("Entrance: " + (prevEntranceDetected ? "Y" : "X") + 
+                            ", Storage: " + (storageDetected ? "Y" : "X") + 
+                            ", Staging: " + (stagingDetected ? "Y" : "X"));
 
-        System.out.println("STORAGE matched " + (
-            storageColor == RED ? "RED" :
-            storageColor == BLUE ? "BLUE" :
-            storageColor == EMPTY ? "EMPTY" :
-            "null"));
-        */
 
         if (driverOverride) return;
 
-        // Check the entrance sensor for incoming balls and update the ball count accordingly
-        if (!prevEntranceDetected && entranceDetected) ballCount++;
-        prevEntranceDetected = entranceDetected;
+        // If there is a ball in the entrance, run the bottom motor.
+        //old condition: (entranceStorageBallCount > 0 && storageStagingBallCount == 0)
+        if (prevEntranceDetected && !storageDetected) {
+            // Spin the bottom motor on a timer
+            entranceTimer.start();
+            motorBottom.set(0.3);
+        }
+        // If 1.5 second has elapsed, stop motor
+        // TODO this may need more logic to make sure we actually get the ball in, 
+        // could use storage color sensor to detect if it got caught
+        if (entranceTimer.hasElapsed(1.5)) {
+            entranceTimer.stop();
+            entranceTimer.reset();
+            motorBottom.set(0);
 
-        // If a ball has entered internals, start the bottom motor
-        if (entranceDetected) motorBottom.set(0.3);
+            prevEntranceDetected = false;
+            //entranceStorageBallCount--;
+            //storageStagingBallCount++;
+            System.out.println("ball moved from entrance to storage");
+        }
+
+        // If there is a ball between storage and staging and staging is empty, run the top and bottom motors
+        //old condition: (storageStagingBallCount > 0 && stagingExitBallCount == 0)
+         if (storageDetected && !stagingDetected && !shotRequested) {
+            // Spin the bottom and top motors on a timer
+            storageTimer.start();
+            motorTop.set(0.5);
+            motorBottom.set(0.3);
 
-        // If there is a ball in storage, start the top motor
-        if (isBall(storageColor)) {
-            // If we haven't already checked rejection logic, reject the ball if it doesn't match alliance color
             if (!rejectingChecked) {
                 rejecting = storageColor != ALLIANCE_COLOR;
                 rejectingChecked = true;
             }
-            motorTop.set(0.5);
-            //System.out.println("STORAGE detected, running top motor");
+        }
+         // If 0.5 seconds have elapsed, stop the motors
+         if (storageTimer.hasElapsed(0.5)) {
+            storageTimer.stop();
+            storageTimer.reset();
+            motorTop.set(0);
+            motorBottom.set(0);
+
+            //storageStagingBallCount--;
+            //stagingExitBallCount++;
+            System.out.println("ball moved from storage to staging");
         }
 
-        // If there is a ball in staging, stop the bottom and top motors
+        // If there is a ball in staging, we don't want to push it into turret, especially if there is a shot going
         if (stagingDetected) {
-            motorBottom.set(0);
             motorTop.set(0);
-            //System.out.println("STAGING detected, stopping both motors");
         }
 
         if (turretSubsystem != null) {
             // If a shot was requested and the turret is ready, load a ball into the turret.
             // If rejecting, the turret can be in a semi-aligned state; otherwise, require it to be fully lined up.
             turretSubsystem.setReject(rejecting);
-            if (shotRequested /* && turretSubsystem.getState() == TurretSubsystem.ModuleState.READY
-                 || rejecting && turretSubsystem.getState() == TurretSubsystem.ModuleState.ALMOST */) {
+            if (shotRequested /* && turretSubsystem.getState() == TurretSubsystem.ModuleState.HIGH_TOLERANCE
+                 || rejecting && turretSubsystem.getState() == TurretSubsystem.ModuleState.LOW_TOLERANCE */) {
                 // Spin the top motor on a timer
-                exitTimer.start();
-                motorTop.set(0.5);
-                //turretSubsystem.setFlywheelPower(0.2);
-                //System.out.println("Shot requested, processing");
-
-                // If 1.5 seconds have elapsed, mark the shot as finished and decrement the ball count.
-                if (exitTimer.hasElapsed(1.5)) {
-                    exitTimer.stop();
-                    exitTimer.reset();
+
+                    exitTimer.start();
+                    motorTop.set(0.5);
+
+                    // If 1.5 seconds have elapsed, mark the shot as finished
+                    if (exitTimer.hasElapsed(1)) {
+                        exitTimer.stop();
+                        exitTimer.reset();
+                        motorTop.set(0);
+
+                        // Reset states
+                        shotRequested = false;
+                        rejectingChecked = false;
+                        //stagingExitBallCount--;
+                        totalBallCount--;
+                        System.out.println("ball exited turret");
+                    }  
+            }
+        }
 
-                    motorTop.set(0);
-                    //turretSubsystem.setFlywheelPower(0);
+        entranceStorageBallCount = prevEntranceDetected ? 1 : 0;
+        storageStagingBallCount = storageDetected ? 1 : 0;
+        stagingExitBallCount = stagingDetected ? 1 : 0;
 
-                    shotRequested = false;
-                    rejectingChecked = false;
-                    ballCount--;
+        // This is really just a different version of our other ball/no ball print so 
+        //it's not *really* needed unless we want to implement ball-counting, though in that 
+        //case we just change that to this.
+        System.out.println("Entrance: " + entranceStorageBallCount + 
+                            " Storage: " + storageStagingBallCount +
+                            " Staging: " + stagingExitBallCount);
 
-                    //System.out.println("Shot finished, terminating loop");
-                }
-            }
-        }
     }
 
     /**
@@ -188,11 +236,11 @@ public void requestShot() {
     }
 
     /**
-     * Gets how many balls are inside internals.
+     * Gets how many total balls are inside internals.
      * @return The current ball count.
      */
     public int getBallCount() {
-        return ballCount;
+        return entranceStorageBallCount + storageStagingBallCount + stagingExitBallCount;
     }
 
     /**