Failure manager

src/main/kotlin/com/team4099/robot2021/Robot.kt

@@ -1,9 +1,12 @@
 package com.team4099.robot2021
 
 import com.team4099.lib.logging.Logger
-import com.team4099.robot2021.commands.MoveClimber
+import com.team4099.lib.units.base.inches
+import com.team4099.lib.units.base.seconds
+import com.team4099.robot2021.commands.climber.MoveClimber
 import com.team4099.robot2021.commands.climber.LockClimber
 import com.team4099.robot2021.commands.climber.UnlockClimber
+import com.team4099.robot2021.commands.failures.ValidateCommand
 import com.team4099.robot2021.commands.feeder.FeederBeamBreak
 import com.team4099.robot2021.commands.feeder.FeederCommand
 import com.team4099.robot2021.config.Constants
@@ -13,16 +16,23 @@ import com.team4099.robot2021.commands.intake.IntakeCommand
 import com.team4099.robot2021.subsystems.Intake
 import edu.wpi.first.wpilibj.DigitalInput
 import edu.wpi.first.wpilibj.RobotController
-import com.team4099.robot2021.config.ControlBoard
+import com.team4099.robot2021.loops.FailureManager
 import com.team4099.robot2021.subsystems.Climber
+import edu.wpi.first.wpilibj.AnalogInput
 import edu.wpi.first.wpilibj.TimedRobot
 import edu.wpi.first.wpilibj2.command.CommandScheduler
 import edu.wpi.first.wpilibj2.command.InstantCommand
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
 import kotlin.math.pow
 
 object Robot : TimedRobot() {
   val robotName: Constants.Tuning.RobotName
 
+  private val pressureSensor = AnalogInput(Constants.PressureSensor.ANALOG_PIN)
+
+  private val pneumaticsPressure: Double
+    get() = 250*(pressureSensor.voltage/Constants.PressureSensor.ANALOG_VOLTAGE) - 25
+
   init {
     val robotId = Constants.Tuning.ROBOT_ID_PINS.withIndex().map { (i, pin) ->
       if (DigitalInput(pin).get()) 0 else 2.0.pow(i).toInt()
@@ -35,15 +45,26 @@ object Robot : TimedRobot() {
 
     // Link between feeder Trigger and Command
     Feeder.defaultCommand = FeederCommand(Feeder.FeederState.NEUTRAL)
-    ControlBoard.runFeederIn.whileActiveOnce(FeederCommand(Feeder.FeederState.FORWARD_FLOOR));
-    ControlBoard.runFeederOut.whileActiveOnce(FeederCommand(Feeder.FeederState.BACKWARD));
+    ControlBoard.runFeederIn.whileActiveOnce(FeederCommand(Feeder.FeederState.FORWARD_FLOOR))
+    ControlBoard.runFeederOut.whileActiveOnce(FeederCommand(Feeder.FeederState.BACKWARD))
 
     Intake.defaultCommand = IntakeCommand(Constants.Intake.IntakeState.DEFAULT, Constants.Intake.ArmPosition.IN)
-    ControlBoard.runIntakeIn.whileActiveContinuous(IntakeCommand(Constants.Intake.IntakeState.IN, Constants.Intake.ArmPosition.OUT).alongWith(FeederBeamBreak()));
-    ControlBoard.runIntakeOut.whileActiveContinuous(IntakeCommand(Constants.Intake.IntakeState.OUT, Constants.Intake.ArmPosition.OUT).alongWith(FeederCommand(Feeder.FeederState.BACKWARD)));
+    ControlBoard.runIntakeIn.whileActiveContinuous(IntakeCommand(Constants.Intake.IntakeState.IN, Constants.Intake.ArmPosition.OUT).alongWith(FeederBeamBreak()))
+    ControlBoard.runIntakeOut.whileActiveContinuous(IntakeCommand(Constants.Intake.IntakeState.OUT, Constants.Intake.ArmPosition.OUT).alongWith(FeederCommand(Feeder.FeederState.BACKWARD)))
+
+    FailureManager.addFailure(FailureManager.Failures.PRESSURE_LEAK,false) { Constants.PressureSensor.PSI_THRESHOLD > pneumaticsPressure  }
+    Logger.addSource("Pressure","Pressure Value"){ pneumaticsPressure }
   }
 
   private val autonomousCommand = InstantCommand()
+  private val testCommand = SequentialCommandGroup(
+    MoveClimber(Constants.ClimberPosition.HIGH),
+    (ValidateCommand(({1.inches>(Constants.ClimberPosition.HIGH.length - Climber.climberRArmSensor.position).absoluteValue && (1.inches)>(Constants.ClimberPosition.HIGH.length - Climber.climberLArmSensor.position).absoluteValue}),5.seconds,FailureManager.Failures.CLIMBER_FAILED_POS)),
+    MoveClimber(Constants.ClimberPosition.LOW), (ValidateCommand(({1.inches<(Constants.ClimberPosition.LOW.length - Climber.climberRArmSensor.position).absoluteValue && (1.inches)<(Constants.ClimberPosition.LOW.length - Climber.climberLArmSensor.position).absoluteValue}),5.seconds,FailureManager.Failures.CLIMBER_FAILED_POS)),
+    IntakeCommand(Constants.Intake.IntakeState.IN, Constants.Intake.ArmPosition.OUT),
+    FeederBeamBreak(),
+    IntakeCommand(Constants.Intake.IntakeState.OUT, Constants.Intake.ArmPosition.OUT),
+    FeederCommand(Feeder.FeederState.BACKWARD))
 
   override fun autonomousInit() {
     autonomousCommand.schedule()
@@ -58,6 +79,7 @@ object Robot : TimedRobot() {
 
   override fun robotPeriodic() {
     CommandScheduler.getInstance().run()
+    FailureManager.checkFailures()
     Logger.saveLogs()
   }
 }

src/main/kotlin/com/team4099/robot2021/commands/climber/LockClimber.kt

@@ -4,6 +4,11 @@ import com.team4099.lib.logging.Logger
 import com.team4099.robot2021.subsystems.Climber
 import edu.wpi.first.wpilibj2.command.CommandBase
 
+/**
+ * Lock climber
+ *
+ * @property Climber.brakeApplied Prevents motors from powering / moving the climber
+ */
 class LockClimber : CommandBase() {
   init {
     addRequirements(Climber)

src/main/kotlin/com/team4099/robot2021/commands/climber/MoveClimber.kt

@@ -1,12 +1,16 @@
-package com.team4099.robot2021.commands
+package com.team4099.robot2021.commands.climber
 
 import com.team4099.lib.logging.Logger
 import com.team4099.robot2021.commands.climber.UnlockClimber
 import com.team4099.robot2021.config.Constants
 import com.team4099.robot2021.config.ControlBoard
 import com.team4099.robot2021.subsystems.Climber
 import edu.wpi.first.wpilibj2.command.CommandBase
-
+/**
+ * Move climber
+ *
+ * @property pos Direct motor to rotate based on position
+ */
 class MoveClimber(val pos: Constants.ClimberPosition): CommandBase() {
   init {
     addRequirements(Climber)
@@ -17,10 +21,9 @@ class MoveClimber(val pos: Constants.ClimberPosition): CommandBase() {
   }
 
   override fun execute() {
-    if (Climber.brakeApplied) {
-      UnlockClimber()
+    if (!Climber.brakeApplied) {
+      Climber.setPosition(pos)
     }
-    Climber.setPosition(pos)
   }
 }
 //check if climber is locked, if locked don't move

src/main/kotlin/com/team4099/robot2021/commands/climber/UnlockClimber.kt

@@ -3,9 +3,15 @@ package com.team4099.robot2021.commands.climber
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.Logger
 import com.team4099.robot2021.config.Constants
+import com.team4099.robot2021.loops.FailureManager
 import com.team4099.robot2021.subsystems.Climber
 import edu.wpi.first.wpilibj2.command.CommandBase
 
+/**
+ * Unlock climber
+ *
+ * @property FailureManager.Failures.PRESSURE_LEAK State based if leak detected in pneumatics
+ */
 class UnlockClimber : CommandBase() {
   init {
     addRequirements(Climber)
@@ -14,7 +20,12 @@ class UnlockClimber : CommandBase() {
   override fun initialize() {
     initTime = Clock.fpgaTime
     Climber.brakeApplied = false
-    Logger.addEvent("Climber", "Climber Unlocked")
+    if (FailureManager.errorFlags[FailureManager.Failures.PRESSURE_LEAK] == true){
+      Climber.brakeApplied = true
+      Logger.addEvent("Climber", "Climber Unlock Failed")
+    }else {
+      Logger.addEvent("Climber", "Climber Unlocked")
+    }
   }
 
   override fun isFinished(): Boolean {

src/main/kotlin/com/team4099/robot2021/commands/failures/FailureResetCommand.kt

@@ -0,0 +1,14 @@
+package com.team4099.robot2021.commands.failures
+import edu.wpi.first.wpilibj2.command.CommandBase
+import com.team4099.robot2021.loops.FailureManager;
+
+/**
+ * Failure reset command
+ *
+ * @property FailureManager.reset Sets all errors back to normal
+ */
+class FailureResetCommand : CommandBase() {
+  override fun initialize(){
+    FailureManager.reset()
+  }
+}

src/main/kotlin/com/team4099/robot2021/commands/failures/ValidateCommand.kt

@@ -0,0 +1,33 @@
+package com.team4099.robot2021.commands.failures
+
+import com.team4099.lib.hal.Clock
+import com.team4099.lib.units.base.Time
+import com.team4099.lib.units.base.seconds
+import com.team4099.robot2021.loops.FailureManager
+import edu.wpi.first.wpilibj2.command.CommandBase
+/**
+ * Validate command
+ *
+ * @property condition Runs if boolean is true
+ * @property timeout When timeout passes a threshold it is registered as a failure
+ * @property failure The failure that the condition is tied to
+ */
+class ValidateCommand(val condition : () -> Boolean, val timeout : Time, val failure : FailureManager.Failures) : CommandBase() {
+  private var startTime : Time = 0.seconds
+  override fun initialize() {
+    startTime = Clock.fpgaTime
+
+  }
+
+  override fun execute() {
+    val elapseTime : Time = Clock.fpgaTime - startTime
+    if(elapseTime > timeout) {
+      FailureManager.errorFlags[failure] = true
+    }
+  }
+
+  override fun isFinished(): Boolean {
+    return condition()
+  }
+
+}

src/main/kotlin/com/team4099/robot2021/commands/feeder/FeederBeamBreak.kt

@@ -3,6 +3,13 @@ import com.team4099.lib.logging.Logger
 import com.team4099.robot2021.subsystems.Feeder
 import edu.wpi.first.wpilibj2.command.CommandBase
 
+/**
+ * Feeder beam break
+ *
+ * @property Feeder.topBeamBroken
+ * @property Feeder.bottomBeamBroken
+ */
+
 class FeederBeamBreak(): CommandBase(){
   init{
     addRequirements(Feeder)

src/main/kotlin/com/team4099/robot2021/commands/feeder/FeederCommand.kt

@@ -4,6 +4,12 @@ import com.team4099.robot2021.subsystems.Feeder
 import com.team4099.robot2021.subsystems.Feeder.feederState
 import edu.wpi.first.wpilibj2.command.CommandBase
 
+
+/**
+ * Feeder command
+ *
+ * @property dir Sets the direction the motors rotate
+ */
 class FeederCommand(var dir: Feeder.FeederState): CommandBase(){
   init{
     addRequirements(Feeder)

src/main/kotlin/com/team4099/robot2021/commands/intake/IntakeCommand.kt

@@ -5,6 +5,13 @@ import com.team4099.robot2021.config.Constants
 import com.team4099.robot2021.subsystems.Intake
 import edu.wpi.first.wpilibj2.command.CommandBase
 
+/**
+ * Intake command
+ *
+ * @property intakeState Sets the velocity of the wheel
+ * @property armState Sets the position of the pneumatic arms
+ * @constructor Create empty Intake command
+ */
 class IntakeCommand(var intakeState: Constants.Intake.IntakeState, var armState: Constants.Intake.ArmPosition): CommandBase() {
 
   init {

src/main/kotlin/com/team4099/robot2021/config/Constants.kt

@@ -13,6 +13,12 @@ object Constants {
     const val EPSILON = 1E-9
   }
 
+  object PressureSensor{
+    const val ANALOG_PIN = 0;
+    const val ANALOG_VOLTAGE = 5.00;
+    const val PSI_THRESHOLD = 10.00;
+  }
+
   object Tuning {
     const val TUNING_TOGGLE_PIN = 0
     val ROBOT_ID_PINS = 1..2

src/main/kotlin/com/team4099/robot2021/loops/FailureManager.kt

@@ -0,0 +1,128 @@
+package com.team4099.robot2021.loops
+
+import com.team4099.lib.logging.Logger
+import com.team4099.robot2021.Robot
+import edu.wpi.first.wpilibj.Sendable
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder
+
+/**
+ * Failure manager
+ *
+ * @constructor Create empty Failure manager
+ */
+object FailureManager: Sendable {
+  /**
+   * Failures
+   *
+   * @param failures Map of Error Flags
+   * @param log String representing the output of failures
+   */
+  private val failures = mutableListOf<FailureSource>()
+  private var log = ""
+
+  /**
+   * Error flags
+   */
+  val errorFlags = mutableMapOf(
+    Failures.PRESSURE_LEAK to false,
+    Failures.INTAKE_SPEC_VIOLATION to false,
+    Failures.CLIMBER_FAILED_POS to false
+  )
+
+  /**
+   * Severity
+   *
+   * @property Severity The levels of bad of the failure
+   * @constructor Create empty Severity
+   */
+  enum class Severity {
+    WARNING,
+    ERROR
+  }
+
+  /**
+   * Failures
+   *
+   * @property severity Enum above giving classification of the severity level of the failure
+   * @property description Gives details about the given failure
+   * @constructor Create empty Failures
+   */
+  enum class Failures (val severity : Severity, val description: String) {
+    PRESSURE_LEAK(Severity.ERROR, "Pressure Leak, pressure under"),
+    INTAKE_SPEC_VIOLATION(Severity.WARNING, "intake bad"),
+    CLIMBER_FAILED_POS(Severity.ERROR, "Climber position is wrong");
+  }
+
+  /**
+   * Add failure
+   *
+   * @param failType Name implies
+   * @param latching If true failure can't be removed
+   * @param condition Given the condition will determine if there is a failure
+   * @receiver
+   */
+  fun addFailure(failType: Failures, latching: Boolean = false, condition: () -> Boolean) {
+    failures.add(FailureSource(failType, latching, condition, false))
+  }
+
+  /**
+   * Add test failure (test mode)
+   *
+   * @param failType Name implies
+   * @param condition Given the condition will determine if there is a failure
+   * @receiver
+   */
+  fun addTestFailure(failType: Failures, condition: () -> Boolean) {
+    failures.add(FailureSource(failType, true, condition, true))
+  }
+
+  /**
+   * Check failures
+   *
+   * Goes through each failure in the list and checks if it's true and if so logs it
+   */
+  fun checkFailures() {
+    failures.forEach { failure ->
+      if (!failure.testOnly || Robot.isTest) {
+        if (!failure.latching) {
+          if (failure.condition() && errorFlags[failure.failType] == false) logDashboard(failure.failType.description)
+          errorFlags[failure.failType] = failure.condition()
+        } else if (failure.condition()) {
+          if (errorFlags[failure.failType] == false) logDashboard(failure.failType.description)
+          errorFlags[failure.failType] = true
+        }
+      }
+    }
+  }
+
+  /**
+   * Reset
+   *
+   * Method that resets each failure to false
+   */
+  fun reset(){
+    failures.forEach{failure -> errorFlags[failure.failType] = false}
+    log = ""
+  }
+
+  /**
+   * Log dashboard
+   *
+   * @param string Description of failure
+   */
+  fun logDashboard(string: String) {
+    Logger.addEvent("FailureManager", string)
+    log += "\n$string"
+    log.trim()
+  }
+
+  /**
+   * Init sendable
+   *
+   * @param builder
+   */
+  override fun initSendable(builder: SendableBuilder?) {
+    builder?.addStringProperty("Failures", { log }) {}
+  }
+}
+data class FailureSource(val failType: FailureManager.Failures, val latching: Boolean, val condition: () -> Boolean, val testOnly: Boolean)