laser can again

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 //?
 }

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()
 }

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() {}

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() {}
 }

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,42 +24,35 @@ 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) {
         intakeState = state
         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<Units.inchesToMeters(3.5))
+    }
+    override fun hasAlgae():Boolean{
+        val measurement:Measurement=algaeLaserCAN.measurement
+        @Suppress("SENSELESS_COMPARISON")
+        return (measurement!=null&&measurement.status==LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT&&measurement.distance_mm<Units.inchesToMeters(12.5))
+    }
 
-                watchingCurrent = false
-            }
-        }
+    override fun periodic() {
     }
 }