Merge pull request #226 from strykeforce/rkalnins/hotfix-v19.2.0

Rkalnins/hotfix v19.2.0

Author: mwitcpalek

README.md

@@ -2,7 +2,7 @@
 
 ## Talons
 
-Subsystem    | Talon   | ID | PDP
+Subsystem    | Talon       | ID | PDP
 ------------ | ----------- | -- | ---
 Drive        | azimuth     | 0  | 11
 Drive        | azimuth     | 1  | 9

nt/networktables-airtight-2.ini renamed to nt/airtight-4-11.ini

@@ -79,12 +79,14 @@ public void robotInit() {
 
     SmartDashboard.putBoolean("Game/SandstormPickUp", false);
     SmartDashboard.putBoolean("Game/haveHatch", false);
-
+    SmartDashboard.putBoolean("Game/climbOnTarget", false);
+    SmartDashboard.putBoolean("Game/climbPrecheck", false);
     //    new SmartDashboardControls();
   }
 
   @Override
   public void autonomousInit() {
+    BISCUIT.setPosition(BISCUIT.getPosition());
     DRIVE.setAngleAdjustment(true);
     getHatch.start();
   }

src/main/java/frc/team2767/deepspace/Robot.java

@@ -33,12 +33,9 @@ protected void initialize() {
     climbState = ClimbState.FORM_SEAL;
     CLIMB.setVelocity(ClimbSubsystem.kSealOutputVelocity);
     logger.info("forming seal");
-    //    CLIMB.setSlowTalonConfig(false);
-    //    climbState = ClimbState.FAST_LOWER;
-    //    CLIMB.setLowerLimit(ClimbSubsystem.kHabHover);
-    //    CLIMB.setVelocity(ClimbSubsystem.kDownVelocity);
   }
 
