diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 896ea199..81aff042 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.RobotBase import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.grams import org.team4099.lib.units.base.inMeters @@ -77,6 +78,9 @@ object DrivetrainConstants { val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds + val STEERING_TEMP_ALERT = 80.celsius + val DRIVE_TEMP_ALERT = 80.celsius + val FRONT_LEFT_MODULE_ZERO = 1.33.radians val FRONT_RIGHT_MODULE_ZERO = 4.51.radians val BACK_LEFT_MODULE_ZERO = 3.20.radians diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index 14c825c5..cec4d3e3 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -7,6 +7,7 @@ import com.team4099.robot2023.config.constants.FieldConstants.Grids.midCubeZ import com.team4099.robot2023.config.constants.FieldConstants.LoadingZone.doubleSubstationShelfZ import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.percent @@ -59,6 +60,8 @@ object ElevatorConstants { val VOLTAGE_COMPENSATION = 12.volts val PHASE_CURRENT_LIMIT = 80.amps // TODO tune stator current limit + val ELEVATOR_TEMP_ALERT = 80.celsius + // TODO figure out what these should be val HOMING_POSITION_THRESHOLD = 30.inches val HOMING_APPLIED_VOLTAGE = -1.volts diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt index 8c34d67d..6cfd11e0 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.grams import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters @@ -40,6 +41,9 @@ object GroundIntakeConstants { val ROLLER_CURRENT_LIMIT = 30.amps // TODO TUNE val ARM_CURRENT_LIMIT = 50.amps // TODO TUNE + val ROLLER_TEMP_ALERT = 80.celsius + val ARM_TEMP_ALERT = 80.celsius + const val ROLLER_MOTOR_INVERTED = true const val ARM_MOTOR_INVERTED = false diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt index 12f1abe3..145b9b29 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ManipulatorConstants.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.grams import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters @@ -52,6 +53,9 @@ object ManipulatorConstants { val ARM_STATOR_CURRENT_LIMIT = 20.amps val ROLLER_STATOR_CURRENT_LIMIT = 40.amps + val ROLLER_TEMP_ALERT = 80.celsius + val ARM_TEMP_ALERT = 80.celsius + val ARM_VOLTAGE_COMPENSATION = 12.volts val ROLLER_VOLTAGE_COMPENSATION = 12.volts diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt index 185c9a71..74361fe9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt @@ -7,6 +7,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.util.Alert import edu.wpi.first.wpilibj.AnalogInput import edu.wpi.first.wpilibj.RobotController import org.littletonrobotics.junction.Logger @@ -70,6 +71,18 @@ class SwerveModuleIOFalcon( } } + private val steeringCurrentLimitAlert = + Alert("$label steering motor surpassed current limit", Alert.AlertType.ERROR) + + private val driveCurrentLimitAlert = + Alert("$label drive motor surpassed current limit", Alert.AlertType.ERROR) + + private val steeringTempAlert = + Alert("$label steering motor surpassed temperature limit", Alert.AlertType.ERROR) + + private val driveTempAlert = + Alert("$label drive motor surpassed temperature limit", Alert.AlertType.ERROR) + init { driveFalcon.configFactoryDefault() steeringFalcon.configFactoryDefault() @@ -163,6 +176,20 @@ class SwerveModuleIOFalcon( Logger.getInstance() .recordOutput("$label/sensorVelocityRawUnits", driveFalcon.selectedSensorVelocity) Logger.getInstance().recordOutput("$label/motorOutput", driveFalcon.motorOutputPercent) + + driveCurrentLimitAlert.set( + driveFalcon.statorCurrent.amps > DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT + ) + + steeringCurrentLimitAlert.set( + steeringFalcon.statorCurrent.amps > DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT + ) + + driveTempAlert.set(driveFalcon.temperature.celsius > DrivetrainConstants.DRIVE_TEMP_ALERT) + + steeringTempAlert.set( + steeringFalcon.temperature.celsius > DrivetrainConstants.STEERING_TEMP_ALERT + ) } override fun setSteeringSetpoint(angle: Angle) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt index 7de4d285..834fac83 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt @@ -6,6 +6,9 @@ import com.revrobotics.SparkMaxPIDController import com.team4099.lib.math.clamp import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ElevatorConstants +import com.team4099.robot2023.subsystems.motorchecker.MotorChecker +import com.team4099.robot2023.subsystems.motorchecker.Neo +import com.team4099.robot2023.util.Alert import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.amps @@ -40,6 +43,18 @@ object ElevatorIONeo : ElevatorIO { private val leaderPIDController: SparkMaxPIDController = leaderSparkMax.pidController + private val leftCurrentLimitAlert = + Alert("Left Elevator motor surpassed current limit", Alert.AlertType.ERROR) + + private val rightCurrentLimitAlert = + Alert("Right Elevator motor surpassed current limit", Alert.AlertType.ERROR) + + private val leftTempAlert = + Alert("Left Elevator motor surpassed temperature limit", Alert.AlertType.ERROR) + + private val rightTempAlert = + Alert("Right Elevator motor surpassed temperature limit", Alert.AlertType.ERROR) + init { // reseting motor @@ -71,6 +86,8 @@ object ElevatorIONeo : ElevatorIO { leaderSparkMax.burnFlash() followerSparkMax.burnFlash() + + MotorChecker.add(Neo(leaderSparkMax)) } override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { @@ -100,6 +117,22 @@ object ElevatorIONeo : ElevatorIO { inputs.followerSupplyCurrent = inputs.followerStatorCurrent * followerSparkMax.appliedOutput inputs.followerTempCelcius = followerSparkMax.motorTemperature.celsius + + leftCurrentLimitAlert.set( + followerSparkMax.outputCurrent.amps > ElevatorConstants.PHASE_CURRENT_LIMIT + ) + + rightCurrentLimitAlert.set( + leaderSparkMax.outputCurrent.amps > ElevatorConstants.PHASE_CURRENT_LIMIT + ) + + leftTempAlert.set( + followerSparkMax.motorTemperature.celsius > ElevatorConstants.ELEVATOR_TEMP_ALERT + ) + + rightTempAlert.set( + leaderSparkMax.motorTemperature.celsius > ElevatorConstants.ELEVATOR_TEMP_ALERT + ) } /** diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt index 8ab14f56..3be544ae 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/groundintake/GroundIntakeIONeo.kt @@ -6,6 +6,7 @@ import com.revrobotics.SparkMaxPIDController import com.team4099.lib.math.clamp import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.GroundIntakeConstants +import com.team4099.robot2023.util.Alert import edu.wpi.first.wpilibj.DutyCycleEncoder import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.amps @@ -80,6 +81,18 @@ object GroundIntakeIONeo : GroundIntakeIO { .degrees } + private val rollerCurrentLimitAlert = + Alert("Ground intake roller motor surpassed current limit", Alert.AlertType.ERROR) + + private val armCurrentLimitAlert = + Alert("Ground arm motor surpassed current limit", Alert.AlertType.ERROR) + + private val rollerTempAlert = + Alert("Ground intake roller motor surpassed temperature limit", Alert.AlertType.ERROR) + + private val armTempAlert = + Alert("Ground arm motor surpassed temperature limit", Alert.AlertType.ERROR) + init { rollerSparkMax.restoreFactoryDefaults() rollerSparkMax.clearFaults() @@ -140,6 +153,20 @@ object GroundIntakeIONeo : GroundIntakeIO { armSparkMax.encoder.velocity * GroundIntakeConstants.ARM_OUTPUT_GEAR_RATIO.asDrivenOverDriving ) + + rollerCurrentLimitAlert.set( + rollerSparkMax.outputCurrent.amps > GroundIntakeConstants.ROLLER_CURRENT_LIMIT + ) + + armCurrentLimitAlert.set( + armSparkMax.outputCurrent.amps > GroundIntakeConstants.ARM_CURRENT_LIMIT + ) + + rollerTempAlert.set( + rollerSparkMax.motorTemperature.celsius > GroundIntakeConstants.ROLLER_TEMP_ALERT + ) + + armTempAlert.set(armSparkMax.motorTemperature.celsius > GroundIntakeConstants.ARM_TEMP_ALERT) } /** diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt index e230851d..235691d6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/manipulator/ManipulatorIONeo.kt @@ -6,6 +6,7 @@ import com.revrobotics.SparkMaxPIDController import com.team4099.lib.math.clamp import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ManipulatorConstants +import com.team4099.robot2023.util.Alert import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.amps @@ -42,6 +43,18 @@ object ManipulatorIONeo : ManipulatorIO { ) private val armPIDController: SparkMaxPIDController = armSparkMax.pidController + private val rollerCurrentLimitAlert = + Alert("Manipulator roller motor surpassed current limit", Alert.AlertType.ERROR) + + private val armCurrentLimitAlert = + Alert("Manipulator extension motor surpassed current limit", Alert.AlertType.ERROR) + + private val rollerTempAlert = + Alert("Manipulator roller motor surpassed temperature limit", Alert.AlertType.ERROR) + + private val armTempAlert = + Alert("Manipulator extension motor surpassed temperature limit", Alert.AlertType.ERROR) + init { // resetting the motors rollerSparkMax.restoreFactoryDefaults() @@ -118,6 +131,20 @@ object ManipulatorIONeo : ManipulatorIO { // percentOutput * statorCurrent inputs.armSupplyCurrent = inputs.armStatorCurrent * armSparkMax.appliedOutput inputs.armTemp = armSparkMax.motorTemperature.celsius + + rollerCurrentLimitAlert.set( + rollerSparkMax.outputCurrent.amps > ManipulatorConstants.ROLLER_STATOR_CURRENT_LIMIT + ) + + armCurrentLimitAlert.set( + armSparkMax.outputCurrent.amps > ManipulatorConstants.ARM_STATOR_CURRENT_LIMIT + ) + + rollerTempAlert.set( + rollerSparkMax.motorTemperature.celsius > ManipulatorConstants.ROLLER_TEMP_ALERT + ) + + armTempAlert.set(armSparkMax.motorTemperature.celsius > ManipulatorConstants.ARM_TEMP_ALERT) } /** diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/motorchecker/Motor.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/motorchecker/Motor.kt new file mode 100644 index 00000000..cd9f9911 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/motorchecker/Motor.kt @@ -0,0 +1,274 @@ +package com.team4099.robot2023.subsystems.motorchecker + +import com.ctre.phoenix.ErrorCode +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration +import com.ctre.phoenix.motorcontrol.StickyFaults +import com.ctre.phoenix.motorcontrol.can.TalonFX +import com.revrobotics.CANSparkMax +import com.revrobotics.REVLibError +import org.team4099.lib.units.base.Current +import org.team4099.lib.units.base.Temperature +import org.team4099.lib.units.base.Time +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.volts + +interface MotorType + +typealias REVNeo = MotorType + +typealias Falcon = MotorType + +enum class CURRENT_STAGE_LIMIT { + NONE, + BASE, + FIRST, + SHUTDOWN +} + +abstract class Motor { + open val name: String = "" + + open val appliedVoltage: ElectricalPotential = 0.0.volts + open val busVoltage: ElectricalPotential = 0.0.volts + + open val temperature: Temperature = 0.0.celsius + + open val statorCurrent: Current = 0.0.amps + open val supplyCurrent: Current = 0.0.amps + + open var currentLimitStage: CURRENT_STAGE_LIMIT = CURRENT_STAGE_LIMIT.NONE + + open val baseCurrentLimit: Current = 0.0.amps + + open val firstStageTemperatureLimit: Temperature = 0.0.celsius + open val firstStageCurrentLimit: Current = 0.0.amps + + open val motorShutDownThreshold: Temperature = 0.0.celsius + + // this should ONLY be called once on init + open val stickyFaults: List = listOf() + + open val currentLimitInUse: Current + get() = + if (currentLimitStage == CURRENT_STAGE_LIMIT.BASE) baseCurrentLimit + else firstStageCurrentLimit + + open val id: Int = -1 + open fun setCurrentLimit( + limit: Current, + thresholdLimit: Current? = null, + thresholdTime: Time? = null + ): Boolean { + return false + } + + open fun shutdown(): Boolean { + return false + } + // inner class MotorLoggableInputs: LoggableInputs{ + // override fun toLog(table: LogTable?) { + // table?.put("motorName", name) + // table?.put("${name}AppliedVoltageVolts", appliedVoltage.inVolts) + // table?.put("${name}BusVoltageVolts", busVoltage.inVolts) + // table?.put("${name}TemperatureCelsius", temperature.inCelsius) + // table?.put("${name}StatorCurrentAmps", statorCurrent.inAmperes) + // table?.put("${name}SupplyCurrentAmps", supplyCurrent.inAmperes) + // table?.put("${name}CurrentLimitStage", currentLimitStage.name) + // table?.put("${name}BaseCurrentLimitAmps", baseCurrentLimit.inAmperes) + // table?.put("${name}FirstStageTemperatureLimitCelsius", + // firstStageTemperatureLimit.inCelsius) + // table?.put("${name}FirstStageCurrentLimitAmps", firstStageCurrentLimit.inAmperes) + // table?.put("${name}MotorShutDownThresholdCelsius", motorShutDownThreshold.inCelsius) + // table?.put("${name}CurrentLimitInUseAmps", currentLimitInUse.inAmperes) + // table?.put("${name}MotorID", id.toLong()) + // } + // + // override fun fromLog(table: LogTable?) { + // TODO("Not yet implemented") // don't tbh + // } + // + // } +} + +class Neo( + private val canSparkMax: CANSparkMax, + override val name: String, + override val baseCurrentLimit: Current, + override val firstStageTemperatureLimit: Temperature, + override val firstStageCurrentLimit: Current, + override val motorShutDownThreshold: Temperature +) : Motor() { + override val busVoltage: ElectricalPotential + get() = canSparkMax.busVoltage.volts + + override val appliedVoltage: ElectricalPotential + get() = busVoltage * canSparkMax.appliedOutput + override val statorCurrent: Current + get() = canSparkMax.outputCurrent.amps + + override val temperature: Temperature + get() = canSparkMax.motorTemperature.celsius + + override val supplyCurrent: Current + get() = statorCurrent * canSparkMax.appliedOutput + + override var currentLimitStage = CURRENT_STAGE_LIMIT.NONE + override val id: Int + get() = canSparkMax.deviceId + + override val stickyFaults: List + get() = + canSparkMax + .stickyFaults + .toUInt() + .toString(radix = 2) + .mapIndexedNotNull { index, c -> index.takeIf { c == '1' } } + .map { CANSparkMax.FaultID.fromId(it).name } + + override fun setCurrentLimit( + limit: Current, + thresholdLimit: Current?, + thresholdTime: Time? + ): Boolean { + val statusCode = canSparkMax.setSmartCurrentLimit(limit.inAmperes.toInt()) + + var secondaryLimitStatusCode: REVLibError = REVLibError.kError + if (statusCode == REVLibError.kOk) { + // setting secondary limit to something absolutely absurd so it'll never shut down because of + // this + secondaryLimitStatusCode = + canSparkMax.setSecondaryCurrentLimit(currentLimitInUse.inAmperes + 150) + + if (limit == firstStageCurrentLimit) { + currentLimitStage = CURRENT_STAGE_LIMIT.FIRST + } else if (limit == baseCurrentLimit) { + currentLimitStage = CURRENT_STAGE_LIMIT.BASE + } + } + + return statusCode == REVLibError.kOk && secondaryLimitStatusCode == REVLibError.kOk + } + + override fun shutdown(): Boolean { + // setting the stator current limit to 0, so it immediately triggers and shuts off motor output + val statusCode = canSparkMax.setSecondaryCurrentLimit(0.0) + if (statusCode == REVLibError.kOk) { + currentLimitStage = CURRENT_STAGE_LIMIT.SHUTDOWN + } + + return statusCode == REVLibError.kOk + } +} + +class Falcon500( + private val falcon500: TalonFX, + override val name: String, + override val baseCurrentLimit: Current, + override val firstStageTemperatureLimit: Temperature, + override val firstStageCurrentLimit: Current, + override val motorShutDownThreshold: Temperature = 110.celsius +) : Motor() { + + override val appliedVoltage: ElectricalPotential + get() = falcon500.motorOutputVoltage.volts + + override val busVoltage: ElectricalPotential + get() = falcon500.busVoltage.volts + + override val temperature: Temperature + get() = falcon500.temperature.celsius + + override val statorCurrent: Current + get() = falcon500.statorCurrent.amps + + override val supplyCurrent: Current + get() = falcon500.supplyCurrent.amps + + override val id: Int + get() = falcon500.deviceID + + override var currentLimitStage = CURRENT_STAGE_LIMIT.NONE + + override val stickyFaults: List + get() { + val faults = StickyFaults() + falcon500.getStickyFaults(faults) + val retVal = mutableListOf() + + if (faults.UnderVoltage) { + retVal.add("UnderVoltage") + } + if (faults.ForwardLimitSwitch) { + retVal.add("ForwardLimitSwitch") + } + if (faults.ReverseLimitSwitch) { + retVal.add("ReverseLimitSwitch") + } + if (faults.ForwardSoftLimit) { + retVal.add("ForwardSoftLimit") + } + if (faults.ReverseSoftLimit) { + retVal.add("ReverseSoftLimit") + } + if (faults.ResetDuringEn) { + retVal.add("ResetDuringEn") + } + if (faults.SensorOverflow) { + retVal.add("SensorOverflow") + } + if (faults.SensorOutOfPhase) { + retVal.add("SensorOutOfPhase") + } + if (faults.HardwareESDReset) { + retVal.add("HardwareESDReset") + } + if (faults.RemoteLossOfSignal) { + retVal.add("RemoteLossOfSignal") + } + if (faults.APIError) { + retVal.add("APIError") + } + if (faults.SupplyOverV) { + retVal.add("SupplyOverV") + } + if (faults.SupplyUnstable) { + retVal.add("SupplyUnstable") + } + + return retVal + } + + override fun setCurrentLimit( + limit: Current, + thresholdLimit: Current?, + thresholdTime: Time? + ): Boolean { + if (thresholdLimit != null && thresholdTime != null) { + val statorCurrentConfigSuccess = + falcon500.configStatorCurrentLimit( + StatorCurrentLimitConfiguration( + true, limit.inAmperes, thresholdLimit.inAmperes, thresholdTime.inSeconds + ) + ) + + if (statorCurrentConfigSuccess == ErrorCode.OK) { + if (limit == firstStageCurrentLimit) { + currentLimitStage = CURRENT_STAGE_LIMIT.FIRST + } else if (limit == baseCurrentLimit) { + currentLimitStage = CURRENT_STAGE_LIMIT.BASE + } + } + + return statorCurrentConfigSuccess == ErrorCode.OK + } + return false + } + + override fun shutdown(): Boolean { + return temperature > 110.celsius // falcons have in built temperature shut down + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/motorchecker/MotorChecker.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/motorchecker/MotorChecker.kt new file mode 100644 index 00000000..d9518d09 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/motorchecker/MotorChecker.kt @@ -0,0 +1,85 @@ +package com.team4099.robot2023.subsystems.motorchecker + +import edu.wpi.first.wpilibj2.command.SubsystemBase +import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inCelsius +import org.team4099.lib.units.derived.inVolts + +object MotorChecker : SubsystemBase() { + + val motors = mutableListOf() + + fun add(subsystemMotorCollection: MotorCollection) { + motors.add(subsystemMotorCollection) + } + + override fun periodic() { + for (motorCollection in motors) { + + // base current limit + if (motorCollection.maxMotorTemperature < motorCollection.firstStageTemperatureLimit && + motorCollection.currentLimitStage != CURRENT_STAGE_LIMIT.BASE + ) { + motorCollection.setCurrentLimit(motorCollection.baseCurrentLimit) + } + + // first stage current limit + if (motorCollection.maxMotorTemperature in + motorCollection.firstStageTemperatureLimit..motorCollection.motorShutDownThreshold && + motorCollection.currentLimitStage != CURRENT_STAGE_LIMIT.FIRST + ) { + motorCollection.setCurrentLimit(motorCollection.firstStageCurrentLimit) + } + + for (motor in motorCollection.motorCollection) { + // complete motor shutdown but we don't want to shut down all motors at once + if (motor.temperature > motor.motorShutDownThreshold) { + motor.shutdown() + } + + logMotor(motor) + } + } + } +} + +// not clean but whatever +fun logMotor(motor: Motor) { + Logger.getInstance() + .recordOutput("MotorChecker/${motor.name}/AppliedVoltageVolts", motor.appliedVoltage.inVolts) + Logger.getInstance() + .recordOutput("MotorChecker/${motor.name}/BusVoltageVolts", motor.busVoltage.inVolts) + Logger.getInstance() + .recordOutput("MotorChecker/${motor.name}/TemperatureCelsius", motor.temperature.inCelsius) + Logger.getInstance() + .recordOutput("MotorChecker/${motor.name}/StatorCurrentAmps", motor.statorCurrent.inAmperes) + Logger.getInstance() + .recordOutput("MotorChecker/${motor.name}/SupplyCurrentAmps", motor.supplyCurrent.inAmperes) + Logger.getInstance() + .recordOutput("MotorChecker/${motor.name}/CurrentLimitStage", motor.currentLimitStage.name) + Logger.getInstance() + .recordOutput( + "MotorChecker/${motor.name}/BaseCurrentLimitAmps", motor.baseCurrentLimit.inAmperes + ) + Logger.getInstance() + .recordOutput( + "MotorChecker/${motor.name}/FirstStageTemperatureLimitCelsius", + motor.firstStageTemperatureLimit.inCelsius + ) + Logger.getInstance() + .recordOutput( + "MotorChecker/${motor.name}/FirstStageCurrentLimitAmps", + motor.firstStageCurrentLimit.inAmperes + ) + Logger.getInstance() + .recordOutput( + "MotorChecker/${motor.name}/MotorShutDownThresholdCelsius", + motor.motorShutDownThreshold.inCelsius + ) + Logger.getInstance() + .recordOutput( + "MotorChecker/${motor.name}/CurrentLimitInUseAmps", motor.currentLimitInUse.inAmperes + ) + Logger.getInstance().recordOutput("MotorChecker/${motor.name}/MotorID", motor.id.toLong()) +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/motorchecker/MotorCollection.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/motorchecker/MotorCollection.kt new file mode 100644 index 00000000..7c1bf371 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/motorchecker/MotorCollection.kt @@ -0,0 +1,31 @@ +package com.team4099.robot2023.subsystems.motorchecker + +import org.team4099.lib.units.base.Current +import org.team4099.lib.units.base.Temperature + +data class MotorCollection( + val motorCollection: List>, + val baseCurrentLimit: Current, + val firstStageTemperatureLimit: Temperature, + val firstStageCurrentLimit: Current, + val motorShutDownThreshold: Temperature +) { + + val maxMotorTemperature: Temperature + get() = motorCollection.maxOf { it.temperature } + + fun setCurrentLimit(limit: Current): Boolean { + var retValue = true + for (motor in motorCollection) { + retValue = retValue and motor.setCurrentLimit(limit) + } + + return retValue + } + + val currentLimitStage: CURRENT_STAGE_LIMIT + get() = + if (motorCollection.all { it.currentLimitStage == motorCollection[0].currentLimitStage }) + motorCollection[0].currentLimitStage + else CURRENT_STAGE_LIMIT.NONE +}