Merge pull request #225 from strykeforce/mwitcpalek/sandstorm

Mwitcpalek/sandstorm

Author: mwitcpalek

nt/dart-4-10.ini

@@ -78,6 +78,7 @@ public void robotInit() {
     }
 
     SmartDashboard.putBoolean("Game/SandstormPickUp", false);
+    SmartDashboard.putBoolean("Game/haveHatch", false);
 
     //    new SmartDashboardControls();
   }

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

@@ -43,7 +43,7 @@ protected void initialize() {
   @Override
   protected void execute() {
     // Calculate Yaw Term based on gyro
-    yawError = targetYaw - DRIVE.getGyro().getAngle();
+    yawError = targetYaw - Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360);
     double yaw = kP_YAW * yawError;
     if (yaw > MAX_YAW) yaw = MAX_YAW;
     if (yaw < -MAX_YAW) yaw = -MAX_YAW;

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

@@ -0,0 +1,38 @@
+package frc.team2767.deepspace.command.approach;
+
+import edu.wpi.first.wpilibj.command.Command;
+import frc.team2767.deepspace.Robot;
+import frc.team2767.deepspace.control.DriverControls;
+import frc.team2767.deepspace.subsystem.DriveSubsystem;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.thirdcoast.util.ExpoScale;
+
+public class HoldHeadingCommand extends Command {
+  private static final DriveSubsystem DRIVE = Robot.DRIVE;
+  private static DriverControls controls;
+  private final Logger logger = LoggerFactory.getLogger(this.getClass());
+  private static double forward;
+  private static ExpoScale driveExpo;
+
+  private static final double FWD_SCALE = 0.3;
+  private static final double DRIVE_EXPO = 0.5;
+  private static final double DEADBAND = 0.05;
+
+  @Override
+  protected void initialize() {
+    controls = Robot.CONTROLS.getDriverControls();
+    logger.info("Hold Heading, Hatch Place");
+  }
+
+  @Override
+  protected void execute() {
+    forward = driveExpo.apply(controls.getForward()) * FWD_SCALE;
+    DRIVE.drive(forward, 0.0, 0.0);
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+}

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

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.team2767.deepspace.Robot;
 import frc.team2767.deepspace.control.DriverControls;
 import frc.team2767.deepspace.subsystem.DriveSubsystem;
@@ -26,7 +27,7 @@ public class HoldHeadingUntilSuctionCommand extends Command {
   private static double outInitTime;
 
   private static final double HATCH_SEAL_GOOD_ENOUGH = 7.5;
-  private static final double FWD_SCALE = 0.3;
+  private static final double FWD_SCALE = 0.2;
   private static final double DRIVE_EXPO = 0.5;
   private static final double DEADBAND = 0.05;
   private static final double OUT_TIME_SEC = 0.25;
@@ -54,6 +55,7 @@ protected void execute() {
         if ((currentPressure - initialPressure) > HATCH_SEAL_GOOD_ENOUGH) {
           driveState = DriveState.OUT;
           VISION.enableLights(false);
+          SmartDashboard.putBoolean("Game/haveHatch", true);
           outInitTime = Timer.getFPGATimestamp();
           logger.info("Have Seal");
         }

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

@@ -1,6 +1,7 @@
 package frc.team2767.deepspace.command.approach;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.team2767.deepspace.Robot;
 import frc.team2767.deepspace.control.DriverControls;
 import frc.team2767.deepspace.subsystem.DriveSubsystem;
@@ -16,17 +17,19 @@
 import org.strykeforce.thirdcoast.telemetry.grapher.Measure;
 import org.strykeforce.thirdcoast.telemetry.item.Item;
 import org.strykeforce.thirdcoast.util.ExpoScale;
+import org.strykeforce.thirdcoast.util.RateLimit;
 
 public class VisionAutoAlignPickupCommand extends Command implements Item {
-  public static final double kP_STRAFE = 0.05;
+  public static final double kP_STRAFE = 0.1; // 0.09
   private static final double DRIVE_EXPO = 0.5;
   private static final double YAW_EXPO = 0.5;
   private static final double DEADBAND = 0.05;
   private static final double kP_YAW = 0.01; // 0.00625 tuning for NT method, 0.01 pyeye
   private static final double MAX_YAW = 0.3;
-  private static final double MIN_RANGE = 70.0;
+  private static final double MIN_RANGE = 35.0;
   private static final double FWD_SCALE = 0.3;
-  private static final double goodEnoughYaw = 1.0;
+  private static final double FWD_SCALE_FAST = 0.5;
+  private static final double goodEnoughYaw = 1.5;
 
   private static final DriveSubsystem DRIVE = Robot.DRIVE;
   private static final VisionSubsystem VISION = Robot.VISION;
@@ -40,11 +43,14 @@ public class VisionAutoAlignPickupCommand extends Command implements Item {
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private double targetYaw;
   private boolean isGood = false;
+  private static double strafeCorrection;
+  private RateLimit rateLimit;
 
   public VisionAutoAlignPickupCommand() {
     requires(DRIVE);
     this.driveExpo = new ExpoScale(DEADBAND, DRIVE_EXPO);
     this.yawExpo = new ExpoScale(DEADBAND, YAW_EXPO);
+    rateLimit = new RateLimit(0.015);
 
     TelemetryService telemetryService = Robot.TELEMETRY;
     telemetryService.stop();
@@ -53,10 +59,11 @@ public VisionAutoAlignPickupCommand() {
 
   @Override
   protected void initialize() {
+    SmartDashboard.putBoolean("Game/haveHatch", false);
     logger.info("Begin Vision Auto Align Pickup");
     controls = Robot.CONTROLS.getDriverControls();
     DRIVE.setDriveMode(SwerveDrive.DriveMode.CLOSED_LOOP);
-
+    strafeCorrection = VISION.getStrafeCorrection();
     if (VISION.direction == FieldDirection.LEFT) {
       targetYaw = -90.0;
     } else targetYaw = 90.0;
@@ -71,24 +78,26 @@ protected void execute() {
     isGood = range >= 0; // check if range is good (we have a target), not -1
 
     // Calculate Yaw Term based on gyro
-    yawError = targetYaw - DRIVE.getGyro().getAngle();
+    yawError = targetYaw - Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360.0);
     double yaw = kP_YAW * yawError;
     if (yaw > MAX_YAW) yaw = MAX_YAW;
     if (yaw < -MAX_YAW) yaw = -MAX_YAW;
 
     // Determine if actual yaw is close enough to target
     boolean onTarget = Math.abs(yawError) <= goodEnoughYaw;
 
+    double forward;
     // forward is still normal
-    double forward = driveExpo.apply(controls.getForward()) * FWD_SCALE;
-
+    if (isGood) {
+      forward = driveExpo.apply(controls.getForward()) * FWD_SCALE;
+    } else forward = driveExpo.apply(controls.getForward()) * FWD_SCALE_FAST;
     double strafe;
-    strafeError = Math.sin(Math.toRadians(VISION.getRawBearing())) * range;
+    strafeError = Math.sin(Math.toRadians(VISION.getRawBearing())) * range - strafeCorrection;
     // Only take over strafe control if pyeye has a target and the robot is straight to the field
     if (isGood && onTarget) strafe = strafeError * kP_STRAFE * forward;
     else strafe = driveExpo.apply(controls.getStrafe());
 
-    DRIVE.drive(forward, strafe, yaw);
+    DRIVE.drive(forward, rateLimit.apply(strafe), yaw);
   }
 
   @Override
@@ -142,6 +151,8 @@ public DoubleSupplier measurementFor(@NotNull Measure measure) {
         return () -> yawError;
       case RANGE:
         return () -> strafeError;
+        //      case COMPONENT_STRAFE:
+        //        return () -> strafe;
       default:
         return () -> 2767;
     }

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

@@ -4,8 +4,10 @@
 import frc.team2767.deepspace.command.approach.ApproachDirectionCommand;
 import frc.team2767.deepspace.command.approach.HoldHeadingUntilSuctionCommand;
 import frc.team2767.deepspace.command.approach.VisionAutoAlignPickupCommand;
+import frc.team2767.deepspace.command.biscuit.BiscuitConfigMotionAccelCommand;
 import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
 import frc.team2767.deepspace.command.biscuit.BiscuitPositionAboveCameraCommand;
+import frc.team2767.deepspace.command.biscuit.ReleaseKrakenCommand;
 import frc.team2767.deepspace.command.elevator.ElevatorExecutePlanCommand;
 import frc.team2767.deepspace.command.log.LogCommand;
 import frc.team2767.deepspace.command.states.SetActionCommand;
@@ -14,10 +16,7 @@
 import frc.team2767.deepspace.command.vacuum.PressureSetCommand;
 import frc.team2767.deepspace.command.vacuum.SetSolenoidStatesCommand;
 import frc.team2767.deepspace.command.vision.LightsOnCommand;
-import frc.team2767.deepspace.subsystem.Action;
-import frc.team2767.deepspace.subsystem.ElevatorLevel;
-import frc.team2767.deepspace.subsystem.GamePiece;
-import frc.team2767.deepspace.subsystem.VacuumSubsystem;
+import frc.team2767.deepspace.subsystem.*;
 
 public class AutoHatchPickupCommandGroup extends CommandGroup {
 
@@ -36,11 +35,13 @@ public AutoHatchPickupCommandGroup() {
 
     addSequential(new PressureSetCommand(VacuumSubsystem.kHatchPressureInHg), 0.02);
     addSequential(new SetSolenoidStatesCommand(VacuumSubsystem.SolenoidStates.GAME_PIECE_PICKUP));
+    addParallel(new ReleaseKrakenCommand(true));
     addParallel(new BiscuitPositionAboveCameraCommand());
     addParallel(new ElevatorExecutePlanCommand());
     addSequential(new VisionAutoAlignPickupCommand());
     addSequential(new BiscuitExecutePlanCommand());
     addSequential(new HoldHeadingUntilSuctionCommand());
+    addParallel(new BiscuitConfigMotionAccelCommand(BiscuitSubsystem.kSlowAccel));
     addParallel(new SetActionCommand(Action.PLACE));
     addSequential(new LogCommand("END AUTO HATCH PICKUP"));
   }

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

@@ -0,0 +1,34 @@
+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.*;
+import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
+import frc.team2767.deepspace.command.biscuit.BiscuitPositionAboveCameraCommand;
+import frc.team2767.deepspace.command.log.LogCommand;
+import frc.team2767.deepspace.command.states.SetFieldDirectionCommand;
+import frc.team2767.deepspace.command.teleop.DriverPlaceAssistCommand;
+import frc.team2767.deepspace.command.vision.LightsOnCommand;
+import frc.team2767.deepspace.subsystem.FieldDirection;
+
+public class AutoHatchPlaceCommandGroup extends CommandGroup {
+
+  public AutoHatchPlaceCommandGroup() {
+    addSequential(new LogCommand("BEGIN AUTO HATCH PLACE"));
+
+    addSequential(
+        new ConditionalCommand(new SetFieldDirectionCommand(FieldDirection.LEFT)) {
+          @Override
+          protected boolean condition() {
+            return DriverStation.getInstance().isAutonomous();
+          }
+        });
+    addSequential(new LightsOnCommand());
+    addSequential(new BiscuitPositionAboveCameraCommand());
+    addSequential(new DriverPlaceAssistCommand());
+    addSequential(new BiscuitExecutePlanCommand());
+    addSequential(new HoldHeadingCommand());
+    addSequential(new LogCommand("END HATCH PLACE"));
+  }
+}