Merge pull request #7 from CshCyberhawks/implemetent-coral-end-effector

Implemetent coral end effector

Author: PY44N

.gitignore

@@ -186,4 +186,4 @@ compile_commands.json
 # Eclipse generated file for annotation processors
 .factorypath
 
-gradle.properties
+gradle.properties

gradle.properties

@@ -1,11 +1,11 @@
 package frc.robot
 
-enum class RobotState {
+enum class RobotType {
     Real,
     Simulated,
     Empty
 }
 
 object RobotConfiguration {
-    val robotState = RobotState.Empty
+    val robotType = RobotType.Empty
 }

src/main/java/frc/robot/RobotConfiguration.kt

@@ -26,16 +26,16 @@ object RobotContainer {
 
     val vision = VisionSystem()
 
-    val drivetrain = when (RobotConfiguration.robotState) {
-        RobotState.Real -> SwerveIOReal()
-        RobotState.Simulated -> SwerveIOSim()
-        RobotState.Empty -> SwerveIOBase()
+    val drivetrain = when (RobotConfiguration.robotType) {
+        RobotType.Real -> SwerveIOReal()
+        RobotType.Simulated -> SwerveIOSim()
+        RobotType.Empty -> SwerveIOBase()
     }
 
-    val teleopDriveCommand = when (RobotConfiguration.robotState) {
-        RobotState.Real -> TeleopDriveCommand()
-        RobotState.Simulated -> SimTeleopDriveCommand()
-        RobotState.Empty -> Commands.run({})
+    val teleopDriveCommand = when (RobotConfiguration.robotType) {
+        RobotType.Real -> TeleopDriveCommand()
+        RobotType.Simulated -> SimTeleopDriveCommand()
+        RobotType.Empty -> Commands.run({})
     }
 
     init {
@@ -45,8 +45,8 @@ object RobotContainer {
     private fun configureBindings() {
         drivetrain.setDefaultCommand(teleopDriveCommand)
 
-
-        drivetrain.registerTelemetry(logger::telemeterize)
+// We might need this?
+//        drivetrain.registerTelemetry(logger::telemeterize)
     }
 
     val autonomousCommand: Command

src/main/java/frc/robot/RobotContainer.kt

@@ -0,0 +1,11 @@
+package frc.robot
+
+import frc.robot.subsystems.intake.AlgaeState
+import frc.robot.subsystems.intake.CoralState
+
+object RobotState {
+    // Sure we need these
+    // I don't think we want to use them in the superstructure, but it might make sense to reference them in swerve or auto
+    var coralState = CoralState.Empty
+    var algaeState = AlgaeState.Empty
+}

src/main/java/frc/robot/RobotState.kt

@@ -18,8 +18,8 @@ import kotlin.math.max
 class VisionSystem {
     val max_distance_m = 6.0
 
-    val limelightNames: Array<String> = when (RobotConfiguration.robotState) {
-        RobotState.Real -> arrayOf("limelight-front")
+    val limelightNames: Array<String> = when (RobotConfiguration.robotType) {
+        RobotType.Real -> arrayOf("limelight-front")
         else -> emptyArray()
     }
 

src/main/java/frc/robot/Vision.kt

@@ -1,16 +1,11 @@
 package frc.robot.subsystems.intake
 
 object IntakeConstants {
-    val kG = 0.0
-    val kS = 0.0
-    val kV = 0.0
-    val kA = 0.0
-    val kP = 0.0
-    val kI = 0.0
-    val kD = 0.0
-    val targetVelocity = 0.0
-    val targetAcceleration = 0.0
-    val targetJerk = 0.0
-    val id = 0
-    val canbus = ""
+    const val coralMotorId = 40
+    const val algaeMotorId = 41
+    const val coralLaserCANId = 42
+    const val algaeLaserCANId = 43
+
+    const val coralScoreTimeoutSeconds = 0.5
+    const val algaeScoreTimeoutSeconds = 0.5
 }

src/main/java/frc/robot/subsystems/intake/IntakeConstants.kt

@@ -1,12 +1,12 @@
 package frc.robot.subsystems.intake
 
-import com.ctre.phoenix6.configs.TalonFXConfiguration
-import com.ctre.phoenix6.hardware.TalonFX
-
 interface IntakeIO {
 //    fun getIntakeEncoder(): Double
 
     //    fun setIntakeMotor(x: Double)
     fun setCoralIntakeState(state: CoralIntakeState)
     fun setAlgaeIntakeState(state: AlgaeIntakeState)
+
+    fun getCoralState(): CoralState
+    fun getAlgaeState(): AlgaeState
 }

src/main/java/frc/robot/subsystems/intake/IntakeIO.kt

@@ -1,13 +1,24 @@
 package frc.robot.subsystems.intake
 
-enum class CoralIntakeState {
-    Intaking,
-    Idle,
-    Scoring
+enum class CoralIntakeState(val speed: Double) {
+    Intaking(0.75),
+    Idle(0.0),
+    Scoring(0.5)
 }
 
 enum class AlgaeIntakeState {
     Intaking,
     Idle,
-    Scoring
+    Scoring,
+    Holding
+}
+
+enum class CoralState {
+    Empty,
+    Stored
+}
+
+enum class AlgaeState {
+    Empty,
+    Stored
 }

src/main/java/frc/robot/subsystems/intake/IntakeState.kt

@@ -1,50 +1,51 @@
 package frc.robot.subsystems.intake
 
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.Commands
 import edu.wpi.first.wpilibj2.command.SubsystemBase
+import frc.robot.RobotState
 
 // By making a subsystem a Kotlin object, we ensure there is only ever one instance of it.
 // It also reduces the need to have reference variables for the subsystems to be passed around.
 class IntakeSystem(private val io: IntakeIO) : SubsystemBase() {
-    /**
-     * Example command factory method.
-     *
-     * @return a command
-     */
-//    fun exampleMethodCommand(): Command = runOnce {
-//        // Subsystem.runOnce() implicitly add `this` as a required subsystem.
-//        // TODO: one-time action goes here
-//    }
-
-    /**
-     * An example method querying a boolean state of the subsystem (for example, a digital sensor).
-     *
-     * @return value of some boolean subsystem state, such as a digital sensor.
-     */
-//    fun exampleCondition(): Boolean {
-//        // Query some boolean state, such as a digital sensor.
-//        return false
-//    }
+    private fun setCoralIntakeState(state: CoralIntakeState) = run { io.setCoralIntakeState(state) }
+    private fun setAlgaeIntakeState(state: AlgaeIntakeState) = run { io.setAlgaeIntakeState(state) }
+
+    private fun awaitCoralState(state: CoralState) = Commands.waitUntil { io.getCoralState() == state }
+    private fun awaitAlgaeState(state: AlgaeState) = Commands.waitUntil { io.getAlgaeState() == state }
+
+    fun coralIntake() = Commands.sequence(
+        setCoralIntakeState(CoralIntakeState.Intaking),
+        awaitCoralState(CoralState.Stored),
+        setCoralIntakeState(CoralIntakeState.Idle)
+    )
+
+    fun coralScore() = Commands.sequence(
+        setCoralIntakeState(CoralIntakeState.Scoring),
+        awaitCoralState(CoralState.Empty),
+        Commands.waitSeconds(IntakeConstants.coralScoreTimeoutSeconds),
+        setCoralIntakeState(CoralIntakeState.Idle)
+    )
+
+    fun algaeIntake() = Commands.sequence(
+        setAlgaeIntakeState(AlgaeIntakeState.Intaking),
+        awaitAlgaeState(AlgaeState.Stored),
+        setAlgaeIntakeState(AlgaeIntakeState.Holding)
+    )
+
+    fun algaeScore() = Commands.sequence(
+        setAlgaeIntakeState(AlgaeIntakeState.Scoring),
+        awaitAlgaeState(AlgaeState.Empty),
+        Commands.waitSeconds(IntakeConstants.algaeScoreTimeoutSeconds),
+        setAlgaeIntakeState(AlgaeIntakeState.Idle)
+    )
+
 
     override fun periodic() {
-        // This method will be called once per scheduler run
+        RobotState.coralState = io.getCoralState()
+        RobotState.algaeState = io.getAlgaeState()
     }
 
     override fun simulationPeriodic() {
-        // This method will be called once per scheduler run during simulation
-    }
-
-//    fun exampleAction()
-//    {
-//        // This action is called by the ExampleCommand
-//        println("ExampleSubsystem.exampleAction has been called")
-//    }
-
-    fun getPos(): Double {
-        TODO("Not yet implemented")
-        return 0.0;
-    }
-
-    fun setPos(x: Double) {
-        TODO("Not yet implemented")
     }
 }