Merge branch 'intake-auto-reset' into intake-fixes

Author: ewpratten

src/main/java/frc/robot/RobotConstants.java

@@ -232,6 +232,9 @@ public static class Intake {
      */
     public static class Hopper {
 
+        // Reset Timer
+        public static final double RESET_TIMEOUT_SECONDS = 4.0;
+
         // Motor
         public static final int HOPPER_BELT_MOTOR = 12;
 

src/main/java/frc/robot/subsystems/cellmech/Hopper.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems.cellmech;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.lib5k.components.motors.TalonHelper;
 import frc.lib5k.components.motors.motorsensors.TalonEncoder;
@@ -18,7 +19,7 @@ public class Hopper extends SubsystemBase {
     public static Hopper s_instance = null;
     private RobotLogger logger = RobotLogger.getInstance();
 
-    private OI m_OI = OI.getInstance(); 
+    private OI m_OI = OI.getInstance();
 
     /** Motor that moves hopper belt up and down */
     private SimTalon m_hopperBelt;
@@ -74,6 +75,9 @@ private enum SystemState {
     /** amount of cells currently in hopper */
     private int m_cellCount = 0;
 
+    // Timer for reset action
+    private Timer m_resetTimer;
+
     private Hopper() {
         // Construct motor controller
         m_hopperBelt = new SimTalon(RobotConstants.Hopper.HOPPER_BELT_MOTOR);
@@ -104,11 +108,11 @@ private Hopper() {
         m_lineTopLastValue = false;
 
         // cache array so I don't have to type a bunch of stuff
-        int[][] rumbles  = RobotConstants.Hopper.HOPPER_DONE_RUMBLE_SEQUENCE;
+        int[][] rumbles = RobotConstants.Hopper.HOPPER_DONE_RUMBLE_SEQUENCE;
 
         // find length of array and create array
         int arrayLength = 0;
-        for(int i = 0; i < rumbles.length; i++) {
+        for (int i = 0; i < rumbles.length; i++) {
             arrayLength += rumbles[i][1];
         }
         m_rumbleSequence = new int[arrayLength];
@@ -117,14 +121,18 @@ private Hopper() {
         m_rumbleCounter = arrayLength;
 
         // parse 2d array of value,duration pairs into a 1d array of values
-        int i=0;
-        for(int y = 0; y < rumbles.length; y++) {
-            for(int x = 0; x < rumbles[y][1]; x++) {
+        int i = 0;
+        for (int y = 0; y < rumbles.length; y++) {
+            for (int x = 0; x < rumbles[y][1]; x++) {
                 m_rumbleSequence[i] = rumbles[y][0];
                 i++;
             }
         }
 
+        // Set up the reset timer
+        m_resetTimer = new Timer();
+        m_resetTimer.reset();
+
         // Add children
         addChild("Belt", m_hopperBelt);
         addChild("Bottom Limit", m_lineBottom);
@@ -148,13 +156,14 @@ public static Hopper getInstance() {
 
     @Override
     public void periodic() {
-        if(m_rumbleCounter < m_rumbleSequence.length) {
-            m_OI.rumbleOperator((double)m_rumbleSequence[m_rumbleCounter]);
+        if (m_rumbleCounter < m_rumbleSequence.length) {
+            m_OI.rumbleOperator((double) m_rumbleSequence[m_rumbleCounter]);
             System.out.println(m_rumbleSequence[m_rumbleCounter]);
             m_rumbleCounter++;
         }
 
-        if(m_systemState == SystemState.INTAKING || m_systemState == SystemState.INTAKEREADY || m_systemState == SystemState.UNJAM || m_systemState == SystemState.SHOOTING) {
+        if (m_systemState == SystemState.INTAKING || m_systemState == SystemState.INTAKEREADY
+                || m_systemState == SystemState.UNJAM || m_systemState == SystemState.SHOOTING) {
             // Count cells
 
             // cache values of line break sensors
@@ -256,11 +265,11 @@ private void handleIntakeReady(boolean newState) {
 
         }
         // increase counter when cell in range, reset to 0 when out of range
-        if(m_lineBottom.get()) {
+        if (m_lineBottom.get()) {
             m_intakeDelayCounter++;
 
             // if count reaches the desired amount of delay, starting intaking the cell
-            if(m_intakeDelayCounter >= RobotConstants.Hopper.CYCLES_BEFORE_INTAKE) {
+            if (m_intakeDelayCounter >= RobotConstants.Hopper.CYCLES_BEFORE_INTAKE) {
                 m_systemState = SystemState.INTAKING;
                 m_intakeDelayCounter = 0;
             }
@@ -293,8 +302,8 @@ private void handleIntaking(boolean newState) {
             m_systemState = SystemState.INTAKEREADY;
         }
 
-        // if no cells in hopper, only rely on sensors 
-        if(m_cellCount > 0) {
+        // if no cells in hopper, only rely on sensors
+        if (m_cellCount > 0) {
             // if belt has gone 12 inches, stop tying and set state to ready to intake
             if (m_ticksAtStartOfIntake - m_hopperEncoder.getTicks() >= 41583) {
                 m_systemState = SystemState.INTAKEREADY;
@@ -349,12 +358,22 @@ private void handleMoveToBottom(boolean newState) {
             // Start belt
             setBeltSpeed(-0.5);
 
+            // Reset the timer
+            m_resetTimer.reset();
+            m_resetTimer.start();
+
         }
 
         // when a cell reaches the bottom, stop
         if (m_lineBottom.get()) {
             m_systemState = SystemState.MOVEUPONEPLACE;
         }
+
+        // If our timer runs out, reset the counters, and go idle
+        if (m_resetTimer.hasPeriodPassed(RobotConstants.Hopper.RESET_TIMEOUT_SECONDS)) {
+            forceCellCount(0);
+            m_systemState = SystemState.IDLE;
+        }
     }
 
     /**
@@ -455,7 +474,7 @@ public boolean topLineBreakState() {
      */
     public void interruptShooting() {
         logger.log("Hopper", "Shooting interrupt requested");
-        if(m_cellCount > 0) {
+        if (m_cellCount > 0) {
             m_systemState = SystemState.MOVETOBOTTOM;
         }
     }