Merge pull request #207 from strykeforce/rkalnins/hatch-place

Rkalnins/hatch place

Author: mwitcpalek

src/main/java/frc/team2767/deepspace/command/TeleOpDriveCommand.java

@@ -8,6 +8,7 @@
 import frc.team2767.deepspace.subsystem.DriveSubsystem;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.thirdcoast.util.ExpoScale;
 
 public final class TeleOpDriveCommand extends Command {
 
@@ -16,9 +17,15 @@ public final class TeleOpDriveCommand extends Command {
   private static DriverControls controls;
 
   private static final double DEADBAND = 0.05;
+  private static final double YAW_EXPO = 0.5;
+  private static final double DRIVE_EXPO = 0.5;
+  private final ExpoScale yawExpo;
+  private final ExpoScale driveExpo;
 
   public TeleOpDriveCommand() {
     requires(DRIVE);
+    this.yawExpo = new ExpoScale(DEADBAND, YAW_EXPO);
+    this.driveExpo = new ExpoScale(DEADBAND, DRIVE_EXPO);
   }
 
   @Override
@@ -30,9 +37,9 @@ protected void initialize() {
 
   @Override
   protected void execute() {
-    double forward = deadband(controls.getForward());
-    double strafe = deadband(controls.getStrafe());
-    double azimuth = deadband(controls.getYaw());
+    double forward = driveExpo.apply(controls.getForward());
+    double strafe = driveExpo.apply(controls.getStrafe());
+    double azimuth = yawExpo.apply(controls.getYaw());
 
     DRIVE.drive(forward, strafe, azimuth);
   }

src/main/java/frc/team2767/deepspace/command/approach/ChooseTeleopHatchPlaceDirection.java

@@ -0,0 +1,19 @@
+package frc.team2767.deepspace.command.approach;
+
+import edu.wpi.first.wpilibj.command.ConditionalCommand;
+import frc.team2767.deepspace.command.states.SetFieldDirectionCommand;
+import frc.team2767.deepspace.subsystem.FieldDirection;
+
+public class ChooseTeleopHatchPlaceDirection extends ConditionalCommand {
+
+  public ChooseTeleopHatchPlaceDirection() {
+    super(
+        new SetFieldDirectionCommand(FieldDirection.LEFT),
+        new SetFieldDirectionCommand(FieldDirection.RIGHT));
+  }
+
+  @Override
+  protected boolean condition() {
+    return true;
+  }
+}

src/main/java/frc/team2767/deepspace/command/approach/OpenLoopDriveUntilCurrentCommand.java

@@ -7,20 +7,25 @@
 import frc.team2767.deepspace.subsystem.VacuumSubsystem;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.thirdcoast.swerve.SwerveDrive;
 
 public class OpenLoopDriveUntilCurrentCommand extends Command {
 
   private static final DriveSubsystem DRIVE = Robot.DRIVE;
   private static final VacuumSubsystem VACUUM = Robot.VACUUM;
-  private static final double TRANSITION_CURRENT_DIFFERENCE = 50.0;
+  private static final double TRANSITION_CURRENT = 20.0;
+  private static final int CURRENT_STABLE_COUNT = 2;
+  private static final double CURRENT_IGNORE = 0.5;
   private static final double DIRECTION = -0.25;
-  private static final double OUT_DRIVE_SECONDS = 0.5;
-  private static final double SOLENOID_DELAY = 0.2;
+  private static final double OUT_DRIVE_SECONDS = 0.25;
+  private static final double SOLENOID_DELAY = 0.5;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private DriveState driveState;
+  private double initBlankTime;
   private double initialCurrent;
   private double outDriveInitTime;
   private double solenoidDelayInit;
+  private int currentStableCount;
 
   public OpenLoopDriveUntilCurrentCommand() {
     setTimeout(5.0);
@@ -31,6 +36,9 @@ public OpenLoopDriveUntilCurrentCommand() {
   protected void initialize() {
     driveState = DriveState.FAST;
     initialCurrent = DRIVE.getAverageOutputCurrent();
+    currentStableCount = 0;
+    DRIVE.setDriveMode(SwerveDrive.DriveMode.CLOSED_LOOP);
+    initBlankTime = Timer.getFPGATimestamp();
   }
 
   @Override
@@ -39,7 +47,14 @@ protected void execute() {
       case FAST:
         DRIVE.setWheels(DIRECTION, DriveState.FAST.velocity);
         double averageCurrent = DRIVE.getAverageOutputCurrent();
-        if (averageCurrent > initialCurrent + TRANSITION_CURRENT_DIFFERENCE) {
+        if (Timer.getFPGATimestamp() - initBlankTime > CURRENT_IGNORE
+            && averageCurrent > initialCurrent + TRANSITION_CURRENT) {
+          currentStableCount++;
+        } else {
+          currentStableCount = 0;
+        }
+
+        if (currentStableCount > CURRENT_STABLE_COUNT) {
           driveState = DriveState.CLOSE_SOLENOID;
           logger.debug("set state to {}, current = {}", driveState, averageCurrent);
         }
@@ -60,6 +75,7 @@ protected void execute() {
         DRIVE.setWheels(DIRECTION, DriveState.OUT.velocity);
         if (Timer.getFPGATimestamp() - outDriveInitTime > OUT_DRIVE_SECONDS) {
           driveState = DriveState.DONE;
+          DRIVE.setWheels(0.0, 0.0);
           logger.debug("set state to {}", driveState);
         }
         break;
@@ -70,12 +86,12 @@ protected void execute() {
 
   @Override
   protected boolean isFinished() {
-    return false;
+    return driveState == DriveState.DONE;
   }
 
   private enum DriveState {
-    FAST(0.5),
-    CLOSE_SOLENOID(0.06),
+    FAST(0.25),
+    CLOSE_SOLENOID(0.08),
     WAIT(0.06),
     OUT(-0.6),
     DONE(0.0);

src/main/java/frc/team2767/deepspace/command/approach/OpenLoopDriveUntilSuctionCommand.java

@@ -14,7 +14,7 @@ public class OpenLoopDriveUntilSuctionCommand extends Command {
   private static final DriveSubsystem DRIVE = Robot.DRIVE;
   private static final VacuumSubsystem VACUUM = Robot.VACUUM;
   private static final double TRANSITION_PRESSURE_DIFFERENCE = 3.0;
-  private static final double HATCH_SEAL_GOOD_ENOUGH = 10.0;
+  private static final double HATCH_SEAL_GOOD_ENOUGH = 7.5;
   private static final double OUT_DRIVE_SECONDS = 0.25;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private double direction;
@@ -34,6 +34,7 @@ protected void initialize() {
       logger.warn("Transition pressures not set correctly");
     }
 
+    DRIVE.setDriveMode(SwerveDrive.DriveMode.CLOSED_LOOP);
     initialPressure = VACUUM.getPressure();
     logger.debug("init pressure = {}", initialPressure);
     DRIVE.setDriveMode(SwerveDrive.DriveMode.OPEN_LOOP);

src/main/java/frc/team2767/deepspace/command/approach/TalonConfigCommand.java

@@ -0,0 +1,23 @@
+package frc.team2767.deepspace.command.approach;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+import frc.team2767.deepspace.Robot;
+import frc.team2767.deepspace.subsystem.DriveSubsystem;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+public class TalonConfigCommand extends InstantCommand {
+  private static final DriveSubsystem DRIVE = Robot.DRIVE;
+  private final DriveSubsystem.DriveTalonConfig config;
+  private final Logger logger = LoggerFactory.getLogger(this.getClass());
+
+  public TalonConfigCommand(DriveSubsystem.DriveTalonConfig config) {
+    this.config = config;
+  }
+
+  @Override
+  protected void initialize() {
+    DRIVE.setSlotConfig(config);
+    logger.debug("set config to {}", config);
+  }
+}

src/main/java/frc/team2767/deepspace/command/approach/YawToTargetCommand.java

@@ -34,7 +34,7 @@ public YawToTargetCommand(double setpoint) {
     controller = getPIDController();
     controller.setContinuous();
     controller.setOutputRange(-1.0, 1.0);
-    controller.setAbsoluteTolerance(1.0);
+    controller.setAbsoluteTolerance(0.35);
     this.setpoint = setpoint;
     count = 0;
     logger.debug("construct yaw command");

src/main/java/frc/team2767/deepspace/command/approach/sequences/AutoHatchPickupCommandGroup.java

@@ -8,9 +8,8 @@ public class AutoHatchPickupCommandGroup extends CommandGroup {
 
   public AutoHatchPickupCommandGroup() {
     addSequential(new LogCommand("BEGIN AUTO HATCH PICKUP"));
-    //    addSequential(new SquareTwistCommandGroup());
     addSequential(new ApproachDirectionCommand());
-    addSequential(new SnapDriveCommandGroup());
+    addSequential(new HatchPickupCommandGroup());
     addSequential(new LogCommand("END AUTO HATCH PICKUP"));
   }
 }

src/main/java/frc/team2767/deepspace/command/approach/sequences/SnapDriveCommandGroup.java renamed to src/main/java/frc/team2767/deepspace/command/approach/sequences/HatchPickupCommandGroup.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import frc.team2767.deepspace.command.approach.CalculateRotationCommand;
 import frc.team2767.deepspace.command.approach.OpenLoopDriveUntilSuctionCommand;
+import frc.team2767.deepspace.command.approach.TalonConfigCommand;
 import frc.team2767.deepspace.command.approach.YawToTargetCommand;
 import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
 import frc.team2767.deepspace.command.elevator.ElevatorExecutePlanCommand;
@@ -17,9 +18,9 @@
 import frc.team2767.deepspace.command.vision.QueryPyeyeCommand;
 import frc.team2767.deepspace.subsystem.*;
 
-public class SnapDriveCommandGroup extends CommandGroup {
+public class HatchPickupCommandGroup extends CommandGroup {
 
-  public SnapDriveCommandGroup() {
+  public HatchPickupCommandGroup() {
     addSequential(new LogCommand("BEGIN AUTO SNAP DRIVE HATCH PICKUP"));
     addSequential(new LightsOnCommand());
     addSequential(new SetSolenoidStatesCommand(VacuumSubsystem.SolenoidStates.PRESSURE_ACCUMULATE));
@@ -34,16 +35,18 @@ public SnapDriveCommandGroup() {
 
     addSequential(new PressureSetCommand(VacuumSubsystem.kHatchPressureInHg));
     addSequential(new SetSolenoidStatesCommand(VacuumSubsystem.SolenoidStates.GAME_PIECE_PICKUP));
+    addSequential(new TalonConfigCommand(DriveSubsystem.DriveTalonConfig.YAW_CONFIG));
     addSequential(new QueryPyeyeCommand());
     addSequential(new CalculateRotationCommand());
-    addSequential(new YawToTargetCommand());
     addSequential(
         new CommandGroup() {
           {
+            addParallel(new YawToTargetCommand());
             addParallel(new ElevatorExecutePlanCommand());
             addParallel(new BiscuitExecutePlanCommand());
           }
         });
+    addSequential(new TalonConfigCommand(DriveSubsystem.DriveTalonConfig.DRIVE_CONFIG));
     addSequential(new OpenLoopDriveUntilSuctionCommand(), 10);
     addParallel(new BlinkLightsCommand(VisionSubsystem.LightPattern.GOT_HATCH), 0.5);
     addParallel(new SetActionCommand(Action.PLACE));

src/main/java/frc/team2767/deepspace/command/approach/sequences/HatchPlaceCommandGroup.java

@@ -0,0 +1,54 @@
+package frc.team2767.deepspace.command.approach.sequences;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.ConditionalCommand;
+import frc.team2767.deepspace.command.approach.CalculateRotationCommand;
+import frc.team2767.deepspace.command.approach.OpenLoopDriveUntilCurrentCommand;
+import frc.team2767.deepspace.command.approach.TalonConfigCommand;
+import frc.team2767.deepspace.command.approach.YawToTargetCommand;
+import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
+import frc.team2767.deepspace.command.elevator.ElevatorExecutePlanCommand;
+import frc.team2767.deepspace.command.log.LogCommand;
+import frc.team2767.deepspace.command.states.SetFieldDirectionCommand;
+import frc.team2767.deepspace.command.vision.LightsOnCommand;
+import frc.team2767.deepspace.command.vision.QueryPyeyeCommand;
+import frc.team2767.deepspace.subsystem.DriveSubsystem;
+import frc.team2767.deepspace.subsystem.FieldDirection;
+
+public class HatchPlaceCommandGroup extends CommandGroup {
+
+  public HatchPlaceCommandGroup() {
+    addSequential(new LogCommand("BEGIN HATCH PLACE"));
+
+    addSequential(
+        new ConditionalCommand(new SetFieldDirectionCommand(FieldDirection.LEFT)) {
+          @Override
+          protected boolean condition() {
+            return DriverStation.getInstance().isAutonomous();
+          }
+        });
+    addSequential(new LightsOnCommand());
+    addSequential(new QueryPyeyeCommand());
+    addParallel(new TalonConfigCommand(DriveSubsystem.DriveTalonConfig.YAW_CONFIG));
+    addSequential(new CalculateRotationCommand());
+    addSequential(
+        new ConditionalCommand(
+            new CommandGroup() {
+              {
+                addParallel(new YawToTargetCommand());
+                addParallel(new ElevatorExecutePlanCommand());
+                addParallel(new BiscuitExecutePlanCommand());
+              }
+            }) {
+          @Override
+          protected boolean condition() {
+            return DriverStation.getInstance().isAutonomous();
+          }
+        });
+    addSequential(new LogCommand("FINISHED MOVING AXIS"));
+    addSequential(new TalonConfigCommand(DriveSubsystem.DriveTalonConfig.DRIVE_CONFIG));
+    addSequential(new OpenLoopDriveUntilCurrentCommand(), 5.0);
+    addSequential(new LogCommand("END HATCH PLACE"));
+  }
+}