Merge pull request #63 from frc3197/detroit-prep

Detroit prep

src/main/java/org/team3197/frc2019/robot/OI.java

@@ -94,8 +94,8 @@ public static double wristSpeed() {
         }
 
         public static double erectorSpeed() {
-                return driver.getTriggerAxis(Hand.kLeft)
-                                - driver.getTriggerAxis(Hand.kRight) * RobotMap.erectorSpeedMultiplier;
+                return (driver.getTriggerAxis(Hand.kLeft) - driver.getTriggerAxis(Hand.kRight))
+                                * RobotMap.MaxSpeeds.kErector.forwardSpeed;
         }
 
         public static double manipulatorSpeed() {

src/main/java/org/team3197/frc2019/robot/RobotMap.java

@@ -18,8 +18,7 @@ private CANSparkMaxID(int id, String name) {
   };
 
   public static enum ElbowPreset {
-    kHatchOne(-17), kHatchTwo(-150), kHatchThree(-150), kCargoOne(-150), kCargoTwo(-150), kCargoThree(-150),
-    kCargoShip(-38);
+    kHatchOne(-17), kHatchTwo(-35), kHatchThree(-35), kCargoOne(-35), kCargoTwo(-35), kCargoThree(-35), kCargoShip(-35);
     public double elbowPos;
 
     private ElbowPreset(double elbowPos) {
@@ -28,7 +27,7 @@ private ElbowPreset(double elbowPos) {
   }
 
   public static enum ElevatorPreset {
-    kHatchLevelOne(-26), kHatchLevelTwo(-18), kHatchLevelThree(27), kCargoLevelOne(-26), kCargoLevelTwo(19),
+    kHatchLevelOne(-26), kHatchLevelTwo(-10), kHatchLevelThree(33), kCargoLevelOne(-26), kCargoLevelTwo(19),
     kCargoLevelThree(53), kCargoLoadingLevel(7.85), kCargoShipCargo(25);
     public double pos;
 
@@ -38,7 +37,7 @@ private ElevatorPreset(double pos) {
   }
 
   public static enum DeadbandType {
-    kElevator(0.05), kElbow(0.06), kWrist(0.06), kDrive(0.08), kClimberVertical(0.05), kErector(0.05),
+    kElevator(0.05), kElbow(0.06), kWrist(0.05), kDrive(0.08), kClimberVertical(0.05), kErector(0.05),
     kClimberGyroVal(5);
     public double speed;
 
@@ -52,7 +51,7 @@ public static enum DriveTrainSide {
   }
 
   public static enum Channel {
-    kWristGyro(0), kClimberGyro(1);
+    kWristGyro(0);
     public final int channel;
 
     private Channel(int channel) {
@@ -61,7 +60,7 @@ private Channel(int channel) {
   };
 
   public static enum GyroSensitivity {
-    kDrive(0.01), kArm(-0.015), kClimber(0.005);
+    kDrive(0.01), kArm(-0.011), kClimber(0.005);
     public double val;
 
     private GyroSensitivity(double val) {
@@ -70,7 +69,7 @@ private GyroSensitivity(double val) {
   };
 
   public static enum MaxSpeeds {
-    kElevator(.7, -.5), kArm(.38), kWrist(.10), kHatch(1), kCargo(1), kClimberVertical(.7), kClimberHorizontal(.9),
+    kElevator(.7, -.5), kArm(.38), kWrist(.10), kHatch(1), kCargo(1), kClimberVertical(.7), kClimberHorizontal(.7),
     kErector(.3);
 
     public double forwardSpeed;
@@ -91,9 +90,9 @@ private MaxSpeeds(double speed) {
 
   public static final double elbowPresetThreshold = 1.5;
 
-  public static final double elevatorDegreeSensitivity = 0.15;
+  public static final double elevatorDegreeSensitivity = 0.05;
 
-  public static final double elbowDegreeSensitivity = 0.15;
+  public static final double elbowDegreeSensitivity = 0.05;
 
   public static final double elevatorSpeedMultiplier = 0.5;
 

src/main/java/org/team3197/frc2019/robot/subsystems/Climber.java

@@ -1,15 +1,19 @@
 package org.team3197.frc2019.robot.subsystems;
 
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
+import com.revrobotics.CANDigitalInput;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.ControlType;
 
+import org.team3197.frc2019.robot.RobotMap;
 import org.team3197.frc2019.robot.RobotMap.CANSparkMaxID;
 import org.team3197.frc2019.robot.RobotMap.DeadbandType;
 import org.team3197.frc2019.robot.commands.defaults.Climb;
 
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Climber extends Subsystem {
   private CANSparkMax vertical = new CANSparkMax(CANSparkMaxID.kLiftVertical.id, MotorType.kBrushless);
@@ -19,6 +23,7 @@ public Climber() {
     super();
     vertical.setIdleMode(IdleMode.kBrake);
     horizontal.setIdleMode(IdleMode.kCoast);
+    vertical.setInverted(true);
 
     final double kP = 5e-5;
     final double kI = 1e-6;
@@ -46,14 +51,18 @@ public void initDefaultCommand() {
   double referenceEncVal = 0;
 
   public void driveVertical(double speed) {
-    if (Math.abs(speed) < DeadbandType.kClimberVertical.speed) {
-      if (!pidLast) {
-        pidLast = true;
-        referenceEncVal = vertical.getEncoder().getPosition();
-      }
+    SmartDashboard.putNumber("temp", vertical.getMotorTemperature());
+    if (!pidLast) {
+      referenceEncVal = vertical.getEncoder().getPosition();
+    }
+    if (pidLast = Math.abs(speed) < DeadbandType.kClimberVertical.speed) {
       vertical.getPIDController().setReference(referenceEncVal, ControlType.kPosition);
+      CANDigitalInput limit = vertical.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+      if (limit.get()) {
+        vertical.set(RobotMap.MaxSpeeds.kClimberVertical.reverseSpeed);
+      }
+      // vertical.getPIDController().setReference(0, ControlType.kVelocity);
     } else {
-      pidLast = false;
       vertical.getEncoder().setPosition(0);
       vertical.getPIDController().setReference(speed, ControlType.kDutyCycle);
     }

src/main/java/org/team3197/frc2019/robot/subsystems/Elbow.java

@@ -12,25 +12,26 @@
 import org.team3197.frc2019.robot.utilities.FunctionCommand;
 
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Elbow extends Subsystem implements Drivable {
 
   private CANSparkMax elbow = new CANSparkMax(RobotMap.CANSparkMaxID.kElbow.id, MotorType.kBrushless);
 
   public FunctionCommand resetEncoder = new FunctionCommand(this::resetEncoderPosition);
 
-  final double maxRPM = 6000;
+  final double maxRPM = 3000;
 
   public Elbow() {
     super();
 
     elbow.setIdleMode(IdleMode.kBrake);
 
-    elbow.setInverted(true);
+    elbow.setInverted(false);
 
-    final double kP = 5e-5;
-    final double kI = 1e-6;
-    final double kD = 0;
+    final double kP = 2e-4;
+    final double kI = 1e-7;
+    final double kD = 0.00015;
     final double kIz = 0;
     final double kFF = 0.000156;
     final double kMaxOutput = 1;
@@ -61,12 +62,15 @@ public void drive(double speed, boolean hold) {
     }
 
     if (pidLast = Math.abs(output) < DeadbandType.kElbow.speed) {
-      elbow.getPIDController().setReference(referenceEncVal, ControlType.kPosition);
+      // elbow.getPIDController().setReference(referenceEncVal,
+      // ControlType.kPosition);
+      elbow.getPIDController().setReference(0, ControlType.kVelocity);
     } else {
       double rpm = output * maxRPM;
       elbow.getPIDController().setReference(rpm, ControlType.kVelocity);
     }
 
+    SmartDashboard.putNumber("ElbowEncoder", getElbowEncoderPosition());
   }
 
   double resetElbowEncoderPosition = 0;

src/main/java/org/team3197/frc2019/robot/subsystems/Elevator.java

@@ -12,6 +12,7 @@
 
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Elevator extends Subsystem implements Drivable {
   private CANSparkMax left = new CANSparkMax(RobotMap.CANSparkMaxID.kElevatorLeft.id, MotorType.kBrushless);
@@ -65,6 +66,7 @@ public void drive(double speed, boolean hold) {
     } else {
       elevatorGroup.set(speed);
     }
+    SmartDashboard.putNumber("ElevatorEncoder", getEncoderPosition());
   }
 
   public void resetEncoderPosition() {

src/main/java/org/team3197/frc2019/robot/subsystems/Erector.java

@@ -27,6 +27,8 @@ public Erector() {
     left.setIdleMode(IdleMode.kBrake);
     right.setIdleMode(IdleMode.kBrake);
 
+    // right.follow(left, true);
+
     right.setInverted(true);
 
     final double kP = 5e-2;
@@ -70,6 +72,7 @@ private void resetPID() {
   }
 
   public void drive(double speed, boolean hold) {
+    // SmartDashboard.putNumber("speed1", speed);
     if (hold && Math.abs(speed) < DeadbandType.kErector.speed) {
       if (!stoppedLast) {
         resetPID();
@@ -78,6 +81,7 @@ public void drive(double speed, boolean hold) {
       left.getPIDController().setReference(encoderLastLeft, ControlType.kPosition);
     } else {
       stoppedLast = false;
+      // left.getPIDController().setReference(speed, ControlType.kDutyCycle);
       group.set(speed);
     }
   }

src/main/java/org/team3197/frc2019/robot/subsystems/Intake.java

@@ -15,9 +15,8 @@ public class Intake extends Subsystem implements Drivable {
   private CANSparkMax roller = new CANSparkMax(RobotMap.CANSparkMaxID.kIntake.id, MotorType.kBrushless);
 
   public Intake() {
-    super();
-
     roller.setIdleMode(IdleMode.kBrake);
+    roller.setInverted(true);
   }
 
   @Override

src/main/java/org/team3197/frc2019/robot/subsystems/Wrist.java

@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.command.InstantCommand;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Wrist extends Subsystem implements Drivable {
   private CANSparkMax wrist = new CANSparkMax(RobotMap.CANSparkMaxID.kWrist.id, MotorType.kBrushless);
@@ -34,18 +35,15 @@ public Wrist() {
     final double kD = 0;
     final double kIz = 0;
     final double kFF = 0.000156;
-    final double kMaxOutput = 1;
-    final double kMinOutput = -1;
 
     wrist.setIdleMode(IdleMode.kBrake);
 
-    wrist.setInverted(false);
+    wrist.setInverted(true);
     wrist.getPIDController().setP(kP);
     wrist.getPIDController().setI(kI);
     wrist.getPIDController().setD(kD);
     wrist.getPIDController().setIZone(kIz);
     wrist.getPIDController().setFF(kFF);
-    wrist.getPIDController().setOutputRange(kMinOutput, kMaxOutput);
   }
 
   @Override
@@ -57,6 +55,7 @@ public void drive(double speed, boolean hold) {
     double output = speed;
 
     double deltaAngle = getAngle();
+    SmartDashboard.putNumber("getAngle()", getAngle());
     double gyroSpeed = GyroSensitivity.kArm.val * deltaAngle;
 
     if (Math.abs(output) < DeadbandType.kWrist.speed) {
@@ -67,6 +66,7 @@ public void drive(double speed, boolean hold) {
       }
     } else {
       resetGyroAngle();
+      SmartDashboard.putNumber("WristSpeed", output);
       wrist.set(output);
     }