Commit Before Worlds

- Modified Limelight code to use a new function.
- Implemented automatic climbing code into the Climb command.

-- What happened to the commits before champs, like AZNorth and Ventura? __The world may never know.__

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2019.2.1"
+    id "edu.wpi.first.GradleRIO" version "2019.4.1"
 }
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"

src/main/java/frc/auto/commands/Climb.java

@@ -0,0 +1,76 @@
+package frc.auto.commands;
+
+import java.util.HashMap;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.auto.commands.utils.Command;
+import frc.auto.commands.utils.ICommand;
+import frc.robot.RobotMap;
+
+/**
+ * Drives the left and right motors a specified distance for a specified time.
+ */
+@Command(name = "Climb")
+public class Climb extends ICommand {
+    NetworkTable nt; // Used for debugging by printing messages.
+    boolean isFront; // Switch for whether the front elevator is active.
+
+    /**
+     * Empty constructor for use in serialization. Do not add anything to this!
+     * ... Do not call past midnight.
+     */
+    public Climb(HashMap<String, Double> map) { 
+        super(map);
+    }
+
+    /**
+     * Creates a new instance of Climb with the following parameters.
+     * When run() is called, the robot will wobble itself onto the habitat.
+     * Once the duration has exceeded the runtime, the command will return.
+     * @param angleSetpoint Target angle for climbing.
+     * @param tiltThreshold Toggle climbing direction when the error exceeds the threshold.
+     * @param duration Duration to climb for before timing out. Not yet implemented.
+     */
+    public Climb(double angleSetpoint, double tiltThreshold, double duration) {
+        super();
+        this.parameters.put("angleSetpoint",  angleSetpoint);
+        this.parameters.put("tiltThreshold",  tiltThreshold);
+        this.parameters.put("duration",       duration);
+
+        nt = NetworkTableInstance.getDefault().getTable("datapublisher");
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    @Override
+    public void run() {
+        RobotMap.logger.printStatus("Attempting to climb. Wish me luck!");
+
+        double error = this.parameters.get("angleSetpoint") - RobotMap.pigeon.getPitch();
+        nt.getEntry("Climb Error").setNumber(error);
+
+        if(isFront == true && error >= this.parameters.get("tiltThreshold")) {
+            isFront = !isFront;
+            RobotMap.logger.printDebug("Set direction on the climber to FALSE.");
+        }
+        else if(error < this.parameters.get("tiltThreshold")) {
+            isFront = !isFront;
+            RobotMap.logger.printDebug("Set direction on the climber to TRUE.");
+        }
+
+        if(isFront == true) {
+            RobotMap.climber.disableArm();
+            RobotMap.elevator.setInnerSpeed(-0.85);
+        }
+        else {
+            RobotMap.climber.enableArm();
+            RobotMap.elevator.setInnerSpeed(0);
+        }
+
+        RobotMap.climber.climbRollers.set(ControlMode.PercentOutput, 0.75);
+    }
+}

src/main/java/frc/enums/ElevatorPositions.java

@@ -1,12 +1,15 @@
 package frc.enums;
 
 /**
- * ElevatorPositions
+ * The main elevator has two values: inner elevator and outer elevator.
+ * The climbing elevator has one value: inner elevator. The second value is unused.
  */
 public enum ElevatorPositions {
-    CARGO_LOW(15837, 3382), CARGO_MEDIUM(4500, -3554), CARGO_HIGH(26331, -19072),
-    HATCH_LOW(0, 0), HATCH_MEDIUM(0, 0), HATCH_HIGH(0, 0),
-    NEUTRAL(704, 3378);
+    CARGO_LOW(-9500, 0), CARGO_MEDIUM(-900, 9000), CARGO_HIGH(150, 23500),
+    HATCH_LOW(-22014, 0), HATCH_MEDIUM(-4500, 0), HATCH_HIGH(-316, 15302),
+    CARGO_SHIP(-2000, 0),
+    CLIMB_NEUTRAL(-10, 0), CLIMB_ENABLED(-895668, 0), CLIMB_MAIN(-24000, 0),
+    NEUTRAL(-22014, 0);
 
     private final int innerEncoderPos;
     private final int outerEncoderPos;

src/main/java/frc/enums/IDs.java

@@ -7,13 +7,15 @@ public enum IDs {
     // Talon SRX IDs
     LEFT(1), LEFT_FOLLOWER(3), RIGHT(2), RIGHT_FOLLOWER(4),
     ELEVATOR_INNER(10), ELEVATOR_OUTER(11), ROLLERS(12),
+    CLIMB_ROLLERS(30), CLIMB_FOLLOWER(16), CLIMB_ARM(33),
 
     // Pneumatic IDs
     DRIVE_SHIFTER(4), PCM(21), ELEVATOR_PISTONS(5), VACUUM_ENABLE(6),
-    VACUUM_DISABLE(7),
+    VACUUM_DISABLE(7), CLIMB_PISTON(3),
 
     // Sensor IDs
-    ELEVATOR_ULTRASONIC(3), ELEVATOR_DIGITAL(0),
+    ELEVATOR_ULTRASONIC(3), ELEVATOR_DIGITAL(0), ELEVATOR_LIGHT(2), 
+    PIGEON(31),
 
     // Input Device IDs
     LEFT_JOYSTICK(0), RIGHT_JOYSTICK(1), SECONDARY_OPERATOR(2);

src/main/java/frc/enums/PIDFeedback.java

@@ -4,7 +4,7 @@
  * Provides PIDF values for different subsystems.
  */
 public enum PIDFeedback {
-    INNER_ELEVATOR(0.2, 0, 0, 0), OUTER_ELEVATOR(0, 0, 0, 0);
+    INNER_ELEVATOR(0.1, 0, 0, 0), OUTER_ELEVATOR(0.35, 0, 0, 0);
 
     private final double kP;
     private final double kI;

src/main/java/frc/opmode/Autonomous.java

@@ -6,6 +6,8 @@
 
 import com.google.common.base.Stopwatch;
 
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import frc.robot.RobotMap;
 
@@ -14,6 +16,8 @@
  */
 public class Autonomous extends OpMode {
 
+    NetworkTable instance;
+
     public Autonomous() {
         super();
     }
@@ -24,6 +28,13 @@ public void init() {
         RobotMap.compressor.set(false);
         RobotMap.driveTrain.zeroSensors();
         driveTrain.configPeakOutput(0.3);
+        RobotMap.elevator.elevatorForward();
+
+        instance = NetworkTableInstance.getDefault().getTable("parameters");
+        instance.getEntry("innerSpeed").setDouble(0);
+        instance.getEntry("outerSpeed").setDouble(0);
+        instance.getEntry("innerSetpoint").setDouble(-22014);
+        instance.getEntry("outerSetpoint").setDouble(0);
     }
 
     @Override
@@ -33,7 +44,21 @@ public void loop() {
         // RobotMap.driveTrain.set(-1, -1);
         // sleep(5000);
 
-        limelight.drive();
+        //limelight.drive();
+
+        // double innerSpeed = instance.getEntry("innerSpeed").getDouble(0);
+        // double outerSpeed = instance.getEntry("outerSpeed").getDouble(0);
+
+        // elevator.setInnerSpeed(innerSpeed);
+        // elevator.setOuterSpeed(outerSpeed);
+
+        // double innerSetpoint = instance.getEntry("innerSetpoint").getDouble(0);
+        // double outerSetpoint = instance.getEntry("outerSetpoint").getDouble(0);
+
+        // elevator.innerStage.set(ControlMode.Position, innerSetpoint);
+        // elevator.outerStage.set(ControlMode.Position, outerSetpoint);
+
+
     }
 
     @SuppressWarnings("unused")

src/main/java/frc/opmode/Disabled.java

@@ -1,5 +1,7 @@
 package frc.opmode;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+
 import frc.robot.RobotMap;
 
 /**
@@ -13,13 +15,14 @@ public Disabled() {
 
     @Override
     public void init() {
+        RobotMap.elevator.innerStage.setNeutralMode(NeutralMode.Coast);
         RobotMap.limelight.setLights(false);
+        RobotMap.limelight.setMode(true);
     }
 
     @Override
     public void loop() {
-
+        RobotMap.elevator.zeroEncoder();
+        RobotMap.climber.zeroEncoder();        
     }
-
-
 }