diff --git a/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt b/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt index 47ff5bd..9022fa9 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt @@ -37,6 +37,7 @@ enum class CTREDeviceId(val num: Int, val bus: String) { BackRightDrivingMotor(3, "*"), FrontRightDrivingMotor(4, "*"), ElevatorMotor(11, "*"), + IntakeArmMotor(12, "*"), PigeonGyro(20, "*"), ElevatorEncoder(30, "*"), } diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/intake/Intake.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/intake/Intake.kt new file mode 100644 index 0000000..a919d2e --- /dev/null +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/intake/Intake.kt @@ -0,0 +1,42 @@ +package com.frcteam3636.frc2025.subsystems.intake + +import com.frcteam3636.frc2025.Robot +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Subsystem +import org.littletonrobotics.junction.Logger + +object Intake: Subsystem { + + private val io: IntakeIO = when (Robot.model) { + Robot.Model.SIMULATION -> TODO() + Robot.Model.COMPETITION -> IntakeIOReal() + Robot.Model.PROTOTYPE -> TODO() + } + + var inputs = LoggedIntakeInputs() + + override fun periodic() { + io.updateInputs(inputs) + Logger.processInputs("Intake", inputs) + } + + fun intake(): Command = + startEnd( + { + io.setSpeed(0.5) + }, + { + io.setSpeed(0.0) + } + ) + + fun outtake(): Command = + startEnd( + { + io.setSpeed(-0.5) + }, + { + io.setSpeed(0.0) + } + ) +} \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/intake/IntakeIO.kt new file mode 100644 index 0000000..e7c3fe4 --- /dev/null +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/intake/IntakeIO.kt @@ -0,0 +1,64 @@ +package com.frcteam3636.frc2025.subsystems.intake + +import com.frcteam3636.frc2025.CTREDeviceId +import com.frcteam3636.frc2025.REVMotorControllerId +import com.frcteam3636.frc2025.SparkFlex +import com.frcteam3636.frc2025.TalonFX +import com.revrobotics.spark.SparkBase.PersistMode +import com.revrobotics.spark.SparkBase.ResetMode +import com.revrobotics.spark.SparkLowLevel +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode +import com.revrobotics.spark.config.SparkFlexConfig +import edu.wpi.first.units.Units.* +import org.team9432.annotation.Logged +import kotlin.math.roundToInt + +@Logged +open class IntakeInputs { + var rollerVelocity = RotationsPerSecond.zero()!! + var rollerCurrent = Amps.zero()!! + var rollerPosition = Radians.zero()!! + + var armVelocity = RotationsPerSecond.zero()!! + var armCurrent = Amps.zero()!! + var armPosition = Radians.zero()!! +} + +interface IntakeIO { + fun setSpeed(percent: Double) + fun setIsLowered(boolean: Boolean) + fun updateInputs(inputs: IntakeInputs) +} + +class IntakeIOReal: IntakeIO { + + private var intakeRollerMotor = SparkFlex( + REVMotorControllerId.IntakeMotor, + SparkLowLevel.MotorType.kBrushless + ).apply { + configure(SparkFlexConfig().apply { + idleMode(IdleMode.kBrake) + smartCurrentLimit(MOTOR_CURRENT_LIMIT.`in`(Amps).roundToInt()) + + }, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters) + } + + private var intakeArmMotor = TalonFX(CTREDeviceId.IntakeArmMotor) + + override fun setSpeed(percent: Double) { + assert(percent in -1.0..1.0) + intakeRollerMotor.set(percent) + } + + override fun updateInputs(inputs: IntakeInputs) { + inputs.rollerVelocity = RotationsPerSecond.of(intakeRollerMotor.encoder.velocity) + inputs.current = Amps.of(intakeRollerMotor.outputCurrent) + inputs.position = Rotations.of(intakeRollerMotor.encoder.position.mod(1.0)) + } +} + +//class IntakeIOSim: IntakeIO { +// intakeSimulation = Intakesimulation() +//} + +internal val MOTOR_CURRENT_LIMIT = Amps.of(35.0) \ No newline at end of file