Add wobbleDrive feature

Robot2019/src/main/java/frc/robot/OI.java

@@ -12,10 +12,11 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import frc.robot.commands.ActuateClimberRails;
-import frc.robot.commands.NormalDrive;
 import frc.robot.commands.Climb;
 import frc.robot.commands.EjectCargo;
 import frc.robot.commands.IntakeOnlyCargo;
+import frc.robot.commands.NormalDrive;
+import frc.robot.commands.ResetWobble;
 import frc.robot.commands.SetArcadeOrTank;
 import frc.robot.commands.SlowDrive;
 import frc.robot.commands.ToggleCamera;
@@ -54,7 +55,8 @@ public class OI {
     rightSlowBtn = new JoystickButton(rightJoy, 1);
     rightSlowBtn.whileHeld(new SlowDrive(SlowDrive.Side.RIGHT));
     wobbleDriveBtn = new JoystickButton(rightJoy, 4); // TODO: confirm button with drivers
-    wobbleDriveBtn.whileHeld(new WobbleDrive(dt));
+    wobbleDriveBtn.whenPressed(new WobbleDrive(dt));
+    wobbleDriveBtn.whenReleased(new ResetWobble(dt));
 
     arcadeOrTankBtn = new JoystickButton(leftJoy, 4);
     arcadeOrTankBtn.whenPressed(new SetArcadeOrTank());

Robot2019/src/main/java/frc/robot/commands/ResetWobble.java

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+import frc.robot.subsystems.Drivetrain;
+
+/**
+ * Add your docs here.
+ */
+public class ResetWobble extends InstantCommand {
+  /**
+   * Add your docs here.
+   */
+  Drivetrain dt;
+  public ResetWobble(Drivetrain dt) {
+    super();
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    this.dt = dt;
+  }
+
+  // Called once when the command executes
+  @Override
+  protected void initialize() {
+    dt.setWobbleDone(true);
+  }
+
+}

Robot2019/src/main/java/frc/robot/commands/SetArcadeOrTank.java

@@ -8,6 +8,7 @@ public SetArcadeOrTank() {
 
     }
 
+    @Override
     protected void initialize() {
         if (SmartDashboard.getBoolean("Arcade Drive", true)) {
             SmartDashboard.putBoolean("Arcade Drive", false);

Robot2019/src/main/java/frc/robot/commands/WobbleDrive.java

@@ -9,15 +9,20 @@
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Drivetrain.Side;
 
 public class WobbleDrive extends Command {
   Drivetrain dt;
   Side side = Side.LEFT; // We start with left side. TODO: confirm this
   Timer tim;
-  private final double wobbleTime = 0.75; // TODO: Set to actual number
-  private final double driveSpeed = 0.5; // TODO: Set to actual number
+  boolean leftSideDone;
+  boolean rightSideDone;
+  private final double minTime = 0.2;
+  private final double wobbleTime = 0.3; // TODO: Set to actual number
+  private final double driveSpeed = 0.3; // TODO: Set to actual number
+  private final double encRateTolerance = 1; // TODO: Set to actual number
 
   public WobbleDrive(Drivetrain dt) {
     // Use requires() here to declare subsystem dependencies
@@ -32,12 +37,35 @@ protected void initialize() {
     tim = new Timer();
     tim.reset();
     tim.start();
+    dt.setWobbleDone(false);
+    leftSideDone = false;
+    rightSideDone = false;
+    SmartDashboard.putBoolean("Wobble drive done", false);
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    if (tim.get() > wobbleTime) {
+    boolean done = dt.getEncRate(Side.LEFT) < encRateTolerance && dt.getEncRate(Side.RIGHT) < encRateTolerance;
+    if (tim.get() > minTime) {
+      if (side == Side.LEFT) {
+        leftSideDone = done;
+      } else {
+        rightSideDone = done;
+      }
+    }
+    if (leftSideDone) {
+      tim.reset();
+      tim.start();
+      side = Side.RIGHT;
+    }
+    if (rightSideDone) {
+      tim.reset();
+      tim.start();
+      side = Side.LEFT;
+    }
+
+    if (tim.get() > wobbleTime && (!leftSideDone && !rightSideDone)) {
       if (side == Side.LEFT) {
         side = Side.RIGHT;
       } else {
@@ -54,13 +82,14 @@ protected void execute() {
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+    return dt.wobbleDone() || (leftSideDone && rightSideDone);
   }
 
   // Called once after isFinished returns true
   @Override
   protected void end() {
     dt.stop();
+    SmartDashboard.putBoolean("Wobble drive done", true);
   }
 
   // Called when another command which requires one or more of the same

Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -40,6 +40,8 @@ public enum Direction {
   private double blKV, blKA, blVI;
   private double brKV, brKA, brVI;
 
+  private boolean wobbleDone;
+
   public Drivetrain(WPI_TalonSRX leftMaster, BaseMotorController leftSlave1, BaseMotorController leftSlave2,
       WPI_TalonSRX rightMaster, BaseMotorController rightSlave1, BaseMotorController rightSlave2, Encoder leftEnc,
       Encoder rightEnc, AHRS ahrs) {
@@ -71,6 +73,7 @@ public Drivetrain(WPI_TalonSRX leftMaster, BaseMotorController leftSlave1, BaseM
     updateDrivetrainParameters();
 
     maxVoltage = 12.0;
+    wobbleDone = false;
   }
 
   /**
@@ -125,6 +128,14 @@ public double getGyroAngle() {
     return ahrs.getYaw();
   }
 
+  public void setWobbleDone(boolean set) {
+    wobbleDone = set;
+  }
+
+  public boolean wobbleDone() {
+    return wobbleDone;
+  }
+
   public double getMaxSpeed() { // Return must be adjusted in the future;
     return 13 * 12;
   }