Merge pull request #238 from strykeforce/rkalnins/vision

Rkalnins/iri

Author: mwitcpalek

docs/Subsystem-Interface-Conversions.xlsx

@@ -6,14 +6,11 @@
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.team2767.deepspace.command.approach.sandstorm.SandstormCommandGroup;
 import frc.team2767.deepspace.command.sequences.pickup.SandstormHatchPickupCommandGroup;
 import frc.team2767.deepspace.control.AutonChooser;
 import frc.team2767.deepspace.control.Controls;
 import frc.team2767.deepspace.subsystem.*;
 import frc.team2767.deepspace.subsystem.safety.SafetySubsystem;
-import org.slf4j.Logger;
-import org.slf4j.LoggerFactory;
 import org.strykeforce.thirdcoast.telemetry.TelemetryController;
 import org.strykeforce.thirdcoast.telemetry.TelemetryService;
 import org.strykeforce.thirdcoast.trapper.Session;
@@ -31,20 +28,16 @@ public class Robot extends TimedRobot {
   public static ClimbSubsystem CLIMB;
   public static Controls CONTROLS;
   public static StartLevel startLevel = StartLevel.ONE;
-  public static boolean isAuton;
   private static AutonChooser AUTON;
   private static boolean isEvent;
-  private static CommandGroup sandstorm;
   private static CommandGroup noAutoSandstorm;
-  private Logger logger;
 
   public static boolean isEvent() {
     return isEvent;
   }
 
   @Override
   public void robotInit() {
-    isAuton = true;
     DigitalInput di = new DigitalInput(7);
     isEvent = di.get();
 
@@ -53,7 +46,6 @@ public void robotInit() {
       System.out.println("Event flag removed - switching logging to log file");
       System.setProperty(ContextInitializer.CONFIG_FILE_PROPERTY, "logback-event.xml");
     }
-    logger = LoggerFactory.getLogger(this.getClass());
 
     TELEMETRY = new TelemetryService(TelemetryController::new);
     DRIVE = new DriveSubsystem();
@@ -79,7 +71,6 @@ public void robotInit() {
     BISCUIT.zero();
     INTAKE.zero();
 
-    sandstorm = new SandstormCommandGroup();
     noAutoSandstorm = new SandstormHatchPickupCommandGroup();
 
     SmartDashboard.putBoolean("Game/SandstormPickUp", false);
@@ -103,11 +94,7 @@ public void autonomousInit() {
     BISCUIT.setPosition(BISCUIT.getPosition());
     DRIVE.sandstormAxisFlip(true);
     DRIVE.setAngleAdjustment(true);
-    if (isAuton) {
-      //      sandstorm.start();
-    } else {
-      //      noAutoSandstorm.start();
-    }
+    noAutoSandstorm.start();
   }
 
   @Override
@@ -134,6 +121,6 @@ public void teleopPeriodic() {
 
   public enum StartLevel {
     ONE,
-    TWO;
+    TWO
   }
 }

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

@@ -15,26 +15,22 @@
 import org.strykeforce.thirdcoast.util.RateLimit;
 
 public class VisionAutoAlignPickupCommand extends Command {
-  public static final double kP_STRAFE = 0.06; // 0.1
+  public static final double kP_STRAFE = 0.1; // 0.1
   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;
   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 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.40;
+  private static final double FWD_SCALE = 0.6;
+  private static final double FWD_SCALE_FAST = 0.6;
+  private static final double AUTON_OUTPUT = 0.35;
 
   private static final DriveSubsystem DRIVE = Robot.DRIVE;
   private static final VisionSubsystem VISION = Robot.VISION;
   private final ExpoScale driveExpo;
-  private final ExpoScale yawExpo;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private double range;
-  private double strafeError;
-  private double yawError;
   private DriverControls controls;
   private double strafeCorrection;
   private boolean isAuton;
@@ -45,7 +41,6 @@ public class VisionAutoAlignPickupCommand extends Command {
   public VisionAutoAlignPickupCommand() {
     requires(DRIVE);
     this.driveExpo = new ExpoScale(DEADBAND, DRIVE_EXPO);
-    this.yawExpo = new ExpoScale(DEADBAND, YAW_EXPO);
     strafeRateLimit = new RateLimit(0.04); // 0.015
   }
 
@@ -73,7 +68,8 @@ 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;
     if (yaw < -MAX_YAW) yaw = -MAX_YAW;
@@ -84,24 +80,25 @@ protected void execute() {
     double forward;
     // forward is still normal
     if (isAuton) {
-      forward = AUTON_OUTPUT;
+      forward = driveExpo.apply(controls.getForward()) * AUTON_OUTPUT;
     } else 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.getCorrectedBearing())) * range - strafeCorrection;
+    double strafeError =
+        Math.sin(Math.toRadians(VISION.getCorrectedBearing())) * range - strafeCorrection;
 
-    logger.info("strafe error = {}", strafeError);
+    VISION.setStrafeError(strafeError);
 
     // 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());
 
     double strafeOutput = strafeRateLimit.apply(strafe);
-    logger.debug("{} {} {}", forward, strafeOutput, yaw);
+
     DRIVE.drive(forward, strafeOutput, yaw);
   }
 
@@ -112,11 +109,8 @@ protected boolean isFinished() {
 
   @Override
   protected void end() {
-    logger.info("End Auto Align Pickup Vision");
-  }
 
-  private double deadband(double value) {
-    if (Math.abs(value) < DEADBAND) return 0.0;
-    return value;
+    logger.debug("range = {}", range);
+    logger.info("End Auto Align Pickup Vision");
   }
 }

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

@@ -1,58 +1,70 @@
 package frc.team2767.deepspace.command.approach;
 
-import static frc.team2767.deepspace.subsystem.FieldDirection.LEFT;
-
+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;
+import frc.team2767.deepspace.control.Controls;
 import frc.team2767.deepspace.subsystem.DriveSubsystem;
 import frc.team2767.deepspace.subsystem.VisionSubsystem;
+import frc.team2767.deepspace.util.AutoPlaceSideChooser;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 import org.strykeforce.thirdcoast.swerve.SwerveDrive;
+import org.strykeforce.thirdcoast.util.ExpoScale;
 import org.strykeforce.thirdcoast.util.RateLimit;
 
 public class VisionAutoAlignPlaceCommand extends Command {
-  private static final double kP_STRAFE = 0.07; // 0.11
+  private static final double kP_STRAFE = 0.1; // 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;
   private static final double MIN_RANGE = 35.0;
-  private static final double FORWARD_OUTPUT = 0.30;
+  private static final double FORWARD_SCALE = 0.35;
+  private static final double DEADBAND = 0.05;
+  private static final double DRIVE_EXPO = 0.5;
 
   private static final DriveSubsystem DRIVE = Robot.DRIVE;
   private static final VisionSubsystem VISION = Robot.VISION;
+  private static Controls CONTROLS;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
-  private final double gyroOffset;
+  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;
 
-  public VisionAutoAlignPlaceCommand(double gyroOffset) {
+  public VisionAutoAlignPlaceCommand() {
     requires(DRIVE);
-    this.gyroOffset = gyroOffset;
     strafeRateLimit = new RateLimit(0.05); // 0.015
+    this.driveExpo = new ExpoScale(DEADBAND, DRIVE_EXPO);
   }
 
   @Override
   protected void initialize() {
+    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");
     DRIVE.setDriveMode(SwerveDrive.DriveMode.CLOSED_LOOP);
     strafeCorrection = VISION.getStrafeCorrection();
-    if (VISION.direction == LEFT) {
-      targetYaw = 90.0;
-    } else {
-      targetYaw = -90.0;
-    }
+
     DRIVE.setTargetYaw(targetYaw);
-    forward = FORWARD_OUTPUT;
     logger.debug("forward = {}", forward);
     logger.info("target yaw: {}", targetYaw);
   }
@@ -78,19 +90,30 @@ protected void execute() {
     double strafe;
     strafeError = Math.sin(Math.toRadians(VISION.getCorrectedBearing())) * range - strafeCorrection;
     VISION.setStrafeError(strafeError);
-    logger.info("strafe error = {}", strafeError);
+    forward = driveExpo.apply(CONTROLS.getDriverControls().getForward()) * FORWARD_SCALE;
+
     // 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 = 0;
+    else strafe = driveExpo.apply(CONTROLS.getDriverControls().getStrafe());
 
-    DRIVE.drive(forward, strafeRateLimit.apply(strafe), yaw);
+    strafe = strafeRateLimit.apply(strafe);
+
+    DRIVE.setGraphableStrafe(strafe);
+    logger.info("strafe = {}", strafe);
+
+    DRIVE.drive(forward, strafe, yaw);
   }
 
   @Override
   protected boolean isFinished() {
     return (range <= MIN_RANGE && isGood);
   }
 
+  @Override
+  protected void interrupted() {
+    DRIVE.undoGyroOffset();
+  }
+
   @Override
   protected void end() {
     logger.info("End Auto Align Place Vision");

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

@@ -11,6 +11,6 @@ public class AutoPlaceTestLeftCommand extends CommandGroup {
   public AutoPlaceTestLeftCommand() {
     addSequential(new SandstormHatchPickupCommandGroup(), 0.5);
     addSequential(new SetFieldDirectionCommand(FieldDirection.LEFT));
-    addSequential(new AutoHatchPlaceCommandGroup(0.0));
+    addSequential(new AutoHatchPlaceCommandGroup());
   }
 }

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

@@ -11,6 +11,6 @@ public class AutoPlaceTestRightCommand extends CommandGroup {
   public AutoPlaceTestRightCommand() {
     addSequential(new SandstormHatchPickupCommandGroup(), 0.5);
     addSequential(new SetFieldDirectionCommand(FieldDirection.RIGHT));
-    addSequential(new AutoHatchPlaceCommandGroup(0.0));
+    addSequential(new AutoHatchPlaceCommandGroup());
   }
 }

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

@@ -24,7 +24,7 @@ public SandstormCommandGroup() {
             addParallel(new LightsOnCommand());
           }
         });
-    addSequential(new AutoHatchPlaceCommandGroup(0.0));
+    addSequential(new AutoHatchPlaceCommandGroup());
     addSequential(new ApproachDirectionCommand());
     addSequential(
         new CommandGroup() {
@@ -50,8 +50,6 @@ public SandstormCommandGroup() {
             addParallel(new LightsOnCommand());
           }
         });
-    addSequential(
-        new ChooseAutonFieldSideCommand(
-            new AutoHatchPlaceCommandGroup(-90.0), new AutoHatchPlaceCommandGroup(90.0)));
+    addSequential(new AutoHatchPlaceCommandGroup());
   }
 }

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

@@ -20,7 +20,7 @@
 
 public class AutoHatchPlaceCommandGroup extends CommandGroup {
 
-  public AutoHatchPlaceCommandGroup(double gyroOffset) {
+  public AutoHatchPlaceCommandGroup() {
 
     addSequential(new LogCommand("BEGIN AUTO HATCH PLACE"));
     addSequential(new FlipSandstormControlsCommand(false));
@@ -33,9 +33,14 @@ public AutoHatchPlaceCommandGroup(double gyroOffset) {
             addParallel(new SetLevelCommand(ElevatorLevel.ONE));
           }
         });
-    addSequential(new ElevatorSetPositionCommand(ElevatorSubsystem.kHatchLowPositionInches));
-    addSequential(new BiscuitPositionAboveCameraCommand());
-    addSequential(new VisionAutoAlignPlaceCommand(gyroOffset));
+    addSequential(
+        new CommandGroup() {
+          {
+            addParallel(new ElevatorSetPositionCommand(ElevatorSubsystem.kHatchLowPositionInches));
+            addParallel(new BiscuitPositionAboveCameraCommand());
+            addSequential(new VisionAutoAlignPlaceCommand());
+          }
+        });
     addSequential(new BiscuitExecutePlanCommand());
     addSequential(new HoldHeadingUntilCompressionCommand());
     addSequential(new SandstormSwapIfAutonConditionalCommand(true));

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

@@ -5,6 +5,7 @@
 import frc.team2767.deepspace.command.log.LogCommand;
 import frc.team2767.deepspace.command.teleop.StowElevatorConditionalCommand;
 import frc.team2767.deepspace.command.vacuum.StowValveControlCommand;
+import frc.team2767.deepspace.command.vision.LightsOffCommand;
 import frc.team2767.deepspace.subsystem.IntakeSubsystem;
 
 public class StowAllCommandGroup extends CommandGroup {
@@ -16,6 +17,7 @@ public StowAllCommandGroup() {
             addParallel(new StowValveControlCommand());
             addParallel(new StowElevatorConditionalCommand());
             addParallel(new IntakePositionCommand(IntakeSubsystem.kStowPositionDeg));
+            addParallel(new LightsOffCommand());
           }
         });
     addSequential(new LogCommand("END STOW ALL"));

src/main/java/frc/team2767/deepspace/command/sequences/StowAllCommandGroup.java

@@ -16,7 +16,7 @@ public class CoconutPickupAutoRetryCommand extends Command {
   private static final double STABLE_COUNTS = 5;
   private static final double WAIT_TIME = 1000; // ms
   private static final double RESET_HEIGHT = 20.25;
-  private static final double STRING_LENGTH = 260.0;
+  private static final double STRING_COMPRESSED_LENGTH = 223.0; // full length is 248
   private static PickupState state;
   private static double initialPressure;
   private static double startSealTime;
@@ -45,7 +45,7 @@ protected void initialize() {
   protected void execute() {
     switch (state) {
       case DOWN:
-        if (BISCUIT.getCompression() <= STRING_LENGTH) {
+        if (BISCUIT.getCompression() <= STRING_COMPRESSED_LENGTH) {
           state = PickupState.WAIT_FOR_PRESSURE;
           ELEVATOR.setPosition(ELEVATOR.getPosition()); // inches
           startSealTime = System.currentTimeMillis();