Rkalnins/pre st. joe (#164)

rkalnins · mwitcpalek · commit 75f73fdf4181 · 2019-03-06T22:18:44.000-05:00
* --wip-- [skip ci]

* Finish adding vacuum healthcheck

* Revert status frames

* Spotless

* Finish Pre st. joe
diff --git a/networktables.ini b/networktables.ini
diff --git a/networktables.ini.bak b/networktables.ini.bak
diff --git a/src/main/java/frc/team2767/deepspace/command/sequences/CoconutPickupCommandGroup.java b/src/main/java/frc/team2767/deepspace/command/sequences/CoconutPickupCommandGroup.java
@@ -35,7 +35,7 @@ public CoconutPickupCommandGroup() {
     addSequential(new ElevatorSetPositionCommand(22.0));
     addSequential(new BiscuitSetPositionCommand(BiscuitSubsystem.kDownRightPositionDeg));
 
-    addSequential(new ElevatorSetPositionCommand(18.0));
+    addSequential(new ElevatorSetPositionCommand(17.8));
     addParallel(new IntakePositionCommand(105)); // 105
     addSequential(new LogCommand("opening valves"));
     addSequential(
diff --git a/src/main/java/frc/team2767/deepspace/subsystem/BiscuitSubsystem.java b/src/main/java/frc/team2767/deepspace/subsystem/BiscuitSubsystem.java
@@ -58,19 +58,19 @@ public BiscuitSubsystem() {
 
   private void biscuitPreferences() {
     // ticks
-    kAbsoluteZeroTicks = (int) getPreference("absolute_zero_ticks", 1452);
+    kAbsoluteZeroTicks = (int) getPreference("absolute_zero_ticks", 2450);
     kCloseEnoughTicks = (int) getPreference("close_enough_ticks", 50);
 
     // degrees
     kUpPositionDeg = getPreference("up_deg", 0);
     kDownRightPositionDeg = getPreference("down_R_deg", 179);
     kDownLeftPositionDeg = getPreference("down_L_deg", -179);
-    kLeftPositionDeg = getPreference("left_deg", -85);
-    kRightPositionDeg = getPreference("right_deg", 85);
+    kLeftPositionDeg = getPreference("left_deg", -86.5);
+    kRightPositionDeg = getPreference("right_deg", 87);
     kBackStopLeftPositionDeg = getPreference("backstop_L_deg", -135);
     kBackStopRightPositionDeg = getPreference("backstop_R_deg", 135);
-    kTiltUpLeftPositionDeg = getPreference("tilt_up_L_deg", -60);
-    kTiltUpRightPositionDeg = getPreference("tilt_up_R_deg", 60);
+    kTiltUpLeftPositionDeg = getPreference("tilt_up_L_deg", -65);
+    kTiltUpRightPositionDeg = getPreference("tilt_up_R_deg", 64);
   }
 
   private void configTalon() {
diff --git a/src/main/java/frc/team2767/deepspace/subsystem/ElevatorSubsystem.java b/src/main/java/frc/team2767/deepspace/subsystem/ElevatorSubsystem.java
@@ -61,17 +61,17 @@ private void elevatorPreferences() {
     kDownOutput = getPreference("down_output", -0.2);
 
     kCloseEnoughTicks = (int) getPreference("close_enough_ticks", 500);
-    kAbsoluteZeroTicks = (int) getPreference("absolute_zero_ticks", 1854);
+    kAbsoluteZeroTicks = (int) getPreference("absolute_zero_ticks", 2064);
 
     kCargoPickupPositionInches = getPreference("cargo_pickup_in", 16.22);
-    kHatchLowPositionInches = getPreference("hatch_low_in", 8.5);
-    kHatchMediumPositionInches = getPreference("hatch_medium_in", 24.0);
-    kHatchHighPositionInches = getPreference("hatch_high_in", 32.5);
+    kHatchLowPositionInches = getPreference("hatch_low_in", 8.43);
+    kHatchMediumPositionInches = getPreference("hatch_medium_in", 22.0);
+    kHatchHighPositionInches = getPreference("hatch_high_in", 33.45);
     kStowPositionInches = getPreference("stow_in", 4.0);
-    kCargoLowPositionInches = getPreference("cargo_low_in", 11.0);
-    kCargoMediumPositionInches = getPreference("cargo_medium_in", 22.0);
+    kCargoLowPositionInches = getPreference("cargo_low_in", 12.5);
+    kCargoMediumPositionInches = getPreference("cargo_medium_in", 25.0);
     kCargoPlayerPositionInches = getPreference("cargo_player_in", 19.9);
-    kCargoHighPositionInches = getPreference("cargo_high_in", 32.5);
+    kCargoHighPositionInches = getPreference("cargo_high_in", 32.57);
   }
 
   private void configTalon() {
diff --git a/src/main/java/frc/team2767/deepspace/subsystem/IntakeSubsystem.java b/src/main/java/frc/team2767/deepspace/subsystem/IntakeSubsystem.java
@@ -59,13 +59,13 @@ private void intakePreferences() {
     // ticks
     kZeroPositionTicks = getPreference("zero_position_ticks", 0);
     kCloseEnoughTicks = (int) getPreference("close_enough_ticks", 100);
-    kAbsoluteZero = (int) getPreference("absolute_zero", 2163);
+    kAbsoluteZero = (int) getPreference("absolute_zero", 1978);
 
     // degrees
-    kStowPositionDeg = getPreference("up_position_deg", 107);
-    kMiddlePositionDeg = getPreference("middle_position_deg", 105);
+    kStowPositionDeg = getPreference("up_position_deg", 109.86);
+    kMiddlePositionDeg = getPreference("middle_position_deg", 95);
     kCargoPlayerPositionDeg = getPreference("cargo_player_position_deg", 97.4);
-    kLoadPositionDeg = getPreference("load_position_deg", 24.4);
+    kLoadPositionDeg = getPreference("load_position_deg", 0.0);
   }
 
   @SuppressWarnings("Duplicates")
diff --git a/src/main/java/frc/team2767/deepspace/subsystem/VacuumSubsystem.java b/src/main/java/frc/team2767/deepspace/subsystem/VacuumSubsystem.java
@@ -2,7 +2,6 @@
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
 import edu.wpi.first.wpilibj.AnalogInput;
@@ -90,7 +89,6 @@ private void configTalon() {
     vacuumConfig.peakOutputForward = 1.0;
     vacuumConfig.peakOutputReverse = 0.0;
 
-    vacuum.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 10);
     vacuum.configAllSettings(vacuumConfig);
     vacuum.enableCurrentLimit(true);
     vacuum.enableVoltageCompensation(true);
@@ -164,8 +162,7 @@ public void setSolenoid(Valve valve, boolean state) {
   }
 
   public double getPumpTemperature() {
-    double temp = (analogInput.getVoltage() - TEMP_OFFSET) / VOLTS_PER_CELSIUS;
-    return temp;
+    return (analogInput.getVoltage() - TEMP_OFFSET) / VOLTS_PER_CELSIUS;
   }
 
   public boolean onTarget() {
diff --git a/src/main/java/frc/team2767/deepspace/subsystem/safety/IntakePosition.java b/src/main/java/frc/team2767/deepspace/subsystem/safety/IntakePosition.java
@@ -1,8 +1,8 @@
 package frc.team2767.deepspace.subsystem.safety;
 
 enum IntakePosition {
-  INTAKE_INTAKE(9664, -250),
-  INTAKE_STOW(3224, -250);
+  INTAKE_INTAKE(12_081, -250),
+  INTAKE_STOW(3_932, -250);
 
   public final int forwardLimit;
   public final int reverseLimit;
diff --git a/src/main/kotlin/frc/team2767/deepspace/command/HealthCheckCommand.kt b/src/main/kotlin/frc/team2767/deepspace/command/HealthCheckCommand.kt
@@ -15,12 +15,38 @@ class HealthCheckCommand : Command() {
         requires(Robot.DRIVE)
         requires(Robot.ELEVATOR)
         requires(Robot.INTAKE)
+        requires(Robot.VACUUM)
     }
 
     private lateinit var healthCheck: HealthCheck
 
     override fun initialize() {
         healthCheck = healthCheck {
+            //
+//            vacuumCheck {
+//                name = "pressure tests"
+//
+//                pressureTest {
+//                    name = "climb pressure test"
+//
+//                    pressure = 16.0
+//                    encoderTimeOutCount = 20_000
+//                    maxAcceptablePressureDrop = 2
+//                }
+//            }
+
+            // pump tests are highly dependent on valve states set in pressure tests
+            talonCheck {
+                name = "pump tests"
+                talons = Robot.VACUUM.talons
+
+                timedTest {
+                    percentOutput = 0.25
+                    currentRange = 0.0..0.0
+                    speedRange = 0..0
+                    duration = 5.0
+                }
+            }
 
 
             talonCheck {
@@ -87,24 +113,35 @@ class HealthCheckCommand : Command() {
                 name = "elevator position tests"
                 talons = Robot.ELEVATOR.talons
 
+                positionTalon {
+                    encoderTarget = 10_000
+                    encoderGoodEnough = 100
+                }
+
                 positionTest {
                     percentOutput = 0.2
 
-                    encoderChangeTarget = 10_000
+                    encoderChangeTarget = 15_000
                     encoderGoodEnough = 500
                     encoderTimeOutCount = 5000
 
                     currentRange = 0.0..0.5
                     speedRange = 500..600
                 }
 
-                positionTalon {
-                    encoderTarget = 10_000
-                    encoderGoodEnough = 100
+                positionTest {
+                    percentOutput = -0.2
+
+                    encoderChangeTarget = 15_000
+                    encoderGoodEnough = 500
+                    encoderTimeOutCount = 5000
 
+                    currentRange = 0.0..0.5
+                    speedRange = 500..600
                 }
             }
 
+
             talonCheck {
                 name = "shoulder position tests"
                 talons = Robot.INTAKE.shoulderTalon
@@ -119,6 +156,17 @@ class HealthCheckCommand : Command() {
                     currentRange = 0.0..0.5
                     speedRange = 500..600
                 }
+
+                positionTest {
+                    percentOutput = -0.2
+
+                    encoderChangeTarget = 2500
+                    encoderGoodEnough = 200
+                    encoderTimeOutCount = 500
+
+                    currentRange = 0.0..0.5
+                    speedRange = 500..600
+                }
             }
 
             talonCheck {
@@ -164,7 +212,6 @@ class HealthCheckCommand : Command() {
                     speedRange = 1000..1100
                 }
             }
-
         }
 
     }
diff --git a/src/main/kotlin/frc/team2767/deepspace/health/HealthCheck.kt b/src/main/kotlin/frc/team2767/deepspace/health/HealthCheck.kt
@@ -22,6 +22,12 @@ class HealthCheck {
     private lateinit var iterator: Iterator<TestGroup>
     private lateinit var currentTestGroup: TestGroup
 
+    fun vacuumCheck(init: VacuumGroup.() -> Unit): VacuumGroup {
+        val test = VacuumGroup(this)
+        test.init()
+        testGroups.add(test)
+        return test
+    }
 
     fun talonCheck(init: TalonGroup.() -> Unit): TalonGroup {
         val test = TalonGroup(this)
diff --git a/src/main/kotlin/frc/team2767/deepspace/health/TalonPosition.kt b/src/main/kotlin/frc/team2767/deepspace/health/TalonPosition.kt
@@ -2,7 +2,8 @@ package frc.team2767.deepspace.health
 
 import com.ctre.phoenix.motorcontrol.ControlMode
 import com.ctre.phoenix.motorcontrol.can.TalonSRX
-import frc.team2767.deepspace.health.TalonPosition.State.*
+import frc.team2767.deepspace.health.TalonPosition.State.RUNNING
+import frc.team2767.deepspace.health.TalonPosition.State.STOPPED
 import kotlinx.html.TagConsumer
 import mu.KotlinLogging
 import kotlin.math.absoluteValue
@@ -16,12 +17,12 @@ class TalonPosition(private val group: TalonGroup) : Test {
     var encoderTarget = 0
     var encoderGoodEnough = 10
 
-    private var state = STARTING
+    private var state = State.STARTING
     private lateinit var talon: TalonSRX
 
     override fun execute() {
         when (state) {
-            STARTING -> {
+            State.STARTING -> {
                 if (group.talons.size != 1) {
                     logger.error { "position test valid for one talon, has ${group.talons.size}, skipping" }
                     state = STOPPED
@@ -33,9 +34,10 @@ class TalonPosition(private val group: TalonGroup) : Test {
                 state = RUNNING
             }
             RUNNING -> {
-                if ((encoderTarget - talon.selectedSensorPosition).absoluteValue < encoderGoodEnough)
+                if ((encoderTarget - talon.selectedSensorPosition).absoluteValue < encoderGoodEnough) {
                     logger.info { "repositioned to $encoderTarget, finishing" }
-                state = STOPPED
+                    state = STOPPED
+                }
             }
             STOPPED -> logger.info { "position talon stopped" }
         }
diff --git a/src/main/kotlin/frc/team2767/deepspace/health/TalonPositionTest.kt b/src/main/kotlin/frc/team2767/deepspace/health/TalonPositionTest.kt
@@ -50,7 +50,7 @@ class TalonPositionTest(private val group: TalonGroup) : Test, Reportable {
                 startingPosition = talon.selectedSensorPosition
                 talon.set(PercentOutput, percentOutput)
                 startTime = Timer.getFPGATimestamp()
-                state = RUNNING
+                state = WARMING
             }
 
             WARMING -> if (Timer.getFPGATimestamp() - startTime >= warmUp) {
diff --git a/src/main/kotlin/frc/team2767/deepspace/health/Test.kt b/src/main/kotlin/frc/team2767/deepspace/health/Test.kt
@@ -10,7 +10,3 @@ interface Test {
     fun isFinished(): Boolean
     fun report(tagConsumer: TagConsumer<Appendable>)
 }
-
-
-private fun <T : Comparable<T>> ClosedRange<T>.withUnits(units: String) =
-    "${this.start} - ${this.endInclusive} $units"
diff --git a/src/main/kotlin/frc/team2767/deepspace/health/TestGroup.kt b/src/main/kotlin/frc/team2767/deepspace/health/TestGroup.kt
@@ -1,7 +1,9 @@
 package frc.team2767.deepspace.health
 
 import com.ctre.phoenix.motorcontrol.can.TalonSRX
+import frc.team2767.deepspace.Robot
 import frc.team2767.deepspace.health.TestGroup.State.*
+import frc.team2767.deepspace.subsystem.VacuumSubsystem
 import kotlinx.html.TagConsumer
 import kotlinx.html.div
 import kotlinx.html.h2
@@ -73,7 +75,6 @@ abstract class TestGroup(val healthCheck: HealthCheck) : Test {
 class TalonGroup(healthCheck: HealthCheck) : TestGroup(healthCheck) {
     var talons = emptyList<TalonSRX>()
 
-
     fun timedTest(init: TalonTimedTest.() -> Unit): Test {
         val spinTest = TalonTimedTest(this)
         spinTest.init()
@@ -95,3 +96,16 @@ class TalonGroup(healthCheck: HealthCheck) : TestGroup(healthCheck) {
         return position
     }
 }
+
+class VacuumGroup(healthCheck: HealthCheck) : TestGroup(healthCheck) {
+    val vacuumSubsystem = Robot.VACUUM
+
+    fun pressureTest(init: VacuumPressureTest.() -> Unit): Test {
+
+        logger.debug { "vacuum group" }
+        val pressureTest = VacuumPressureTest(this)
+        pressureTest.init()
+        tests.add(pressureTest)
+        return pressureTest
+    }
+}
diff --git a/src/main/kotlin/frc/team2767/deepspace/health/VacuumPressureTest.kt b/src/main/kotlin/frc/team2767/deepspace/health/VacuumPressureTest.kt
diff --git a/src/main/resources/META-INF/healthcheck.css b/src/main/resources/META-INF/healthcheck.css
