Skip to content
This repository has been archived by the owner on Dec 19, 2024. It is now read-only.

Commit

Permalink
No errors
Browse files Browse the repository at this point in the history
  • Loading branch information
Taimorioka committed Nov 8, 2024
1 parent c36ed95 commit 60b3c19
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 23 deletions.
Original file line number Diff line number Diff line change
@@ -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)

}
Original file line number Diff line number Diff line change
Expand Up @@ -6,31 +6,29 @@ 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))
}

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])
Expand All @@ -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)
}
}

0 comments on commit 60b3c19

Please sign in to comment.