+  @SuppressWarnings("Duplicates")
   @Override
   protected void execute() {
     switch (climbState) {
@@ -59,18 +56,36 @@ protected void execute() {
           CLIMB.enableRatchet();
           CLIMB.releaseKickstand();
           VISION.startLightBlink(VisionSubsystem.LightPattern.CLIMB_GOOD);
-          //          CLIMB.setLowerLimit(ClimbSubsystem.kClimb);
           CLIMB.setVelocity(ClimbSubsystem.kDownClimbVelocity);
+          break;
         }
 
         if (CLIMB.getStringPotPosition() >= ClimbSubsystem.kTooLowIn - GOOD_ENOUGH) {
           logger.info("resetting");
+          if (VACUUM.isClimbPrecheck()) {
+            climbState = ClimbState.PRECHECK;
+            logger.debug("going to precheck");
+            CLIMB.stop();
+            break;
+          }
           CLIMB.setSlowTalonConfig(false);
           climbState = ClimbState.RESET;
-          //          CLIMB.setUpperLimit(ClimbSubsystem.kHabHover);
           CLIMB.setVelocity(ClimbSubsystem.kUpVelocity);
+
+          break;
         }
 
+        break;
+      case PRECHECK:
+        if (VACUUM.isClimbOnTarget()) {
+          logger.info("fast climbing");
+          CLIMB.setSlowTalonConfig(false);
+          climbState = ClimbState.FAST_CLIMB;
+          CLIMB.enableRatchet();
+          CLIMB.releaseKickstand();
+          VISION.startLightBlink(VisionSubsystem.LightPattern.CLIMB_GOOD);
+          CLIMB.setVelocity(ClimbSubsystem.kDownClimbVelocity);
+        }
         break;
       case FAST_CLIMB:
         if (CLIMB.getStringPotPosition() >= ClimbSubsystem.kClimb) {
@@ -86,11 +101,23 @@ protected void execute() {
         }
         break;
       case RESET:
+        if (VACUUM.isClimbOnTarget()) {
+          logger.info("fast climbing during reset");
+          CLIMB.setSlowTalonConfig(false);
+          climbState = ClimbState.FAST_CLIMB;
+          CLIMB.enableRatchet();
+          CLIMB.releaseKickstand();
+          VISION.startLightBlink(VisionSubsystem.LightPattern.CLIMB_GOOD);
+          CLIMB.setVelocity(ClimbSubsystem.kDownClimbVelocity);
+          break;
+        }
+
         if (CLIMB.getStringPotPosition() <= ClimbSubsystem.kHabHover + GOOD_ENOUGH) {
           climbState = ClimbState.PAUSE;
           CLIMB.stop();
           pauseInitTime = Timer.getFPGATimestamp();
         }
+
         break;
     }
   }
@@ -110,6 +137,7 @@ private enum ClimbState {
     FAST_LOWER,
     FORM_SEAL,
     RESET,
+    PRECHECK,
     PAUSE,
     FAST_CLIMB,
     DONE

src/main/java/frc/team2767/deepspace/command/climb/ClimbAutoCommand.java

@@ -3,12 +3,14 @@
 import edu.wpi.first.wpilibj.command.Command;
 import frc.team2767.deepspace.Robot;
 import frc.team2767.deepspace.subsystem.ClimbSubsystem;
+import frc.team2767.deepspace.subsystem.VacuumSubsystem;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 
 public class ClimbDeployCommand extends Command {
 
   private static final ClimbSubsystem CLIMB = Robot.CLIMB;
+  private static final VacuumSubsystem VACUUM = Robot.VACUUM;
   private static final int GOOD_ENOUGH = 5;
   private State currentState;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
@@ -32,7 +34,6 @@ protected void execute() {
     switch (currentState) {
       case RELEASE:
         if (CLIMB.getStringPotPosition() >= ClimbSubsystem.kLowRelease - GOOD_ENOUGH) {
-          //          CLIMB.setUpperLimit(ClimbSubsystem.kHighRelease);
           CLIMB.openLoop(-1.0); // kUpVel
           currentState = State.POSITION;
           logger.debug("released, climber position = {}", CLIMB.getStringPotPosition());

src/main/java/frc/team2767/deepspace/command/climb/ClimbDeployCommand.java

@@ -1,14 +1,16 @@
 package frc.team2767.deepspace.command.climb;
 
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj.command.Command;
 import frc.team2767.deepspace.Robot;
 import frc.team2767.deepspace.subsystem.ClimbSubsystem;
+import frc.team2767.deepspace.subsystem.VacuumSubsystem;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 
-public class ClimbJogCommand extends InstantCommand {
+public class ClimbJogCommand extends Command {
 
   private static final ClimbSubsystem CLIMB = Robot.CLIMB;
+  private static final VacuumSubsystem VACUUM = Robot.VACUUM;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private double percent;
 
@@ -21,4 +23,14 @@ protected void initialize() {
     logger.info("open loop climb at {}", percent);
     CLIMB.openLoop(percent);
   }
+
+  @Override
+  protected boolean isFinished() {
+    return CLIMB.getStringPotPosition() < (ClimbSubsystem.kHighRelease - 10);
+  }
+
+  @Override
+  protected void end() {
+    CLIMB.stop();
+  }
 }

src/main/java/frc/team2767/deepspace/command/climb/ClimbJogCommand.java

@@ -0,0 +1,35 @@
+package frc.team2767.deepspace.command.climb;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.team2767.deepspace.Robot;
+import frc.team2767.deepspace.subsystem.ClimbSubsystem;
+import frc.team2767.deepspace.subsystem.VacuumSubsystem;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+public class ClimbSlowVelocityCommand extends Command {
+  private static final VacuumSubsystem VACUUM = Robot.VACUUM;
+  private static final ClimbSubsystem CLIMB = Robot.CLIMB;
+  private final Logger logger = LoggerFactory.getLogger(this.getClass());
+
+  public ClimbSlowVelocityCommand() {
+    requires(CLIMB);
+  }
+
+  @Override
+  protected void initialize() {
+    logger.info("Manual Lower Suction Cup Slow Velocity");
+    CLIMB.setSlowTalonConfig(true);
+    CLIMB.setVelocity(ClimbSubsystem.kSealOutputVelocity);
+  }
+
+  public void execute() {
+    SmartDashboard.putBoolean("Game/climbOnTarget", VACUUM.isClimbOnTarget());
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/team2767/deepspace/command/climb/ClimbSlowVelocityCommand.java

@@ -10,7 +10,7 @@ public class RollerInCommand extends InstantCommand {
   private final double output;
 
   public RollerInCommand() {
-    this(0.8);
+    this(0.9);
   }
 
   public RollerInCommand(double output) {

src/main/java/frc/team2767/deepspace/command/intake/RollerInCommand.java

@@ -10,7 +10,7 @@ public class RollerOutCommand extends InstantCommand {
   private final double output;
 
   public RollerOutCommand() {
-    this(0.8);
+    this(0.9);
   }
 
   public RollerOutCommand(double output) {

src/main/java/frc/team2767/deepspace/command/intake/RollerOutCommand.java

@@ -5,11 +5,10 @@
 import frc.team2767.deepspace.subsystem.IntakeSubsystem;
 
 public class WaitForBeamBreakCommand extends Command {
+  private static final double WAIT_TIME_MS = 300;
+  private double breakTime;
+  private boolean hasBroken;
   private final IntakeSubsystem INTAKE = Robot.INTAKE;
-  private static boolean hasBroken;
-  private static double breakTime;
-  private static double currentTime;
-  private static double WAIT_TIME_MS = 200;
 
   @Override
   protected void initialize() {
@@ -26,8 +25,7 @@ protected void execute() {
 
   @Override
   protected boolean isFinished() {
-    currentTime = System.currentTimeMillis();
-    return hasBroken && (currentTime - breakTime > WAIT_TIME_MS);
+    return hasBroken && (System.currentTimeMillis() - breakTime > WAIT_TIME_MS);
   }
 
   @Override