Merge pull request #235 from strykeforce/rkalnins/field-oriented-switching

Rkalnins/field oriented switching

Author: jhh

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

@@ -102,7 +102,7 @@ public void disabledInit() {
   public void autonomousInit() {
     BISCUIT.setPosition(BISCUIT.getPosition());
     DRIVE.sandstormAxisFlip(true);
-    DRIVE.setEnableDriveAxisFlip(true);
+    DRIVE.setAngleAdjustment(true);
     if (isAuton) {
       //      sandstorm.start();
     } else {

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

@@ -0,0 +1,20 @@
+package frc.team2767.deepspace.command.approach;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+import frc.team2767.deepspace.Robot;
+import frc.team2767.deepspace.subsystem.DriveSubsystem;
+
+public class FlipSandstormControlsCommand extends InstantCommand {
+
+  private static final DriveSubsystem DRIVE = Robot.DRIVE;
+  private final boolean enable;
+
+  public FlipSandstormControlsCommand(boolean enable) {
+    this.enable = enable;
+  }
+
+  @Override
+  protected void initialize() {
+    DRIVE.sandstormAxisFlip(enable);
+  }
+}

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

@@ -0,0 +1,16 @@
+package frc.team2767.deepspace.command.approach;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.command.ConditionalCommand;
+
+public class SandstormSwapIfAutonConditionalCommand extends ConditionalCommand {
+
+  public SandstormSwapIfAutonConditionalCommand(boolean enable) {
+    super(new FlipSandstormControlsCommand(enable));
+  }
+
+  @Override
+  protected boolean condition() {
+    return DriverStation.getInstance().isAutonomous();
+  }
+}

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

@@ -100,7 +100,9 @@ protected void execute() {
     if (isGood && onTarget) strafe = strafeError * kP_STRAFE * forward;
     else strafe = driveExpo.apply(controls.getStrafe());
 
-    DRIVE.drive(forward, strafeRateLimit.apply(strafe), yaw);
+    double strafeOutput = strafeRateLimit.apply(strafe);
+    logger.debug("{} {} {}", forward, strafeOutput, yaw);
+    DRIVE.drive(forward, strafeOutput, yaw);
   }
 
   @Override

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

@@ -1,10 +1,7 @@
 package frc.team2767.deepspace.command.approach.sequences;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
-import frc.team2767.deepspace.command.approach.ApproachDirectionCommand;
-import frc.team2767.deepspace.command.approach.BallToHatchAutoSafetyCommand;
-import frc.team2767.deepspace.command.approach.HoldHeadingUntilSuctionCommand;
-import frc.team2767.deepspace.command.approach.VisionAutoAlignPickupCommand;
+import frc.team2767.deepspace.command.approach.*;
 import frc.team2767.deepspace.command.biscuit.BiscuitConfigMotionAccelCommand;
 import frc.team2767.deepspace.command.biscuit.BiscuitFastExecuteCommand;
 import frc.team2767.deepspace.command.biscuit.ReleaseKrakenCommand;
@@ -21,6 +18,7 @@ public class AutoHatchPickupCommandGroup extends CommandGroup {
 
   public AutoHatchPickupCommandGroup() {
     addSequential(new LogCommand("BEGIN AUTO HATCH PICKUP"));
+    addSequential(new FlipSandstormControlsCommand(false));
     addSequential(new ApproachDirectionCommand());
     addSequential(new LightsOnCommand());
     addSequential(
@@ -41,6 +39,7 @@ public AutoHatchPickupCommandGroup() {
     addSequential(new HoldHeadingUntilSuctionCommand());
     addParallel(new BiscuitConfigMotionAccelCommand(BiscuitSubsystem.kSlowAccel));
     addParallel(new SetActionCommand(Action.PLACE));
+    addSequential(new SandstormSwapIfAutonConditionalCommand(true));
     addSequential(new LogCommand("END AUTO HATCH PICKUP"));
   }
 }

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

@@ -1,7 +1,9 @@
 package frc.team2767.deepspace.command.approach.sequences;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
+import frc.team2767.deepspace.command.approach.FlipSandstormControlsCommand;
 import frc.team2767.deepspace.command.approach.HoldHeadingUntilCompressionCommand;
+import frc.team2767.deepspace.command.approach.SandstormSwapIfAutonConditionalCommand;
 import frc.team2767.deepspace.command.approach.VisionAutoAlignPlaceCommand;
 import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
 import frc.team2767.deepspace.command.biscuit.BiscuitPositionAboveCameraCommand;
@@ -21,6 +23,7 @@ public class AutoHatchPlaceCommandGroup extends CommandGroup {
   public AutoHatchPlaceCommandGroup(double gyroOffset) {
 
     addSequential(new LogCommand("BEGIN AUTO HATCH PLACE"));
+    addSequential(new FlipSandstormControlsCommand(false));
     addSequential(new LightsOnCommand());
     addSequential(
         new CommandGroup() {
@@ -35,6 +38,7 @@ public AutoHatchPlaceCommandGroup(double gyroOffset) {
     addSequential(new VisionAutoAlignPlaceCommand(gyroOffset));
     addSequential(new BiscuitExecutePlanCommand());
     addSequential(new HoldHeadingUntilCompressionCommand());
+    addSequential(new SandstormSwapIfAutonConditionalCommand(true));
     addSequential(new LogCommand("END HATCH PLACE"));
   }
 }

src/main/java/frc/team2767/deepspace/command/teleop/InterruptCommand.java

@@ -1,5 +1,6 @@
 package frc.team2767.deepspace.command.teleop;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.command.InstantCommand;
 import frc.team2767.deepspace.Robot;
 import frc.team2767.deepspace.subsystem.Action;
@@ -24,6 +25,10 @@ public InterruptCommand() {
 
   @Override
   protected void initialize() {
+    if (DriverStation.getInstance().isAutonomous()) {
+      DRIVE.sandstormAxisFlip(true);
+    }
+
     DRIVE.setSlotConfig(DriveSubsystem.DriveTalonConfig.DRIVE_CONFIG);
     DRIVE.undoGyroOffset();
     BISCUIT.setMotionMagicAccel(BiscuitSubsystem.kSlowAccel);

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

@@ -56,9 +56,9 @@ public DriveSubsystem() {
     wheels = swerve.getWheels();
   }
 
-  public void sandstormAxisFlip(boolean enable) {
-    swerve.setFieldOriented(!enable);
-    setEnableDriveAxisFlip(enable);
+  public void sandstormAxisFlip(boolean isCameraOriented) {
+    swerve.setFieldOriented(!isCameraOriented);
+    setEnableDriveAxisFlip(isCameraOriented);
   }
 
   @Override
@@ -132,8 +132,9 @@ public void interruptPath() {
     pathController.interrupt();
   }
 
-  public void setEnableDriveAxisFlip(boolean enable) {
+  private void setEnableDriveAxisFlip(boolean enable) {
     enableDriveAxisFlip = enable;
+    logger.debug("driving is {} oriented", enable);
   }
 
   public void setTargetYaw(double targetYaw) {