Merge pull request #240 from strykeforce/mwitcpalek/driverTune

Mwitcpalek/driver tune

Author: mwitcpalek

README.md

@@ -1,5 +1,12 @@
 # 2019 FIRST DEEP SPACE
 
+![airtight](https://strykeforce.smugmug.com/2019-Stryke-Force/2019-Deep-Space/i-2dCqG95/A)
+
+## Controls
+![driver](docs/2019-driver-controls.JPG)
+
+![copilot](docs/2019-copilot-controls.JPG)
+
 ## Talons
 
 Subsystem    | Talon       | ID | PDP
@@ -61,7 +68,8 @@ Vision    | cam0Tune | User Button
 
 ## PCM
 
-Subsystem | Valve        | Solenoid
---------- | ------------ | --------
-Vacuum    | trident      | 0
-Vacuum    | climb        | 2
+Subsystem | Valve         | Solenoid
+--------- | ------------- | --------
+Vacuum    | trident(hatch)| 0
+Vacuum    | climb         | 2
+Vacuum    | trident(cargo)| 3

docs/2019-copilot-controls.JPG

@@ -16,9 +16,11 @@ public final class TeleOpDriveCommand extends Command {
   private static final DriveSubsystem DRIVE = Robot.DRIVE;
   private static final VisionSubsystem VISION = Robot.VISION;
   private static final double DEADBAND = 0.05;
-  private static final double YAW_EXPO = 0.5;
-  private static final double DRIVE_EXPO = 0.5;
+  private static final double YAW_EXPO = 0.65; // 0.5
+  private static final double DRIVE_EXPO = 0.65; // 0.5
   private static final double kP = 0.1;
+  private static final double MAX_FWD_STR = 0.7;
+  private static final double MAX_YAW = 0.6;
   private static DriverControls controls;
   private final Logger logger = LoggerFactory.getLogger(this.getClass());
   private final ExpoScale yawExpo;
@@ -39,33 +41,15 @@ protected void initialize() {
 
   @Override
   protected void execute() {
-    /*VISION.queryPyeye(); // gets corrected heading and range from NT
-
-    double maxYawVelocity = 0.3; // max yaw input
-
-    double error = VISION.getCorrectedHeading();
-    boolean isGood =
-        VISION.getCorrectedRange() >= 0; // check if range is good (we have a target), not -1*/
     double yaw;
-
-    /*if (isGood) {
-
-      yaw = error * kP; // corrected heading is error from camera center
-
-      // normalize yaw
-      if (yaw > maxYawVelocity) {
-        yaw = maxYawVelocity;
-      } else if (yaw < -maxYawVelocity) {
-        yaw = -maxYawVelocity;
-      }
-    } else {*/
     yaw = yawExpo.apply(controls.getYaw());
-    // }
-
-    // forward and strafe are still normal
     double forward = driveExpo.apply(controls.getForward());
     double strafe = driveExpo.apply(controls.getStrafe());
 
+    forward = maximum(forward, MAX_FWD_STR);
+    strafe = maximum(strafe, MAX_FWD_STR);
+    yaw = maximum(yaw, MAX_YAW);
+
     DRIVE.drive(forward, strafe, yaw);
   }
 
@@ -83,4 +67,10 @@ private double deadband(double value) {
     if (Math.abs(value) < DEADBAND) return 0.0;
     return value;
   }
+
+  private double maximum(double value, double max) {
+    if (value > max) return max;
+    else if (value < -max) return -max;
+    else return value;
+  }
 }

docs/2019-driver-controls.JPG

@@ -5,7 +5,6 @@
 import edu.wpi.first.wpilibj.command.Command;
 import frc.team2767.deepspace.command.ZeroGyroCommand;
 import frc.team2767.deepspace.command.approach.sequences.AutoHatchPickupCommandGroup;
-import frc.team2767.deepspace.command.approach.sequences.AutoHatchPlaceCommandGroup;
 import frc.team2767.deepspace.command.biscuit.BiscuitNegativeCommand;
 import frc.team2767.deepspace.command.biscuit.BiscuitPositiveCommand;
 import frc.team2767.deepspace.command.biscuit.BiscuitStopCommand;
@@ -88,7 +87,8 @@ public class DriverControls {
 
     // gamepiece place
     new JoystickButton(joystick, Shoulder.LEFT_DOWN.id).whenPressed(new ReleaseGamepieceCommand());
-    new JoystickButton(joystick, Shoulder.LEFT_UP.id).whenPressed(new AutoHatchPlaceCommandGroup());
+    // new JoystickButton(joystick, Shoulder.LEFT_UP.id).whenPressed(new
+    // AutoHatchPlaceCommandGroup());
     // new JoystickButton(joystick, Shoulder.LEFT_UP.id).whenReleased(new InterruptCommand());
   }