Add pre elims changes

Author: Roberts Kalnins

docs/Subsystem-Interface-Conversions.xlsx

@@ -24,10 +24,11 @@ public class VisionAutoAlignPickupCommand extends Command {
   private static final double MIN_RANGE = 35.0;
   private static final double FWD_SCALE = 0.5;
   private static final double FWD_SCALE_FAST = 0.5;
-  private static final double AUTON_OUTPUT = 0.35;
+  private static final double AUTON_OUTPUT = 0.30;
 
   private static final DriveSubsystem DRIVE = Robot.DRIVE;
   private static final VisionSubsystem VISION = Robot.VISION;
+
   private final ExpoScale driveExpo;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private double range;
@@ -46,6 +47,7 @@ public VisionAutoAlignPickupCommand() {
 
   @Override
   protected void initialize() {
+    VISION.queryPyeye();
     isAuton = DriverStation.getInstance().isAutonomous();
     SmartDashboard.putBoolean("Game/haveHatch", false);
     logger.info("Begin Vision Auto Align Pickup");

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

@@ -1,6 +1,5 @@
 package frc.team2767.deepspace.command.approach;
 
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.team2767.deepspace.Robot;
@@ -29,14 +28,10 @@ public class VisionAutoAlignPlaceCommand extends Command {
   private static Controls CONTROLS;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private AutoPlaceSideChooser autoPlaceSideChooser = new AutoPlaceSideChooser();
-  private double gyroOffset;
   private double range;
   private double forward;
-  private double strafeError;
-  private double yawError;
   private double strafeCorrection;
   private double targetYaw;
-  private boolean isSandstorm;
   private boolean isGood = false;
   private RateLimit strafeRateLimit;
   private final ExpoScale driveExpo;
@@ -49,15 +44,14 @@ public VisionAutoAlignPlaceCommand() {
 
   @Override
   protected void initialize() {
-    gyroOffset =
+    double gyroOffset =
         autoPlaceSideChooser.determineGyroOffset(
             VISION.direction, Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360.0));
     targetYaw = autoPlaceSideChooser.determineTargetYaw(VISION.direction);
 
     logger.debug("offset = {} target = {}", gyroOffset, targetYaw);
 
     CONTROLS = Robot.CONTROLS;
-    isSandstorm = DriverStation.getInstance().isAutonomous();
     DRIVE.setGyroOffset(gyroOffset);
     SmartDashboard.putBoolean("Game/haveHatch", true);
     logger.info("Begin Vision Auto Align Place");
@@ -78,7 +72,7 @@ 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 - Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360.0);
+    double yawError = targetYaw - Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360.0);
     DRIVE.setYawError(yawError);
     double yaw = kP_YAW * yawError;
     if (yaw > MAX_YAW) yaw = MAX_YAW;
@@ -88,7 +82,8 @@ protected void execute() {
     boolean onTarget = Math.abs(yawError) <= goodEnoughYaw;
 
     double strafe;
-    strafeError = Math.sin(Math.toRadians(VISION.getCorrectedBearing())) * range - strafeCorrection;
+    double strafeError =
+        Math.sin(Math.toRadians(VISION.getCorrectedBearing())) * range - strafeCorrection;
     VISION.setStrafeError(strafeError);
     forward = driveExpo.apply(CONTROLS.getDriverControls().getForward()) * FORWARD_SCALE;
 

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

@@ -66,6 +66,7 @@ private void addMatchCommands() {
     SmartDashboard.putData("Game/Gyro", Robot.DRIVE.getGyro());
     SmartDashboard.putData("Game/Lvl2Deploy", new ClimbLevel2DeployCommand());
     SmartDashboard.putData("Game/Lvl2Climb", new ClimbLevel2AutoCommand());
+    SmartDashboard.putData("Stop pump", new StopPumpCommandGroup());
   }
 
   private void addClimbTab() {

src/main/java/frc/team2767/deepspace/control/SmartDashboardControls.java

@@ -257,6 +257,10 @@ public int getTicks() {
   @SuppressWarnings("Duplicates")
   @Override
   public void setLimits(int forward, int reverse) {
+    if (elevator.hasResetOccurred()) {
+      logger.warn("ELEVATOR TALON RESET");
+    }
+
     if (forward != currentForwardLimit) {
       elevator.configForwardSoftLimitThreshold(forward, 0);
       currentForwardLimit = forward;

src/main/java/frc/team2767/deepspace/subsystem/ElevatorSubsystem.java

@@ -30,7 +30,7 @@ public class VisionSubsystem extends Subsystem implements Item {
   private static final double CAMERA_Y_LEFT = -13.5;
   private static final double CAMERA_Y_RIGHT = 13.5;
   private static final double GLUE_CORRECTION_FACTOR_RIGHT = -2.21; // -2.26 comp
-  private static final double GLUE_CORRECTION_FACTOR_LEFT = 0.21; // 0.6147 comp
+  private static final double GLUE_CORRECTION_FACTOR_LEFT = 0.6417; // 0.6147 comp
   private static final double CAMERA_DEGREES_PER_PIXEL_ADJUSTMENT_RIGHT =
       1.0; // 1.0 is zero value 0.85
   private static final double CAMERA_DEGREES_PER_PIXEL_ADJUSTMENT_LEFT =
@@ -43,8 +43,8 @@ public class VisionSubsystem extends Subsystem implements Item {
   private static final double CAMERA_RANGE_OFFSET_LEFT = -4.92; // -5.6325
   // NEGATIVE = TOWARDS FIELD LEFT (this one was negative)
   private static final double STRAFE_CORRECTION_RIGHT =
-      0.0; // -1.0 // NEGATIVE TO FIELD LEFT FOR THIS ONE?
-  private static final double STRAFE_CORRECTION_LEFT = 1.0; // was positive
+      -0.5; // -1.0 // NEGATIVE TO FIELD LEFT FOR THIS ONE?
+  private static final double STRAFE_CORRECTION_LEFT = 0.5; // was positive
 
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private final DigitalOutput lightsOutput6 = new DigitalOutput(6);

src/main/java/frc/team2767/deepspace/subsystem/VisionSubsystem.java

@@ -1,7 +1,7 @@
 package frc.team2767.deepspace.subsystem.safety;
 
 enum IntakePosition {
-  INTAKE_INTAKE(12_081, -700),
+  INTAKE_INTAKE(12_081, -800),
   INTAKE_STOW(12_081, -475);
 
   public final int forwardLimit;