Merge first (and second) competition code into master

build.gradle

@@ -54,6 +54,8 @@ dependencies {
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
 
+    implementation 'com.google.code.gson:gson:2.9.0'
+
     roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
     roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
 

src/main/deploy/autos/auto-test.json

@@ -0,0 +1,28 @@
+{
+    "auto": [
+        {
+            "type": "drive",
+            "unit": "SECONDS",
+            "amount": 0.1,
+            "args": [
+                -0.05, 
+                -0.05
+            ]
+        },
+        {
+            "type": "wait",
+            "args": [
+                1
+            ]
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": -90,
+            "args": [
+                0.1,
+                -0.1
+            ]
+        }
+    ]
+}

src/main/deploy/autos/autonomous.json

@@ -0,0 +1,13 @@
+{
+    "auto": [
+        {
+            "type": "drive",
+            "unit": "FEET",
+            "amount": 3.00,
+            "args": [
+                -0.1,
+                -0.1
+            ]
+        }
+    ]
+}

src/main/deploy/autos/current-test.json

@@ -0,0 +1,28 @@
+{
+    "auto": [
+        {
+            "type": "drive",
+            "unit": "CURRENT",
+            "amount": 20,
+            "args": [
+                0.05,
+                0.05
+            ]
+        },
+        {
+            "type": "wait",
+            "args": [
+                1
+            ]
+        },
+        {
+            "type": "drive",
+            "unit": "FEET",
+            "amount": 5,
+            "args": [
+                -0.05,
+                -0.05
+            ]
+        }
+    ]
+}

src/main/deploy/autos/distance-test.json

@@ -0,0 +1,76 @@
+{
+    "auto": [
+        {
+            "type": "drive",
+            "unit": "INCHES",
+            "amount": 50,
+            "args": [
+                -0.05,
+                -0.05
+            ]
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": 90,
+            "args": [
+                -0.05,
+                0.05
+            ]
+        },
+        {
+            "type": "drive",
+            "unit": "INCHES",
+            "amount": 50,
+            "args": [
+                -0.05,
+                -0.05
+            ]
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": 90,
+            "args": [
+                -0.05,
+                0.05
+            ]
+        },
+        {
+            "type": "drive",
+            "unit": "INCHES",
+            "amount": 50,
+            "args": [
+                -0.05,
+                -0.05
+            ]
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": 90,
+            "args": [
+                -0.05,
+                0.05
+            ]
+        },
+        {
+            "type": "drive",
+            "unit": "INCHES",
+            "amount": 50,
+            "args": [
+                -0.05,
+                -0.05
+            ]
+        },
+        {
+            "type": "turnDeg",
+            "unit": "DEGREES",
+            "amount": 90,
+            "args": [
+                -0.05,
+                0.05
+            ]
+        }
+    ]
+}

src/main/deploy/autos/shooter-test.json

@@ -0,0 +1,17 @@
+{
+    "auto": [
+        {
+            "type": "shoot",
+            "unit": "SPEED",
+            "args": [
+                1000
+            ]
+        },
+        {
+            "type": "wait",
+            "args": [
+                5
+            ]
+        }
+    ]
+}

src/main/java/frc/robot/Autonomous.java

@@ -0,0 +1,5 @@
+package frc.robot;
+
+public abstract class Autonomous {
+    public abstract void run();
+}

src/main/java/frc/robot/BoostInput.java

@@ -1,21 +1,21 @@
 
 package frc.robot;
 
-public class BoostInput implements InputScaler
-{
-  private final double powerCap;
-  private boolean enabled;
+public class BoostInput implements InputScaler {
+    private final double powerCap;
+    private double boostScale;
 
-  public BoostInput(double powerCap) {
-    this.powerCap = powerCap;
-    this.enabled = false;
-  }
+    public BoostInput(double powerCap) {
+        this.powerCap = powerCap;
+        this.boostScale = 0;
+    }
 
-  public double scale(double input) {
-    return enabled ? input : input * powerCap;
-  }
+    public double scale(double input) {
+        double scale = (1 - powerCap) * boostScale;
+        return input * (powerCap + scale);
+    }
 
-  public void setEnabled(boolean enabled) {
-    this.enabled = enabled;
-  }
+    public void setScale(double boostScale) {
+        this.boostScale = boostScale;
+    }
 }

src/main/java/frc/robot/Climber.java

@@ -44,16 +44,18 @@ enum MotorStates {
     }
 
     public static double MAX_INSTANT_CURRENT = 200.0;
-    public static double MAX_AVERAGE_CURRENT = 110.0;
+    public static double MAX_AVERAGE_CURRENT = 120.0;
     public static double NEXT_AB_STATE_CURRENT = 65.0;
     public static double NEXT_BC_STATE_CURRENT = 45.0;
     public static int FILTER_FRAME_RANGE = 10;
 
-    public static double TOUCH_A_POSITION = 0; // TBD
-    public static double SWING_AB_POSITION = 0; // TBD
-    public static double SWING_B_POSITION = 0; // TBD
-    public static double SWING_BC_POSITION = 0; // TBD
-    public static double SWING_MIN_VELOCITY = 1000; // TBD
+    public static double TOUCH_A_POSITION = 144000; // TBD
+    public static double SWING_AB_POSITION = 43000; // TBD
+    public static double SWING_B_POSITION = 900; // TBD
+    public static double SWING_BC_POSITION = 107000; // TBD
+    public static double SWING_MIN_VELOCITY = 1500; // TBD
+
+    private double motorSpeed;
 
     TalonFX climbingMotor;
     TalonFX secondaryClimbingMotor;
@@ -133,8 +135,8 @@ public void update() {
             // 00 RESTING: Default resting
             case RESTING:
                 climberSolenoidA.set(false);
-                climberSolenoidB1.set(true);
-                climberSolenoidB2.set(true);
+                climberSolenoidB1.set(false);
+                climberSolenoidB2.set(false);
                 climberSolenoidC.set(false);
                 break;
 
@@ -147,11 +149,12 @@ public void update() {
                 climberSolenoidB2.set(false);
                 climberSolenoidC.set(true);
                 // TODO: set motor target here
-                // climbingMotor.set(ControlMode.Position, TOUCH_A_POSITION);
+                setSpeed(motorSpeed);
                 break;
 
             // 10 TOUCH_A: Pin A (button/sensor)
             case TOUCH_A:
+                setSpeed(0);
                 climberSolenoidA.set(false);
                 climberSolenoidB1.set(true);
                 climberSolenoidB2.set(false);
@@ -167,16 +170,20 @@ public void update() {
             case ROTATE_B:
                 // TODO: set motor power here
                 this.setSpeed(-0.4);
-                if (climbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT
-                        || secondaryClimbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT) {
-                    setClimbingState(ClimbingStates.TOUCH_AB);
-                    this.setSpeed(0);
-                }
+                climberSolenoidA.set(false);
+                climberSolenoidB1.set(true);
+                climberSolenoidB2.set(false);
+                climberSolenoidC.set(true);
+                // if (climbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT
+                // || secondaryClimbingMotor.getStatorCurrent() > NEXT_AB_STATE_CURRENT) {
+                // setClimbingState(ClimbingStates.TOUCH_AB);
+                // this.setSpeed(0);
+                // }
                 break;
 
             // 20 TOUCH_AB: Pin B (high current/sensor)
             case TOUCH_AB:
-                this.setSpeed(0);
+                this.setSpeed(-0.05);
                 climberSolenoidA.set(false);
                 climberSolenoidB1.set(false);
                 climberSolenoidB2.set(false);
@@ -190,10 +197,9 @@ public void update() {
             case ROTATE_AB_DOWN:
                 this.setPower(0);
                 // TODO: set motor target here
-                if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_AB_POSITION) < 500) {// Determine
-                                                                                                    // tolerance
+                if (climbingMotor.getSelectedSensorPosition() > SWING_AB_POSITION) {// Was less then when broken
                     System.out.println("DONE SWINGING!");
-                    // setClimbingState(ClimbingStates.RELEASE_A);
+                    setClimbingState(ClimbingStates.RELEASE_A);
                 }
                 break;
 
@@ -206,18 +212,18 @@ public void update() {
                 climberSolenoidC.set(true);
                 // TODO: set motor target here
                 // if (!gates.getA()) {
-                // setClimbingState(ClimbingStates.ROTATE_B_DOWN);;
+                setClimbingState(ClimbingStates.ROTATE_B_DOWN);;
                 // }
                 break;
 
             // 35 ROTATE_B_DOWN: Wait for swinging (photogate)
             case ROTATE_B_DOWN:
                 // TODO: set motor target here
                 this.setPower(0);
-                if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_B_POSITION) < 500
+                if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_B_POSITION) < 1000
                         && Math.abs(
                                 climbingMotor.getSelectedSensorVelocity()) < SWING_MIN_VELOCITY) {// Determine
-                                                                                                  // tolerance
+                                                                                                                                                                                                // tolerance
                     System.out.println("DONE SWINGING!");
                     // setClimbingState(ClimbingStates.ROTATE_C);
                 }
@@ -226,7 +232,7 @@ public void update() {
             // 40 ROTATE_C: Rotate to C bar (gyro/accel)
             case ROTATE_C:
                 // TODO: set motor target here
-                this.setSpeed(-0.2);
+                this.setSpeed(-0.3);
                 if (climbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT
                         || secondaryClimbingMotor.getStatorCurrent() > NEXT_BC_STATE_CURRENT) {
                     setClimbingState(ClimbingStates.TOUCH_BC);
@@ -251,8 +257,7 @@ public void update() {
             case ROTATE_BC_DOWN:
                 // TODO: set motor target here
                 this.setPower(0);
-                if (Math.abs(climbingMotor.getSelectedSensorPosition() - SWING_BC_POSITION) < 500) {// Determine
-                                                                                                    // tolerance
+                if (climbingMotor.getSelectedSensorPosition() < SWING_BC_POSITION) {// Determine tolerance
                     System.out.println("DONE SWINGING!");
                     // setClimbingState(ClimbingStates.RELEASE_B);
                 }
@@ -302,6 +307,14 @@ public ClimbingStates getNextClimbingState() {
         return ClimbingStates.values()[this.currentClimberState.ordinal() + 1];
     }
 
+    public ClimbingStates getPreviousClimbingState() {
+        return ClimbingStates.values()[this.currentClimberState.ordinal() - 1];
+    }
+
+    public int getClimberStateId() {
+        return this.currentClimberState.ordinal();
+    }
+
     public void disableClimber() {
         // Stop the motors
         this.climbingMotor.set(ControlMode.PercentOutput, 0);
@@ -397,7 +410,8 @@ public void setMotors(double value) {
                 break;
 
             case ACTIVE:
-                setSpeed(value);
+                motorSpeed = value;
+                setSpeed(motorSpeed);
                 break;
 
             default:

src/main/java/frc/robot/ClimberGates.java

@@ -44,7 +44,9 @@ public void setupLogging(Logger logger) {
 
     @Override
     public void log(Logger logger) {
-        // TODO Auto-generated method stub
-
+        logger.log("GateA", this.getA());
+        logger.log("GateB1", this.getB2());
+        logger.log("GateB2", this.getB2());
+        logger.log("GateC", this.getC());
     }
 }