diff --git a/src/main/kotlin/com/team4099/robot2021/Robot.kt b/src/main/kotlin/com/team4099/robot2021/Robot.kt index a31c1974..fed7878d 100644 --- a/src/main/kotlin/com/team4099/robot2021/Robot.kt +++ b/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() } } diff --git a/src/main/kotlin/com/team4099/robot2021/commands/climber/LockClimber.kt b/src/main/kotlin/com/team4099/robot2021/commands/climber/LockClimber.kt index c5ae1219..2aa110cf 100644 --- a/src/main/kotlin/com/team4099/robot2021/commands/climber/LockClimber.kt +++ b/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) diff --git a/src/main/kotlin/com/team4099/robot2021/commands/climber/MoveClimber.kt b/src/main/kotlin/com/team4099/robot2021/commands/climber/MoveClimber.kt index 41b56894..9439c19b 100644 --- a/src/main/kotlin/com/team4099/robot2021/commands/climber/MoveClimber.kt +++ b/src/main/kotlin/com/team4099/robot2021/commands/climber/MoveClimber.kt @@ -1,4 +1,4 @@ -package com.team4099.robot2021.commands +package com.team4099.robot2021.commands.climber import com.team4099.lib.logging.Logger import com.team4099.robot2021.commands.climber.UnlockClimber @@ -6,7 +6,11 @@ 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 diff --git a/src/main/kotlin/com/team4099/robot2021/commands/climber/UnlockClimber.kt b/src/main/kotlin/com/team4099/robot2021/commands/climber/UnlockClimber.kt index 3520d8bb..13f027bf 100644 --- a/src/main/kotlin/com/team4099/robot2021/commands/climber/UnlockClimber.kt +++ b/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 { diff --git a/src/main/kotlin/com/team4099/robot2021/commands/failures/FailureResetCommand.kt b/src/main/kotlin/com/team4099/robot2021/commands/failures/FailureResetCommand.kt new file mode 100644 index 00000000..9f75f534 --- /dev/null +++ b/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() + } +} diff --git a/src/main/kotlin/com/team4099/robot2021/commands/failures/ValidateCommand.kt b/src/main/kotlin/com/team4099/robot2021/commands/failures/ValidateCommand.kt new file mode 100644 index 00000000..ab30be33 --- /dev/null +++ b/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() + } + +} diff --git a/src/main/kotlin/com/team4099/robot2021/commands/feeder/FeederBeamBreak.kt b/src/main/kotlin/com/team4099/robot2021/commands/feeder/FeederBeamBreak.kt index b0d39eec..1352efcb 100644 --- a/src/main/kotlin/com/team4099/robot2021/commands/feeder/FeederBeamBreak.kt +++ b/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) diff --git a/src/main/kotlin/com/team4099/robot2021/commands/feeder/FeederCommand.kt b/src/main/kotlin/com/team4099/robot2021/commands/feeder/FeederCommand.kt index 6966f0ef..c2a1804f 100644 --- a/src/main/kotlin/com/team4099/robot2021/commands/feeder/FeederCommand.kt +++ b/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) diff --git a/src/main/kotlin/com/team4099/robot2021/commands/intake/IntakeCommand.kt b/src/main/kotlin/com/team4099/robot2021/commands/intake/IntakeCommand.kt index 7e2a3d0f..f295d6d6 100644 --- a/src/main/kotlin/com/team4099/robot2021/commands/intake/IntakeCommand.kt +++ b/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 { diff --git a/src/main/kotlin/com/team4099/robot2021/config/Constants.kt b/src/main/kotlin/com/team4099/robot2021/config/Constants.kt index 69861aec..676e31e4 100644 --- a/src/main/kotlin/com/team4099/robot2021/config/Constants.kt +++ b/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 diff --git a/src/main/kotlin/com/team4099/robot2021/loops/FailureManager.kt b/src/main/kotlin/com/team4099/robot2021/loops/FailureManager.kt new file mode 100644 index 00000000..41ea3285 --- /dev/null +++ b/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() + 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) diff --git a/src/main/kotlin/com/team4099/robot2021/subsystems/Climber.kt b/src/main/kotlin/com/team4099/robot2021/subsystems/Climber.kt index 5da7f125..b769dcc9 100644 --- a/src/main/kotlin/com/team4099/robot2021/subsystems/Climber.kt +++ b/src/main/kotlin/com/team4099/robot2021/subsystems/Climber.kt @@ -11,13 +11,22 @@ import com.team4099.robot2021.config.Constants import edu.wpi.first.wpilibj.Solenoid import edu.wpi.first.wpilibj2.command.SubsystemBase +/** + * Climber + * + * @property climberRArm Object representing the right motor + * @property climberLArm Object representing the left motor + * @property pneumaticLBrake Object representing the right pneumatic brake + * @property pneumaticRBrake Object representing the left pneumatic brake + * @property brakeApplied Prevents motors from powering / sets brake state for pneumatics + */ object Climber: SubsystemBase() { private val climberRArm = CANSparkMax(Constants.Climber.CLIMBER_R_ARM_SPARKMAX_ID, CANSparkMaxLowLevel.MotorType.kBrushless) private val climberLArm = CANSparkMax(Constants.Climber.CLIMBER_L_ARM_SPARKMAX_ID, CANSparkMaxLowLevel.MotorType.kBrushless) private val climberRArmPIDController = climberRArm.pidController private val climberLArmPIDController = climberLArm.pidController - private val climberRArmSensor = sparkMaxLinearMechanismSensor(climberRArm, Constants.Climber.CLIMBER_SENSOR_LINEARMECH_GEARRATIO, Constants.Climber.CLIMBER_SENSOR_LINEARMECH_PULLEYDIAMETER)//diameter: .0508 meters = 2 in - private val climberLArmSensor = sparkMaxLinearMechanismSensor(climberLArm, Constants.Climber.CLIMBER_SENSOR_LINEARMECH_GEARRATIO, Constants.Climber.CLIMBER_SENSOR_LINEARMECH_PULLEYDIAMETER)//diameter: .0508 meters = 2 in + val climberRArmSensor = sparkMaxLinearMechanismSensor(climberRArm, Constants.Climber.CLIMBER_SENSOR_LINEARMECH_GEARRATIO, Constants.Climber.CLIMBER_SENSOR_LINEARMECH_PULLEYDIAMETER)//diameter: .0508 meters = 2 in + val climberLArmSensor = sparkMaxLinearMechanismSensor(climberLArm, Constants.Climber.CLIMBER_SENSOR_LINEARMECH_GEARRATIO, Constants.Climber.CLIMBER_SENSOR_LINEARMECH_PULLEYDIAMETER)//diameter: .0508 meters = 2 in private val pneumaticRBrake = Solenoid(Constants.Climber.CLIMBER_SOLENOID_ACTUATIONSTATE) //unactuated state is having the pneumatic extended out to lock private val pneumaticLBrake = Solenoid(Constants.Climber.CLIMBER_SOLENOID_ACTUATIONSTATE) //unactuated state is having the pneumatic extended out to lock var brakeApplied = false diff --git a/src/main/kotlin/com/team4099/robot2021/subsystems/Feeder.kt b/src/main/kotlin/com/team4099/robot2021/subsystems/Feeder.kt index b852a60c..f9cf92d7 100644 --- a/src/main/kotlin/com/team4099/robot2021/subsystems/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2021/subsystems/Feeder.kt @@ -7,8 +7,26 @@ import com.team4099.robot2021.config.Constants import edu.wpi.first.wpilibj.DigitalInput import edu.wpi.first.wpilibj2.command.SubsystemBase -object Feeder:SubsystemBase(){ +/** + * Feeder + * + * @property floorMotor The motor for the floor of the feeder (the spinny wheel at the bottom) + * @property verticalMotor The motor for vertical part of the feeder (the poly cord) + * @property feederState Represents the stage of the feeder + */ +object Feeder : SubsystemBase() { + + private val floorMotor = TalonSRX(Constants.Feeder.FLOOR_ID) + private val verticalMotor = TalonSRX(Constants.Feeder.VERTICAL_ID) + + /** + * @param floorMotor Reset motor to ensure clear data + * @param verticalMotor Reset motor to ensure clear data + */ init { + floorMotor.configFactoryDefault(); + verticalMotor.configFactoryDefault(); + Logger.addSource(Constants.Feeder.TAB, "Feeder State") { feederState } Logger.addSource(Constants.Feeder.TAB, "Feeder Floor Motor Power") { floorMotor.motorOutputPercent } @@ -26,10 +44,12 @@ object Feeder:SubsystemBase(){ } /** - * An enum representing the state of the feeder - * floorMotor power, verticalMotor power + * Feeder state + * + * @property floorMotorPower Represents state of floorMotor power + * @property verticalMotorPower Represents state of verticalMotor power + * @constructor Create empty Feeder state */ - enum class FeederState(val floorMotorPower: Double, val verticalMotorPower: Double) { FORWARD_ALL(Constants.Feeder.FEEDER_POWER, Constants.Feeder.FEEDER_POWER), FORWARD_FLOOR(Constants.Feeder.FEEDER_POWER, 0.0), @@ -37,17 +57,12 @@ object Feeder:SubsystemBase(){ NEUTRAL(0.0, 0.0) } - // The motor for the floor of the feeder (the spinny wheel at the bottom) - private val floorMotor = TalonSRX(Constants.Feeder.FLOOR_ID) - - // The motor for vertical part of the feeder (the poly cord) - private val verticalMotor = TalonSRX(Constants.Feeder.VERTICAL_ID) - + /** + * @property topBeamDIO The DIO pin state of the top Beam Break + * @property bottomBeamDIO The DIO pin state of the bottom Beam Break + */ - //The DIO pin state of the top Beam Break private val topBeamDIO = DigitalInput(Constants.Feeder.TOP_DIO_PIN); - - //The DIO pin state of the bottom Beam Break private val bottomBeamDIO = DigitalInput(Constants.Feeder.BOTTOM_DIO_PIN); /** @@ -63,8 +78,7 @@ object Feeder:SubsystemBase(){ /** * Interacts with feeder State - * @param state Feeder State - * @return None + * Sets Feeder state to the given argument in FeederCommand **/ var feederState = FeederState.NEUTRAL diff --git a/src/main/kotlin/com/team4099/robot2021/subsystems/Intake.kt b/src/main/kotlin/com/team4099/robot2021/subsystems/Intake.kt index 0aba0722..2c921e41 100644 --- a/src/main/kotlin/com/team4099/robot2021/subsystems/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2021/subsystems/Intake.kt @@ -7,27 +7,45 @@ import com.team4099.lib.units.base.Length import com.team4099.lib.units.ctreLinearMechanismSensor import com.team4099.lib.units.perSecond import com.team4099.robot2021.config.Constants +import com.team4099.robot2021.loops.FailureManager import edu.wpi.first.wpilibj.DoubleSolenoid import edu.wpi.first.wpilibj2.command.SubsystemBase - +/** + * Intake + * + * @property intakeTalon Object representing the motor for the intake + * @property intakeDoubleSolenoid Object representing the solenoid for the arm + * @property armState Represents the stage of the arm + * @property intakeState Represents the stage of the intake + */ object Intake : SubsystemBase() { private val intakeTalon = TalonFX(Constants.Intake.INTAKE_MOTOR) private val intakeDoubleSolenoid = DoubleSolenoid(Constants.Intake.ARM_SOLENOID_PORT_1, Constants.Intake.ARM_SOLENOID_PORT_2) + /** + * Sets Intake state to the given argument in IntakeCommand + */ var intakeState = Constants.Intake.IntakeState.DEFAULT set(value) { intakeTalon.set(ControlMode.PercentOutput, value.speed) field = value } + /** + * Sets Arm state to the given argument in IntakeCommand + */ var armState = Constants.Intake.ArmPosition.DEFAULT set(value) { intakeDoubleSolenoid.set(value.position) field = value } + /** + * @property intakeTalon configurations reset to ensure no saved data + */ init { + intakeTalon.configFactoryDefault() Logger.addSource(Constants.Intake.TAB, "Intake State") { intakeState.toString() } Logger.addSource(Constants.Intake.TAB, "Intake Motor Power") { intakeTalon.motorOutputPercent } Logger.addSource(Constants.Intake.TAB, "Intake Motor Stator Current") { intakeTalon.statorCurrent }