Merge pull request #61 from frc3197/mke-prep

MKE Prep and testing systems

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

@@ -46,15 +46,15 @@ public class OI {
 
         static {
 
-                driverA.whenPressed(Robot.driveTrain.changeDriveMode);
+                // driverA.whenPressed(Robot.driveTrain.changeDriveMode);
 
-                driverY.whileHeld(Robot.autoClimb);
+                // driverY.whileHeld(Robot.autoClimb);
 
                 secondaryX.whenPressed(Robot.arm.toggleGyro);
 
                 secondaryY.whenPressed(Robot.arm.resetEncoder);
                 secondaryY.whenPressed(Robot.elevator.reset);
-                secondaryB.whenPressed(Robot.arm.resetGyro);
+                secondaryY.whenPressed(Robot.arm.resetGyro);
 
                 /**
                  * If the right bumper is pushed, then the cargo intake will move. If the right

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

@@ -1,6 +1,5 @@
 package org.team3197.frc2019.robot;
 
-import org.team3197.frc2019.robot.commands.GyroClimb;
 import org.team3197.frc2019.robot.commands.test.DriveTrainTest;
 import org.team3197.frc2019.robot.subsystems.Arm;
 import org.team3197.frc2019.robot.subsystems.Climber;
@@ -10,9 +9,9 @@
 import org.team3197.frc2019.robot.subsystems.Hatch;
 import org.team3197.frc2019.robot.subsystems.Intake;
 
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.command.Scheduler;
@@ -28,9 +27,10 @@ public class Robot extends TimedRobot {
   public static final Erector erector = new Erector();
   public static final Climber climber = new Climber();
 
-  public static final GyroClimb autoClimb = new GyroClimb(climber);
+  // public static final GyroClimb autoClimb = new GyroClimb(climber);
 
-  public static final PowerDistributionPanel pdp = new PowerDistributionPanel();
+  // public static final PowerDistributionPanel pdp = new
+  // PowerDistributionPanel();
 
   public static final NetworkTableInstance ntInst = NetworkTableInstance.getDefault();
 
@@ -40,6 +40,7 @@ public class Robot extends TimedRobot {
 
   @Override
   public void robotInit() {
+    CameraServer.getInstance().addAxisCamera("driver-camera", "10.31.97.7");
   }
 
   @Override

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(-38), kHatchThree(-38), kCargoOne(-38), kCargoTwo(-38), kCargoThree(-38), kCargoShip(-38);
+    kHatchOne(-17), kHatchTwo(-150), kHatchThree(-150), kCargoOne(-150), kCargoTwo(-150), kCargoThree(-150),
+    kCargoShip(-38);
     public double elbowPos;
 
     private ElbowPreset(double elbowPos) {
@@ -27,7 +28,7 @@ private ElbowPreset(double elbowPos) {
   }
 
   public static enum ElevatorPreset {
-    kHatchLevelOne(-27), kHatchLevelTwo(-12.5), kHatchLevelThree(30), kCargoLevelOne(20), kCargoLevelTwo(20),
+    kHatchLevelOne(-26), kHatchLevelTwo(-18), kHatchLevelThree(27), kCargoLevelOne(-26), kCargoLevelTwo(19),
     kCargoLevelThree(53), kCargoLoadingLevel(7.85), kCargoShipCargo(25);
     public double pos;
 
@@ -37,7 +38,7 @@ private ElevatorPreset(double pos) {
   }
 
   public static enum DeadbandType {
-    kElevator(0.03), kElbow(0.06), kWrist(0.06), kDrive(0.08), kClimberVertical(0.05), kErector(0.05),
+    kElevator(0.05), kElbow(0.06), kWrist(0.06), kDrive(0.08), kClimberVertical(0.05), kErector(0.05),
     kClimberGyroVal(5);
     public double speed;
 
@@ -60,7 +61,7 @@ private Channel(int channel) {
   };
 
   public static enum GyroSensitivity {
-    kDrive(0.01), kArm(-0.005), kClimber(0.005);
+    kDrive(0.01), kArm(-0.015), kClimber(0.005);
     public double val;
 
     private GyroSensitivity(double val) {

src/main/java/org/team3197/frc2019/robot/subsystems/Arm.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 Arm extends Subsystem implements Drivable {
 
@@ -31,14 +32,15 @@ public class Arm extends Subsystem implements Drivable {
 
   private boolean useGyro = true;
 
-  final double maxRPM = 3360;
+  final double maxRPM = 6000;
 
   public Arm() {
     super();
 
     elbow.setIdleMode(IdleMode.kBrake);
     wrist.setIdleMode(IdleMode.kBrake);
 
+    elbow.setInverted(true);
     wrist.setInverted(false);
 
     final double kP = 5e-5;
@@ -55,6 +57,13 @@ public Arm() {
     elbow.getPIDController().setIZone(kIz);
     elbow.getPIDController().setFF(kFF);
     elbow.getPIDController().setOutputRange(kMinOutput, kMaxOutput);
+
+    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
@@ -81,13 +90,14 @@ public void elbow(double speed) {
         referenceEncVal = elbow.getEncoder().getPosition();
       }
 
-      elbow.getPIDController().setReference(0, ControlType.kSmartVelocity);
+      elbow.getPIDController().setReference(0, ControlType.kVelocity);
     } else {
       pidLast = false;
       double rpm = output * maxRPM;
-      elbow.getPIDController().setReference(rpm, ControlType.kSmartVelocity);
+      // elbow.getPIDController().setReference(rpm, ControlType.kVelocity);
+      elbow.set(output);
     }
-    // SmartDashboard.putNumber("ElbowEncoder", getElbowEncoderPosition());
+    SmartDashboard.putNumber("ElbowEncoder", getElbowEncoderPosition());
 
   }
 
@@ -96,18 +106,21 @@ public void wrist(double speed) {
 
     double deltaAngle = getAngle();
     double gyroSpeed = GyroSensitivity.kArm.val * deltaAngle;
-    // SmartDashboard.putNumber("wristGyroSpeed", gyroSpeed);
-    // SmartDashboard.putNumber("deltaAngle", deltaAngle);
-    // SmartDashboard.putNumber("WristEncoder", getWristEncoderPosition());
-
-    if (useGyro && Math.abs(output) < DeadbandType.kWrist.speed) {
-      output = gyroSpeed;
+    SmartDashboard.putNumber("wristGyroSpeed", gyroSpeed);
+    SmartDashboard.putNumber("deltaAngle", deltaAngle);
+    SmartDashboard.putNumber("WristEncoder", getWristEncoderPosition());
+
+    if (Math.abs(output) < DeadbandType.kWrist.speed) {
+      if (useGyro) {
+        wrist.set(gyroSpeed);
+      } else {
+        wrist.getPIDController().setReference(0, ControlType.kVelocity);
+      }
     } else {
       resetGyroAngle();
+      wrist.set(output);
     }
 
-    // SmartDashboard.putNumber("output", output);
-    wrist.set(-output);
   }
 
   private double getAngle() {
@@ -155,5 +168,6 @@ private void resetGyroAngle() {
 
   private void toggleGyro() {
     useGyro = !useGyro;
+
   }
 }

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

@@ -18,7 +18,7 @@ public class Climber extends Subsystem {
   private CANSparkMax vertical = new CANSparkMax(CANSparkMaxID.kLiftVertical.id, MotorType.kBrushless);
   private CANSparkMax horizontal = new CANSparkMax(CANSparkMaxID.kLiftHorizontal.id, MotorType.kBrushless);
 
-  private AnalogGyro gyro = new AnalogGyro(Channel.kClimberGyro.channel);
+  // private AnalogGyro gyro = new AnalogGyro(Channel.kClimberGyro.channel);
 
   public FunctionCommand resetGyro = new FunctionCommand(this::resetGyroAngle);
 
@@ -81,10 +81,12 @@ public void driveHorizontal(double speed) {
   }
 
   public double getAngle() {
-    return gyro.getAngle();
+    // return gyro.getAngle();
+    return 0;
   }
 
   private void resetGyroAngle() {
-    gyro.reset();
+    // gyro.reset();
+
   }
 }

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

@@ -5,7 +5,6 @@
 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.DeadbandType;
@@ -16,6 +15,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);
@@ -29,34 +29,76 @@ public class Elevator extends Subsystem implements Drivable {
   private TriggerWrapper limitReset = new TriggerWrapper(bottomLimit::get);
   public FunctionCommand reset = new FunctionCommand(this::resetEncoderPosition);
 
+  public final double maxRPM = 600;
+
   public Elevator() {
     super();
 
     left.setIdleMode(IdleMode.kBrake);
     right.setIdleMode(IdleMode.kBrake);
 
-    left.follow(right, true);
+    left.setInverted(true);
+
+    // left.follow(right, true);
     limitReset.whenActive(reset);
+
+    final double kP = 5e-3;
+    final double kI = 1e-6;
+    final double kD = 0;
+    final double kIz = 0;
+    final double kFF = 0.000156;
+    final double kMaxOutput = 1;
+    final double kMinOutput = -1;
+
+    right.getPIDController().setP(kP);
+    right.getPIDController().setI(kI);
+    right.getPIDController().setD(kD);
+    right.getPIDController().setIZone(kIz);
+    right.getPIDController().setFF(kFF);
+    right.getPIDController().setOutputRange(kMinOutput, kMaxOutput);
+
+    left.getPIDController().setP(kP);
+    left.getPIDController().setI(kI);
+    left.getPIDController().setD(kD);
+    left.getPIDController().setIZone(kIz);
+    left.getPIDController().setFF(kFF);
+    left.getPIDController().setOutputRange(kMinOutput, kMaxOutput);
   }
 
   @Override
   public void initDefaultCommand() {
     setDefaultCommand(new Elevate(this));
   }
 
+  private boolean lastEnc = false;
+  private double currentEncRight = 0;
+  private double currentEncLeft = 0;
+
   public void drive(double speed, boolean hold) {
     // SmartDashboard.putNumber("speed", speed);
-    // SmartDashboard.putNumber("ElevatorEncoder", getEncoderPosition());
-
-    double output = speed;
-
-    if (Math.abs(speed) < DeadbandType.kElevator.speed) {
-      right.getPIDController().setReference(0, ControlType.kSmartMotion);
+    SmartDashboard.putNumber("ElevatorEncoder", getEncoderPosition());
+
+    // if (Math.abs(speed) < DeadbandType.kElevator.speed) {
+    // if (!lastEnc) {
+    // lastEnc = true;
+    // currentEncRight = right.getEncoder().getPosition();
+    // currentEncLeft = right.getEncoder().getPosition();
+    // }
+    // right.getPIDController().setReference(currentEncRight,
+    // ControlType.kPosition);
+    // left.getPIDController().setReference(currentEncLeft, ControlType.kPosition);
+    // } else {
+    // // right.getEncoder().setPosition(0);
+    // lastEnc = false;
+    // double rpm = speed * maxRPM;
+    // right.getPIDController().setReference(rpm, ControlType.kVelocity);
+    // left.getPIDController().setReference(-rpm, ControlType.kVelocity);
+    // }
+    if (hold && Math.abs(speed) < DeadbandType.kElevator.speed) {
+      elevatorGroup.set(DeadbandType.kElevator.speed);
     } else {
-      right.getPIDController().setReference(speed, ControlType.kDutyCycle);
+      elevatorGroup.set(speed);
     }
-
-    elevatorGroup.set(output);
   }
 
   public void resetEncoderPosition() {

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

@@ -11,20 +11,25 @@
 import org.team3197.frc2019.robot.utilities.Drivable;
 import org.team3197.frc2019.robot.utilities.FunctionCommand;
 
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.command.Subsystem;
 
 public class Erector extends Subsystem implements Drivable {
 
   private CANSparkMax left = new CANSparkMax(RobotMap.CANSparkMaxID.kErectorLeft.id, MotorType.kBrushless);
   private CANSparkMax right = new CANSparkMax(RobotMap.CANSparkMaxID.kErectorRight.id, MotorType.kBrushless);
 
+  private SpeedControllerGroup group = new SpeedControllerGroup(left, right);
+
   public Erector() {
     super();
 
     left.setIdleMode(IdleMode.kBrake);
     right.setIdleMode(IdleMode.kBrake);
 
-    right.follow(left, true);
+    // right.follow(left, true);
+
+    right.setInverted(true);
 
     final double kP = 5e-2;
     final double kI = 1e-4;
@@ -40,6 +45,7 @@ public Erector() {
     left.getPIDController().setIZone(kIz);
     left.getPIDController().setFF(kFF);
     left.getPIDController().setOutputRange(kMinOutput, kMaxOutput);
+
     right.getPIDController().setP(kP);
     right.getPIDController().setI(kI);
     right.getPIDController().setD(kD);
@@ -76,7 +82,7 @@ public void drive(double speed, boolean hold) {
     } else {
       stoppedLast = false;
       // left.getPIDController().setReference(speed, ControlType.kDutyCycle);
-      left.set(speed);
+      group.set(speed);
     }
   }
 }