diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt index f42fcd3..b3cd0dc 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt @@ -1,31 +1,41 @@ package com.frcteam3636.frc2024.subsystems.indexer +import com.revrobotics.ColorMatch +import edu.wpi.first.wpilibj.util.Color import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands.startEnd import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger -object Indexer { +object Indexer: Subsystem { private var io: IndexerIO = IndexerIOReal() + private var colorMatcher = ColorMatch().apply { + addColorMatch(RED_BALLOON_COLOR) + addColorMatch(BLUE_BALLOON_COLOR) + } var inputs = IndexerIO.IndexerInputs() + var currentColor: Color? = null override fun periodic() { io.updateInputs(inputs) Logger.processInputs("Indexer", inputs) + currentColor = colorMatcher.matchClosestColor(inputs.balloonColor).color } fun progressBalloon(): Command = startEnd( - {io.setSpinSpeed(0.5)} + {io.setSpinSpeed(0.5)}, + {io.setSpinSpeed(0.0)} ) fun outtakeBalloon(): Command = startEnd( - {io.setSpinSpeed(-0.5)} - ) - - fun stopIndexerSpin(): Command = - startEnd( + {io.setSpinSpeed(-0.5)}, {io.setSpinSpeed(0.0)} ) + + val RED_BALLOON_COLOR = Color(255, 0, 0) + val BLUE_BALLOON_COLOR = Color(0, 0, 255) + } \ No newline at end of file diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt index 8a877b8..50c8ec0 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt @@ -6,23 +6,22 @@ import org.littletonrobotics.junction.inputs.LoggableInputs import com.frcteam3636.frc2024.CANSparkFlex import com.frcteam3636.frc2024.REVMotorControllerId import com.revrobotics.CANSparkLowLevel +import edu.wpi.first.networktables.NetworkTableInstance import kotlin.doubleArrayOf import edu.wpi.first.wpilibj.util.Color -import edu.wpi.first.wpilibj. + interface IndexerIO{ class IndexerInputs : LoggableInputs { var indexerVelocity = Rotation2d() var indexerCurrent: Double = 0.0 - var isSpinningBalloon: Boolean = false var hasBalloon: Boolean = false - var balloonColor: Color = Color.kblack + var balloonColor: Color = Color.kBlack override fun toLog(table: LogTable?) { table?.put("Indexer Wheel Velocity", indexerVelocity) table?.put("Indexer Wheel Current", indexerCurrent) - table?.put("Is spinning", isSpinningBalloon) table?.put("Has balloon", hasBalloon) table?.put("Balloon Color", doubleArrayOf(balloonColor.red, balloonColor.green, balloonColor.blue)) } @@ -30,7 +29,6 @@ interface IndexerIO{ override fun fromLog(table: LogTable) { indexerVelocity = table.get("Indexer Velocity", indexerVelocity)!![0] indexerCurrent = table.get("Indexer Wheel Current", indexerCurrent) - isSpinningBalloon = table.get("Is spinning", isSpinningBalloon) hasBalloon = table.get("Has balloon", hasBalloon) val balloonColorArray: DoubleArray = table.get("Balloon Color", doubleArrayOf(balloonColor.red, balloonColor.green, balloonColor.blue)) balloonColor = Color(balloonColorArray[0], balloonColorArray[1], balloonColorArray[2]) @@ -49,25 +47,19 @@ class IndexerIOReal: IndexerIO{ CANSparkLowLevel.MotorType.kBrushless ) - private val colorSensorDistance = NetworkTables.getEntry("/proximity1") + private val colorSensorDistance = NetworkTableInstance.getDefault().getEntry("/proximity1") - private val colorSensorColor = NetworkTables.getEntry("/rawcolor1") + private val colorSensorColor = NetworkTableInstance.getDefault().getEntry("/rawcolor1") override fun updateInputs(inputs: IndexerIO.IndexerInputs) { inputs.indexerVelocity = Rotation2d(indexerWheel.encoder.velocity) inputs.indexerCurrent = indexerWheel.outputCurrent - inputs.isSpinningBalloon = colorSensor.get() - inputs.hasBalloon = colorSensorDistance.getValue() > 1000 - inputs.balloonColor = colorSensorColor.getValue() + inputs.hasBalloon = colorSensorDistance.getDouble(0.0) > 1000.0 + val colorArray = colorSensorColor.getDoubleArray(doubleArrayOf(0.0,0.0,0.0)) + inputs.balloonColor = Color(colorArray[0], colorArray[1], colorArray[2]) } override fun setSpinSpeed(speed: Double) { indexerWheel.set(speed) } - - internal companion object Constants { - const val COLOR_SENSOR_PORT = i2cPort - const val RED_BALLOON_COLOR = (255, 0, 0) - const val BLUE_BALLOON_COLOR = (0, 0, 255) - } } \ No newline at end of file