Detune azimuths

Author: Roberts Kalnins

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

@@ -98,7 +98,7 @@ public void disabledInit() {
   public void autonomousInit() {
     BISCUIT.setPosition(BISCUIT.getPosition());
     DRIVE.setAngleAdjustment(true);
-    //    sandstorm.start();
+    sandstorm.start();
   }
 
   @Override

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

@@ -18,7 +18,7 @@ public class HoldHeadingUntilCompressionCommand extends Command {
   private static final VacuumSubsystem VACUUM = Robot.VACUUM;
   private static final VisionSubsystem VISION = Robot.VISION;
   private static final double FORWARD_OUTPUT = 0.2;
-  private static final double OUT_TIME_SEC = 0.075;
+  private static final double OUT_TIME_SEC = 0.05;
   private static final double PAUSE_TIME = 0.2;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private DriveState driveState;

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

@@ -13,7 +13,7 @@
 import org.strykeforce.thirdcoast.util.RateLimit;
 
 public class VisionAutoAlignPlaceCommand extends Command {
-  private static final double kP_STRAFE = 0.11; // 0.11
+  private static final double kP_STRAFE = 0.07; // 0.11
   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 goodEnoughYaw = 1.5;

src/main/java/frc/team2767/deepspace/command/approach/sandstorm/AutoPlaceTestCommand.java

@@ -0,0 +1,16 @@
+package frc.team2767.deepspace.command.approach.sandstorm;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import frc.team2767.deepspace.command.approach.sequences.AutoHatchPlaceCommandGroup;
+import frc.team2767.deepspace.command.sequences.pickup.SandstormHatchPickupCommandGroup;
+import frc.team2767.deepspace.command.states.SetFieldDirectionCommand;
+import frc.team2767.deepspace.subsystem.FieldDirection;
+
+public class AutoPlaceTestCommand extends CommandGroup {
+
+  public AutoPlaceTestCommand() {
+    addSequential(new SandstormHatchPickupCommandGroup(), 0.5);
+    addSequential(new SetFieldDirectionCommand(FieldDirection.LEFT));
+    addSequential(new AutoHatchPlaceCommandGroup(0.0));
+  }
+}

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

@@ -4,7 +4,7 @@
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import edu.wpi.first.wpilibj.command.Command;
 import frc.team2767.deepspace.command.ZeroGyroCommand;
-import frc.team2767.deepspace.command.approach.sandstorm.SandstormCommandGroup;
+import frc.team2767.deepspace.command.approach.sandstorm.AutoPlaceTestCommand;
 import frc.team2767.deepspace.command.approach.sequences.AutoHatchPickupCommandGroup;
 import frc.team2767.deepspace.command.biscuit.BiscuitNegativeCommand;
 import frc.team2767.deepspace.command.biscuit.BiscuitPositiveCommand;
@@ -58,8 +58,8 @@ public class DriverControls {
     // interrupt
     // new JoystickButton(joystick, Trim.LEFT_Y_NEG.id).whenPressed(new InterruptCommand());
     // new JoystickButton(joystick, Trim.LEFT_Y_POS.id).whenPressed(new InterruptCommand());
-    new JoystickButton(joystick, Trim.LEFT_Y_POS.id).whenPressed(new SandstormCommandGroup());
-    new JoystickButton(joystick, Trim.LEFT_Y_NEG.id).whenPressed(new SandstormCommandGroup());
+    new JoystickButton(joystick, Trim.LEFT_Y_POS.id).whenPressed(new AutoPlaceTestCommand());
+    new JoystickButton(joystick, Trim.LEFT_Y_NEG.id).whenPressed(new AutoPlaceTestCommand());
 
     // biscuit
     new JoystickButton(joystick, Trim.LEFT_X_POS.id).whenPressed(new BiscuitPositiveCommand());

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

@@ -42,7 +42,8 @@ public class VisionSubsystem extends Subsystem implements Item {
   private static final double CAMERA_RANGE_SLOPE_LEFT = 1.0499; // 0.8259
   private static final double CAMERA_RANGE_OFFSET_LEFT = -4.8818; // -5.6325
   // NEGATIVE = TOWARDS FIELD LEFT
-  private static final double STRAFE_CORRECTION_RIGHT = 0.4; // -1.0
+  private static final double STRAFE_CORRECTION_RIGHT =
+      0.325; // -1.0 // NEGATIVE TO FIELD LEFT FOR THIS ONE?
   private static final double STRAFE_CORRECTION_LEFT = 0.9;
 
   private final Logger logger = LoggerFactory.getLogger(this.getClass());