Resolve merge conflict in OI.java using the auto-importer

Author: dfang314

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

@@ -7,9 +7,11 @@
 import java.io.IOException;
 import java.io.Reader;
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 import org.apache.commons.math3.stat.regression.OLSMultipleLinearRegression;
+import org.apache.commons.math3.linear.SingularMatrixException;
 import org.apache.commons.lang3.ArrayUtils;
 import org.apache.commons.csv.CSVRecord;
 import org.apache.commons.csv.CSVFormat;
@@ -64,9 +66,14 @@ public static void ordinaryLeastSquares(String file1, String file2, String outfi
             ys[i] = voltages[i];
         }
 
-        algorithm = new OLSMultipleLinearRegression();
-        algorithm.newSampleData(ys, xs);
-        params = algorithm.estimateRegressionParameters();
+        try {
+            algorithm = new OLSMultipleLinearRegression();
+            algorithm.newSampleData(ys, xs);
+            params = algorithm.estimateRegressionParameters();
+        } catch (SingularMatrixException e) {
+            System.out.println(Arrays.deepToString(xs));
+            System.out.println(Arrays.toString(ys));
+        }
         // System.out.println(params.length);
         rightKv = params[1];
         rightKa = params[2];

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

@@ -16,6 +16,7 @@
 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;
@@ -57,7 +58,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/RobotMap.java

@@ -124,11 +124,6 @@ private static WPI_TalonSRX createConfiguredTalon(int port) {
     catchError(tsrx.configNeutralDeadband(0.001, 10));
     tsrx.setNeutralMode(NeutralMode.Brake);
 
-    ecDeadband = tsrx.configNeutralDeadband(0.001, 10);
-    if (!ecDeadband.equals(ErrorCode.OK)) {
-      throw new RuntimeException(ecDeadband.toString());
-    }
-
     return tsrx;
   }
 
@@ -144,11 +139,6 @@ private static WPI_VictorSPX createConfiguredVictor(int port) {
     catchError(vspx.configNeutralDeadband(0.001, 10));
     vspx.setNeutralMode(NeutralMode.Brake);
 
-    ecDeadband = vspx.configNeutralDeadband(0.001, 10);
-    if (!ecDeadband.equals(ErrorCode.OK)) {
-      throw new RuntimeException(ecDeadband.toString());
-    }
-
     return vspx;
   }
 

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

@@ -19,11 +19,12 @@ public class Climb extends Command {
   private State state;
 
   private final double backDrive = -0.5; // TODO: Set this to reasonable/tested value;
-  private final double climbUp = 0.5; // TODO: Set this to reasonable/tested value;
-  private final double climbDown = -0.5; // TODO: Set this to reasonable/tested value;
-  private final double retract = -0.5; // TODO: Set this to reasonable/tested value;
+  private final double climbUp = 1;
+  private final double climbDown = -1;
+  private final double retract = -1;
   private final double overrideThreshold = 0.1; // TODO: Set this to reasonable/tested value;
   private final double retractGoal = 0; // TODO: Set this to reasonable/tested value;
+  private final double offGroundHeight = 10; // TODO: Set this to reasonable/tested value;
 
   public Climb(Climber climber, Drivetrain dt, Joystick joy) {
     // Use requires() here to declare subsystem dependencies
@@ -102,7 +103,7 @@ protected void interrupted() {
    *         overriding the climb
    */
   private boolean robotOnPlatform() {
-    return dt.isStalled() || Math.abs(dtJoy.getY()) > overrideThreshold;
+    return (dt.isStalled() && climber.getEncDistance() > offGroundHeight) || Math.abs(dtJoy.getY()) > overrideThreshold;
   }
 
   private enum State {

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;
   }
@@ -198,6 +209,22 @@ public void updateDrivetrainParameters() {
       blKA = avg;
       brKA = avg;
     } catch (FileNotFoundException e) {
+      flKV = 0.06369046755507658;
+      flKA = 0.0215894793277297;
+      flVI = 0.8403701236277824;
+
+      frKV = 0.0619423013628032;
+      frKA = 0.04044703465602449;
+      frVI = 0.810212379284332;
+
+      blKV = 0.06388520699977113;
+      blKA = 0.025492804438184545;
+      blVI = 0.8071078220643216;
+
+      brKV = 0.06140765089854154;
+      brKA = 0.042046502553651215;
+      brVI = 0.7929289166816246;
+      
       e.printStackTrace();
     }
   }