diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt new file mode 100644 index 00000000..b3be6deb --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -0,0 +1,21 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.volts + +object IntakeConstants { + val VOLTAGE_COMPENSATION = 12.0.volts + + // TODO: Add value for encoder offset + val ABSOLUTE_ENCODER_OFFSET = 0.0.degrees + + // TODO: Change gear ratio according to robot + val ROLLER_CURRENT_LIMIT = 50.0.amps + val ARM_CURRENT_LIMIT = 50.0.amps + const val ROLLER_MOTOR_INVERTED = true + const val ARM_MOTOR_INVERTED = false + const val ROLLER_GEAR_RATIO = 36.0 / 18.0 + const val ARM_GEAR_RATIO = ((60.0 / 12.0) * (80.0 / 18.0) * (32.0 / 16.0)) + const val ARM_ENCODER_GEAR_RATIO = 36.0 / 18.0 +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index df493fe8..c1ef2b6f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.Shooter -class Shooter { -} \ No newline at end of file +class Shooter diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt index a1af62e2..53e9e9ed 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.TelescopingArm -class TelescopingArm { -} \ No newline at end of file +class TelescopingArm diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index bb51b835..f7209b2a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -1,6 +1,5 @@ package com.team4099.robot2023.subsystems.feeder import edu.wpi.first.wpilibj2.command.SubsystemBase -class Feeder(val io: FeederIO) : SubsystemBase() { -} \ No newline at end of file +class Feeder(val io: FeederIO) : SubsystemBase() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 41ddbf24..780fc386 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.feeder -interface FeederIO { -} \ No newline at end of file +interface FeederIO diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 25f7823f..4e49f3d6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.intake -class Intake { -} \ No newline at end of file +class Intake diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt index ba1bd94c..b06c5c0a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt @@ -6,33 +6,127 @@ 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.inCelsius -import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.inRotationsPerMinute import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond interface IntakeIO { - class IntakeIOInputs : LoggableInputs { - var rollerVelocity = 0.0.rotations.perMinute + class IntakeIOInputs : LoggableInputs { + var rollerVelocity = 0.0.rotations.perMinute + var rollerAppliedVoltage = 0.0.volts - var rollerAppliedVoltage = 0.0.volts - var rollerSupplyCurrent = 0.0.amps - var rollerStatorCurrent = 0.0.amps - var rollerTemp = 0.0.celsius + var rollerSupplyCurrent = 0.0.amps + var rollerStatorCurrent = 0.0.amps + var rollerTemp = 0.0.celsius - override fun toLog(table: LogTable?) { - table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) + var armPosition = 0.0.degrees + var armVelocity = 0.0.degrees.perSecond + var armAbsoluteEncoderPosition = 0.0.degrees - table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) + var armAppliedVoltage = 0.0.volts + var armSupplyCurrent = 0.0.amps + var armStatorCurrent = 0.0.amps + var armTemp = 0.0.celsius - table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes) + override fun toLog(table: LogTable?) { + table?.put("armPositionDegrees", armPosition.inDegrees) + table?.put("armAbsoluteEncoderPositionDegrees", armAbsoluteEncoderPosition.inDegrees) + table?.put("armVelocityDegreesPerSec", armVelocity.inDegreesPerSecond) + table?.put("armAppliedVoltage", armAppliedVoltage.inVolts) + table?.put("armSupplyCurrentAmps", armSupplyCurrent.inAmperes) + table?.put("armStatorCurrentAmps", armStatorCurrent.inAmperes) + table?.put("armTempCelsius", armTemp.inCelsius) - table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes) + table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) + table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) + table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes) + table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes) + table?.put("rollerTempCelsius", rollerTemp.inCelsius) + } + + override fun fromLog(table: LogTable?) { + table?.get("armPositionDegrees", armPosition.inDegrees)?.let { armPosition = it.degrees } + + table?.get("armAbsoluteEncoderPositionDegrees", armAbsoluteEncoderPosition.inDegrees)?.let { + armAbsoluteEncoderPosition = it.degrees + } + + table?.get("armVelocityDegreesPerSec", armVelocity.inDegreesPerSecond)?.let { + armVelocity = it.degrees.perSecond + } + + table?.get("armAppliedVoltage", armAppliedVoltage.inVolts)?.let { + armAppliedVoltage = it.volts + } + + table?.get("armSupplyCurrentAmps", armSupplyCurrent.inAmperes)?.let { + armSupplyCurrent = it.amps + } + + table?.get("armStatorCurrentAmps", armStatorCurrent.inAmperes)?.let { + armStatorCurrent = it.amps + } + + table?.get("armTempCelsius", armTemp.inCelsius)?.let { armTemp = it.celsius } + + table?.get("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)?.let { + rollerVelocity = it.rotations.perSecond + } + + table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let { + rollerAppliedVoltage = it.volts + } - table?.put("rollerTempCelsius", rollerTemp.inCelsius) - } + table?.get("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)?.let { + rollerSupplyCurrent = it.amps + } + + table?.get("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)?.let { + rollerStatorCurrent = it.amps + } + + table?.get("rollerTempCelsius", rollerTemp.inCelsius)?.let { rollerTemp = it.celsius } } -} \ No newline at end of file + } + + fun updateInputs(io: IntakeIOInputs) {} + + fun setRollerVoltage(voltage: ElectricalPotential) {} + + fun setArmVoltage(voltage: ElectricalPotential) {} + + fun setArmPosition(armPosition: Angle, feedforward: ElectricalPotential) {} + + /** + * Updates the PID constants using the implementation controller + * + * @param kP accounts for linear error + * @param kI accounts for integral error + * @param kD accounts for derivative error + */ + fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} + + /** Sets the current encoder position to be the zero value */ + fun zeroEncoder() {} + + fun setRollerBrakeMode(brake: Boolean) {} + + fun setArmBrakeMode(brake: Boolean) +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt new file mode 100644 index 00000000..103faf83 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt @@ -0,0 +1,256 @@ +package com.team4099.robot2023.subsystems.intake + +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import com.revrobotics.SparkMaxPIDController +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.IntakeConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.subsystems.falconspin.Neo +import edu.wpi.first.wpilibj.DutyCycleEncoder +import org.littletonrobotics.junction.Logger +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.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxAngularMechanismSensor +import kotlin.math.IEEErem +import kotlin.math.absoluteValue + +object IntakeIONEO : IntakeIO { + private val rollerSparkMax = + CANSparkMax(Constants.Intake.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val armSparkMax = + CANSparkMax(Constants.Intake.ARM_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val rollerSensor = + sparkMaxAngularMechanismSensor( + rollerSparkMax, IntakeConstants.ROLLER_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION + ) + + private val armSensor = + sparkMaxAngularMechanismSensor( + armSparkMax, IntakeConstants.ARM_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION + ) + + private val armEncoder = armSparkMax.encoder + + private val throughBoreEncoder = DutyCycleEncoder(Constants.Intake.REV_ENCODER_PORT) + + private val armPIDController: SparkMaxPIDController = armSparkMax.pidController + + // gets the reported angle from the through bore encoder + private val encoderAbsolutePosition: Angle + get() { + var output = + ( + (-throughBoreEncoder.absolutePosition.rotations) * + IntakeConstants.ARM_ENCODER_GEAR_RATIO + ) + + if (output in (-55).degrees..0.0.degrees) { + output -= 180.degrees + } + + return output + } + + // uses the absolute encoder position to calculate the arm position + private val armAbsolutePosition: Angle + get() { + return (encoderAbsolutePosition - IntakeConstants.ABSOLUTE_ENCODER_OFFSET).inDegrees.IEEErem( + 360.0 + ) + .degrees + } + + init { + rollerSparkMax.restoreFactoryDefaults() + rollerSparkMax.clearFaults() + + rollerSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) + rollerSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) + rollerSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED + + rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + + rollerSparkMax.openLoopRampRate = 0.0 + rollerSparkMax.burnFlash() + + armSparkMax.restoreFactoryDefaults() + armSparkMax.clearFaults() + + armSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) + armSparkMax.setSmartCurrentLimit(IntakeConstants.ARM_CURRENT_LIMIT.inAmperes.toInt()) + armSparkMax.inverted = IntakeConstants.ARM_MOTOR_INVERTED + armSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + + armSparkMax.burnFlash() + + MotorChecker.add( + "Ground Intake", + "Roller", + MotorCollection( + mutableListOf(Neo(rollerSparkMax, "Roller Motor")), + IntakeConstants.ROLLER_CURRENT_LIMIT, + 70.celsius, + IntakeConstants.ROLLER_CURRENT_LIMIT - 0.amps, + 90.celsius + ), + ) + + MotorChecker.add( + "Ground Intake", + "Pivot", + MotorCollection( + mutableListOf(Neo(armSparkMax, "Pivot Motor")), + IntakeConstants.ARM_CURRENT_LIMIT, + 70.celsius, + IntakeConstants.ARM_CURRENT_LIMIT - 30.amps, + 90.celsius + ) + ) + } + + override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) { + inputs.rollerVelocity = rollerSensor.velocity + inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput + inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps + + // BusVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BusVoltage + // SupplyCurrent = (percentOutput * BusVoltage / BusVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.rollerSupplyCurrent = + inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue + inputs.rollerTemp = rollerSparkMax.motorTemperature.celsius + + inputs.armPosition = armSensor.position + inputs.armAbsoluteEncoderPosition = armAbsolutePosition + inputs.armVelocity = armSensor.velocity + inputs.armAppliedVoltage = armSparkMax.busVoltage.volts * armSparkMax.appliedOutput + inputs.armStatorCurrent = armSparkMax.outputCurrent.amps + inputs.armTemp = armSparkMax.motorTemperature.celsius + + // same math as rollersupplycurrent + inputs.armSupplyCurrent = inputs.armStatorCurrent * armSparkMax.appliedOutput.absoluteValue + + Logger.recordOutput( + "GroundIntake/absoluteEncoderPositionDegrees", encoderAbsolutePosition.inDegrees + ) + + Logger.recordOutput( + "GroundIntake/rollerMotorOvercurrentFault", + rollerSparkMax.getFault(CANSparkMax.FaultID.kOvercurrent) + ) + Logger.recordOutput("GroundIntake/busVoltage", rollerSparkMax.busVoltage) + + Logger.recordOutput( + "GroundIntake/armSensorVelocity", + armSparkMax.encoder.velocity * IntakeConstants.ARM_GEAR_RATIO + ) + } + + /** + * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation + * + * @param voltage the voltage to set the roller motor to + */ + override fun setRollerVoltage(voltage: ElectricalPotential) { + rollerSparkMax.setVoltage( + clamp(voltage, -IntakeConstants.VOLTAGE_COMPENSATION, IntakeConstants.VOLTAGE_COMPENSATION) + .inVolts + ) + } + + /** + * Sets the arm motor voltage, ensures the voltage is limited to battery voltage compensation + * + * @param voltage the voltage to set the arm motor to + */ + override fun setArmVoltage(voltage: ElectricalPotential) { + armSparkMax.setVoltage(voltage.inVolts) + + Logger.recordOutput("GroundIntake/commandedVoltage", voltage.inVolts) + } + + /** + * Sets the arm to a desired angle, uses feedforward to account for external forces in the system + * The armPIDController uses the previously set PID constants and ff to calculate how to get to + * the desired position + * + * @param armPosition the desired angle to set the aerm to + * @param feedforward the amount of volts to apply for feedforward + */ + override fun setArmPosition(armPosition: Angle, feedforward: ElectricalPotential) { + armPIDController.setReference( + armSensor.positionToRawUnits(armPosition), + CANSparkMax.ControlType.kPosition, + 0, + feedforward.inVolts + ) + } + + /** + * Updates the PID constants using the implementation controller, uses arm sensor to convert from + * PID constants to motor controller units + * + * @param kP accounts for linear error + * @param kI accounts for integral error + * @param kD accounts for derivative error + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + armPIDController.p = armSensor.proportionalPositionGainToRawUnits(kP) + armPIDController.i = armSensor.integralPositionGainToRawUnits(kI) + armPIDController.d = armSensor.derivativePositionGainToRawUnits(kD) + } + + /** recalculates the current position of the neo encoder using value from the absolute encoder */ + override fun zeroEncoder() { + armEncoder.position = armSensor.positionToRawUnits(armAbsolutePosition) + } + + /** + * Sets the roller motor brake mode + * + * @param brake if it brakes + */ + override fun setRollerBrakeMode(brake: Boolean) { + if (brake) { + rollerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + } else { + rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + } + } + + /** + * Sets the arm motor brake mode + * + * @param brake if it brakes + */ + override fun setArmBrakeMode(brake: Boolean) { + if (brake) { + rollerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + } else { + rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + } + } +}