diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt index 35b54b0..a78485c 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt @@ -7,4 +7,7 @@ object IntakeConstants { const val algaeScoreTimeoutSeconds = 0.5 const val intakeCurrentThreshold = 10.0 // 10 Amps before system knows it has a game piece + + const val coralLaserCANid=0 //? + const val algaeLaserCANid=1 //? } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt index 85d85fb..834264e 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt @@ -2,7 +2,10 @@ package frc.robot.subsystems.superstructure.intake interface IntakeIO { fun setIntakeState(state: IntakeState) - fun watchForIntake() + fun getIntakeState():IntakeState + + fun hasCoral():Boolean + fun hasAlgae():Boolean fun periodic() } diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt index 2857a01..2cfe7c8 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt @@ -13,14 +13,12 @@ import frc.robot.subsystems.superstructure.intake.* // It also reduces the need to have reference variables for the subsystems to be passed around. class IntakeSystem(private val io: IntakeIO) : SubsystemBase() { private fun setIntakeState(state: IntakeState) = Request.withAction { io.setIntakeState(state) } - private fun watchForIntake() = Request.withAction { io.watchForIntake() } private fun setGamePieceState(state: GamePieceState) = Request.withAction { RobotState.gamePieceState = state } fun coralIntake() = SequentialRequest( setIntakeState(IntakeState.CoralIntake), - WaitRequest(IntakeConstants.coralIntakeTimeoutSeconds), - watchForIntake() + WaitRequest(IntakeConstants.coralIntakeTimeoutSeconds) ) fun coralScore() = SequentialRequest( @@ -32,8 +30,7 @@ class IntakeSystem(private val io: IntakeIO) : SubsystemBase() { fun algaeIntake() = SequentialRequest( setIntakeState(IntakeState.AlgaeIntake), - WaitRequest(IntakeConstants.algaeIntakeTimeoutSeconds), - watchForIntake() + WaitRequest(IntakeConstants.algaeIntakeTimeoutSeconds) ) fun algaeScore() = SequentialRequest( @@ -46,6 +43,21 @@ class IntakeSystem(private val io: IntakeIO) : SubsystemBase() { override fun periodic() { io.periodic() + when (io.getIntakeState()) { + IntakeState.CoralIntake -> { + if(io.hasCoral()) + RobotState.gamePieceState = GamePieceState.Coral + } + + IntakeState.AlgaeIntake -> { + if (io.hasAlgae()) + RobotState.gamePieceState = GamePieceState.Algae + } + + else -> {} + } + + setIntakeState(IntakeState.Idle) } override fun simulationPeriodic() {} diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt index 2b93c1d..78b965b 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt @@ -6,6 +6,8 @@ import frc.robot.subsystems.superstructure.intake.IntakeState class IntakeIOEmpty() : IntakeIO { override fun setIntakeState(state: IntakeState) {} - override fun watchForIntake() {} + override fun getIntakeState(): IntakeState {return IntakeState.Idle} + override fun hasCoral():Boolean{return true} + override fun hasAlgae():Boolean{return false} override fun periodic() {} } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt index e28e469..93a50be 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt @@ -11,6 +11,7 @@ import com.revrobotics.spark.SparkLowLevel import com.revrobotics.spark.SparkMax import com.revrobotics.spark.config.SparkMaxConfig import edu.wpi.first.math.util.Units +import edu.wpi.first.units.Measure import frc.robot.RobotState import frc.robot.constants.CANConstants import frc.robot.subsystems.superstructure.intake.GamePieceState @@ -23,12 +24,13 @@ class IntakeIOReal : IntakeIO { private var intakeState = IntakeState.Idle - private var watchingCurrent = false + private val coralLaserCAN=LaserCan(IntakeConstants.coralLaserCANid) + private val algaeLaserCAN=LaserCan(IntakeConstants.algaeLaserCANid) init { - val coralIntakeMotorConfiguration = TalonFXConfiguration() - coralIntakeMotorConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive - intakeMotor.configurator.apply(coralIntakeMotorConfiguration) + val intakeMotorConfiguration = TalonFXConfiguration() + intakeMotorConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive + intakeMotor.configurator.apply(intakeMotorConfiguration) } override fun setIntakeState(state: IntakeState) { @@ -36,29 +38,21 @@ class IntakeIOReal : IntakeIO { intakeMotor.set(state.speed) } - override fun watchForIntake() { - watchingCurrent = true + override fun getIntakeState():IntakeState{ + return intakeState } - override fun periodic() { - if (watchingCurrent) { - if (intakeMotor.supplyCurrent.valueAsDouble > IntakeConstants.intakeCurrentThreshold) { - when (intakeState) { - IntakeState.CoralIntake -> { - RobotState.gamePieceState = GamePieceState.Coral - } - - IntakeState.AlgaeIntake -> { - RobotState.gamePieceState = GamePieceState.Algae - } - - else -> {} - } - - setIntakeState(IntakeState.Idle) + override fun hasCoral():Boolean{ + val measurement:Measurement=coralLaserCAN.measurement + @Suppress("SENSELESS_COMPARISON") + return (measurement!=null&&measurement.status==LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT&&measurement.distance_mm