Gradual drive (#103)

* add gradual drive

* change characterization to a toggle

* confirmed gradual drive button

Author: kevinzwang

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

@@ -14,6 +14,7 @@
 import frc.robot.commands.ToggleClimberRails;
 import frc.robot.commands.Climb;
 import frc.robot.commands.EjectCargo;
+import frc.robot.commands.GradualDrive;
 import frc.robot.commands.ToggleHatchEject;
 import frc.robot.commands.IntakeCargo;
 import frc.robot.commands.ToggleHatchIntake;
@@ -41,6 +42,7 @@ public class OI {
   JoystickButton leftSlowBtn, rightSlowBtn;
   JoystickButton arcadeOrTankBtn;
   JoystickButton normDriveBtn;
+  JoystickButton gradDriveBtn;
   JoystickButton hatchIntakeBtn, hatchEjectBtn;
   JoystickButton cargoIntakeBtn, cargoEjectBtn;
   JoystickButton climberRailBtn;
@@ -67,7 +69,9 @@ public class OI {
     arcadeOrTankBtn = new JoystickButton(leftJoy, 4);
     arcadeOrTankBtn.whenPressed(new SetArcadeOrTank());
     normDriveBtn = new JoystickButton(leftJoy, 3);
-    normDriveBtn.whileHeld(new NormalDrive());
+    normDriveBtn.whenPressed(new NormalDrive());
+    gradDriveBtn = new JoystickButton(leftJoy, 5);
+    gradDriveBtn.whenPressed(new GradualDrive());
 
     hatchIntakeBtn = new JoystickButton(manipulator, Manip.X);
     hatchIntakeBtn.whenPressed(new ToggleHatchIntake(hp));

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

@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Add your docs here.
+ */
+public class GradualDrive extends InstantCommand {
+  /**
+   * Add your docs here.
+   */
+  public GradualDrive() {
+    super();
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+  }
+
+  // Called once when the command executes
+  @Override
+  protected void initialize() {
+    SmartDashboard.putBoolean("Gradual Drive", !SmartDashboard.getBoolean("Gradual Drive", false));
+  }
+
+}

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

@@ -7,26 +7,26 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.InstantCommand;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-public class NormalDrive extends Command {
-  protected void initialize() {
-    SmartDashboard.putBoolean("Characterized Drive", false);
-  }
-
-  @Override
-  protected boolean isFinished() {
-    return false;
+/**
+ * Add your docs here.
+ */
+public class NormalDrive extends InstantCommand {
+  /**
+   * Add your docs here.
+   */
+  public NormalDrive() {
+    super();
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
   }
 
+  // Called once when the command executes
   @Override
-  protected void end() {
-    SmartDashboard.putBoolean("Characterized Drive", true);
+  protected void initialize() {
+    SmartDashboard.putBoolean("Characterized Drive", !SmartDashboard.getBoolean("Characterized Drive", false));
   }
 
-  @Override
-  protected void interrupted() {
-    end();
-  }
 }

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

@@ -16,6 +16,8 @@ public class TeleopDrive extends Command {
   Drivetrain dt;
   Joystick leftJoy, rightJoy;
 
+  double prevSpeed = 0, prevLeft = 0, prevRight = 0;
+
   /**
    * Handles all the teleoperated driving functionality
    * 
@@ -27,6 +29,10 @@ public TeleopDrive(Drivetrain dt, Joystick leftJoy, Joystick rightJoy) {
     this.dt = dt;
     this.leftJoy = leftJoy;
     this.rightJoy = rightJoy;
+
+    if (!SmartDashboard.containsKey("Gradual Drive Max dV")) {
+      SmartDashboard.putNumber("Gradual Drive Max dV", 0.5); // between zero (no movement) to 2 (any movement)
+    }
   }
 
   @Override
@@ -56,6 +62,14 @@ private void arcadeDrive() {
       rot *= SmartDashboard.getNumber("Rotation Slow Ratio", 0.35);
     }
 
+    if (SmartDashboard.getBoolean("Gradual Drive", false)) {
+      double dV = SmartDashboard.getNumber("Gradual Drive Max dV", 0.5);
+      if (Math.abs(speed - prevSpeed) > dV) {
+        speed = prevSpeed + dV * Math.signum(speed - prevSpeed);
+      }
+    }
+    prevSpeed = speed;
+
     double left, right;
 
     // double maxInput = Math.copySign(Math.max(Math.abs(speed), Math.abs(rot)),
@@ -107,6 +121,18 @@ private void tankDrive() {
       right *= SmartDashboard.getNumber("Speed Slow Ratio", 0.5);
     }
 
+    if (SmartDashboard.getBoolean("Gradual Drive", false)) {
+      double dV = SmartDashboard.getNumber("Gradual Drive Max dV", 0.5);
+      if (Math.abs(left - prevLeft) > dV) {
+        left = prevLeft + dV * Math.signum(left - prevLeft);
+      }
+      if (Math.abs(right - prevRight) > dV) {
+        right = prevRight + dV * Math.signum(right - prevRight);
+      }
+    }
+    prevLeft = left;
+    prevRight = right;
+
     if (SmartDashboard.getBoolean("Characterized Drive", true)) {
       charDrive(left, right);
     } else {