Merge pull request #65 from frc3197/detroit-bot-tweaks

Detroit bot tweaks
frc3197 · May 7, 2019 · 73f449d · 73f449d
2 parents 9e5f2cb + 0b368ed
commit 73f449d
Show file tree

Hide file tree

Showing 6 changed files with 31 additions and 12 deletions.
diff --git a/src/main/java/org/team3197/frc2019/robot/OI.java b/src/main/java/org/team3197/frc2019/robot/OI.java
@@ -64,7 +64,7 @@ public class OI {
         }
 
         public static double arcadeDriveY() {
-                return driver.getY(Hand.kRight);
+                return driver.getY(Hand.kRight) * MaxSpeeds.kDrive.forwardSpeed;
         }
 
         public static double arcadeDriveR() {

diff --git a/src/main/java/org/team3197/frc2019/robot/RobotMap.java b/src/main/java/org/team3197/frc2019/robot/RobotMap.java
@@ -18,7 +18,8 @@ private CANSparkMaxID(int id, String name) {
   };
 
   public static enum ElbowPreset {
-    kHatchOne(-17), kHatchTwo(-35), kHatchThree(-35), kCargoOne(-35), kCargoTwo(-35), kCargoThree(-35), kCargoShip(-35);
+    kHatchOne(-17), kHatchTwo(-146), kHatchThree(-146), kCargoOne(-35), kCargoTwo(-35), kCargoThree(-35),
+    kCargoShip(-35);
     public double elbowPos;
 
     private ElbowPreset(double elbowPos) {
@@ -27,7 +28,7 @@ private ElbowPreset(double elbowPos) {
   }
 
   public static enum ElevatorPreset {
-    kHatchLevelOne(-26), kHatchLevelTwo(-10), kHatchLevelThree(33), kCargoLevelOne(-26), kCargoLevelTwo(19),
+    kHatchLevelOne(-26), kHatchLevelTwo(-18), kHatchLevelThree(25), kCargoLevelOne(-26), kCargoLevelTwo(19),
     kCargoLevelThree(53), kCargoLoadingLevel(7.85), kCargoShipCargo(25);
     public double pos;
 
@@ -69,8 +70,8 @@ private GyroSensitivity(double val) {
   };
 
   public static enum MaxSpeeds {
-    kElevator(.7, -.5), kArm(.38), kWrist(.10), kHatch(1), kCargo(1), kClimberVertical(.7), kClimberHorizontal(.7),
-    kErector(.3);
+    kDrive(.9), kElevator(.85, -.6), kArm(.5), kWrist(.20), kHatch(1), kCargo(1), kClimberVertical(.7),
+    kClimberHorizontal(.7), kErector(.3);
 
     public double forwardSpeed;
     public double reverseSpeed;
@@ -90,6 +91,8 @@ private MaxSpeeds(double speed) {
 
   public static final double elbowPresetThreshold = 1.5;
 
+  public static final double elbowVelocityPresetThreshold = 60;
+
   public static final double elevatorDegreeSensitivity = 0.05;
 
   public static final double elbowDegreeSensitivity = 0.05;

diff --git a/src/main/java/org/team3197/frc2019/robot/commands/ExtendToPreset.java b/src/main/java/org/team3197/frc2019/robot/commands/ExtendToPreset.java
@@ -38,7 +38,7 @@ public ExtendToPreset(ElbowPreset target, ElbowPreset targetWithTrigger, Trigger
   @Override
   protected void execute() {
     double elbowSpeed = getElbowSpeed();
-    elbow.drive(elbowSpeed, true);
+    elbow.drive(elbowSpeed, false);
   }
 
   @Override
@@ -61,6 +61,8 @@ private double getElbowSpeed() {
 
     double error = elbow.getElbowEncoderPosition() - currentTarget.elbowPos;
     finished = Math.abs(error) < RobotMap.elbowPresetThreshold;
+    // finished = Math.abs(elbow.getVelocity()) <
+    // RobotMap.elbowVelocityPresetThreshold;
 
     double speed = -RobotMap.elbowDegreeSensitivity * error;
     return speed;

diff --git a/src/main/java/org/team3197/frc2019/robot/subsystems/Elbow.java b/src/main/java/org/team3197/frc2019/robot/subsystems/Elbow.java
@@ -20,7 +20,7 @@ public class Elbow extends Subsystem implements Drivable {
 
   public FunctionCommand resetEncoder = new FunctionCommand(this::resetEncoderPosition);
 
-  final double maxRPM = 3000;
+  final double maxRPM = 5000;
 
   public Elbow() {
     super();
@@ -55,7 +55,7 @@ public void initDefaultCommand() {
   double referenceEncVal = 0;
 
   public void drive(double speed, boolean hold) {
-    double output = speed;
+    double output = -speed;
 
     if (!pidLast) {
       referenceEncVal = elbow.getEncoder().getPosition();
@@ -67,10 +67,20 @@ public void drive(double speed, boolean hold) {
       elbow.getPIDController().setReference(0, ControlType.kVelocity);
     } else {
       double rpm = output * maxRPM;
-      elbow.getPIDController().setReference(rpm, ControlType.kVelocity);
+      SmartDashboard.putNumber("rpm", rpm);
+      // if (hold) {
+      // elbow.getPIDController().setReference(rpm, ControlType.kSmartVelocity);
+      // } else {
+      // elbow.set(output);
+      // }
+      elbow.set(output);
     }
+    // elbow.set(output);
+    // elbow.getPIDController().setReference(maxRPM * output,
+    // ControlType.kVelocity);
 
     SmartDashboard.putNumber("ElbowEncoder", getElbowEncoderPosition());
+    SmartDashboard.putNumber("ElbowVelocity", getVelocity());
   }
 
   double resetElbowEncoderPosition = 0;
@@ -79,6 +89,10 @@ public double getElbowEncoderPosition() {
     return elbow.getEncoder().getPosition() - resetElbowEncoderPosition;
   }
 
+  public double getVelocity() {
+    return elbow.getEncoder().getVelocity();
+  }
+
   private void resetEncoderPosition() {
     resetElbowEncoderPosition = elbow.getEncoder().getPosition();
   }

diff --git a/src/main/java/org/team3197/frc2019/robot/subsystems/Intake.java b/src/main/java/org/team3197/frc2019/robot/subsystems/Intake.java
@@ -16,7 +16,7 @@ public class Intake extends Subsystem implements Drivable {
 
   public Intake() {
     roller.setIdleMode(IdleMode.kBrake);
-    roller.setInverted(true);
+    roller.setInverted(false);
   }
 
   @Override

diff --git a/src/main/java/org/team3197/frc2019/robot/subsystems/Wrist.java b/src/main/java/org/team3197/frc2019/robot/subsystems/Wrist.java
@@ -60,14 +60,14 @@ public void drive(double speed, boolean hold) {
 
     if (Math.abs(output) < DeadbandType.kWrist.speed) {
       if (useGyro) {
-        wrist.set(gyroSpeed);
+        wrist.set(-gyroSpeed);
       } else {
         wrist.getPIDController().setReference(0, ControlType.kVelocity);
       }
     } else {
       resetGyroAngle();
       SmartDashboard.putNumber("WristSpeed", output);
-      wrist.set(output);
+      wrist.set(-output);
     }
 
   }