Created an input system and got intake ready for testing

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

@@ -6,6 +6,12 @@ enum class RobotType {
     Empty
 }
 
+enum class OperatorType {
+    Manual
+}
+
 object RobotConfiguration {
     val robotType = RobotType.Empty
+
+    val operatorType = OperatorType.Manual
 }

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

@@ -1,22 +1,18 @@
 package frc.robot
 
-import MiscCalculations
-import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType
-import com.ctre.phoenix6.swerve.SwerveRequest
-import edu.wpi.first.units.Units.*
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Commands
-import edu.wpi.first.wpilibj2.command.FunctionalCommand
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import frc.robot.commands.SimTeleopDriveCommand
 import frc.robot.commands.TeleopDriveCommand
-import frc.robot.subsystems.swerve.TunerConstants
-import frc.robot.subsystems.swerve.CommandSwerveDrivetrain
+import frc.robot.subsystems.superstructure.intake.implementation.IntakeIOEmpty
+import frc.robot.subsystems.superstructure.intake.implementation.IntakeIOReal
+import frc.robot.subsystems.superstructure.intake.IntakeSystem
 import frc.robot.subsystems.swerve.SwerveIOBase
 import frc.robot.subsystems.swerve.SwerveIOReal
 import frc.robot.subsystems.swerve.SwerveIOSim
-import frc.robot.util.Telemetry
+import frc.robot.util.IO.ManualOperatorInput
 
 object RobotContainer {
     val leftJoystick: CommandJoystick = CommandJoystick(0)
@@ -38,6 +34,13 @@ object RobotContainer {
         RobotType.Empty -> Commands.run({})
     }
 
+    val intakeSystem = IntakeSystem(
+        when (RobotConfiguration.robotType) {
+            RobotType.Real -> IntakeIOReal()
+            else -> IntakeIOEmpty()
+        }
+    )
+
     init {
         configureBindings()
     }
@@ -47,6 +50,11 @@ object RobotContainer {
 
 // We might need this?
 //        drivetrain.registerTelemetry(logger::telemeterize)
+
+
+        when (RobotConfiguration.operatorType) {
+            OperatorType.Manual -> ManualOperatorInput.configureBindings()
+        }
     }
 
     val autonomousCommand: Command

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

@@ -1,7 +1,7 @@
 package frc.robot
 
-import frc.robot.subsystems.intake.AlgaeState
-import frc.robot.subsystems.intake.CoralState
+import frc.robot.subsystems.superstructure.intake.AlgaeState
+import frc.robot.subsystems.superstructure.intake.CoralState
 
 object RobotState {
     // Sure we need these

src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt

@@ -0,0 +1,18 @@
+package frc.robot.subsystems.superstructure
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+import frc.robot.subsystems.superstructure.elevator.ElevatorSystem
+import frc.robot.subsystems.superstructure.elevator.implementation.ElevatorIOEmpty
+import frc.robot.subsystems.superstructure.intake.implementation.IntakeIOEmpty
+import frc.robot.subsystems.superstructure.pivot.PivotSystem
+import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOEmpty
+import frc.robot.subsystems.superstructure.intake.IntakeSystem
+import frc.robot.subsystems.superstructure.wrist.WristSystem
+import frc.robot.subsystems.superstructure.wrist.implementation.WristIOEmpty
+
+class Superstructure() : SubsystemBase() {
+    val pivotSystem = PivotSystem(PivotIOEmpty())
+    val wristSystem = WristSystem(WristIOEmpty())
+    val elevatorSystem = ElevatorSystem(ElevatorIOEmpty())
+    val intakeSystem = IntakeSystem(IntakeIOEmpty())
+}

src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.kt → src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt

@@ -1,4 +1,4 @@
-package frc.robot.subsystems.elevator
+package frc.robot.subsystems.superstructure.elevator
 
 object ElevatorConstants {
     val kG = 0.0

src/main/java/frc/robot/subsystems/elevator/ElevatorIO.kt → src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorIO.kt

@@ -1,4 +1,4 @@
-package frc.robot.subsystems.elevator
+package frc.robot.subsystems.superstructure.elevator
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration
 import com.ctre.phoenix6.hardware.TalonFX

src/main/java/frc/robot/subsystems/elevator/ElevatorSystem.kt → src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt

@@ -1,7 +1,8 @@
-package frc.robot.subsystems.elevator
+package frc.robot.subsystems.superstructure.elevator
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import edu.wpi.first.wpilibj2.command.Command
+import frc.robot.subsystems.superstructure.elevator.ElevatorIO
 
 // 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.

src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOEmpty.kt

@@ -0,0 +1,7 @@
+package frc.robot.subsystems.superstructure.elevator.implementation
+
+import frc.robot.subsystems.superstructure.elevator.ElevatorIO
+
+class ElevatorIOEmpty() : ElevatorIO {
+    override fun setPosition(positionMeters: Double) {}
+}

src/main/java/frc/robot/subsystems/elevator/implementation/ElevatorIOReal.kt → src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOReal.kt

@@ -1,10 +1,10 @@
-package frc.robot.subsystems.elevator.implementation
+package frc.robot.subsystems.superstructure.elevator.implementation
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration
 import com.ctre.phoenix6.hardware.TalonFX
 import com.ctre.phoenix6.controls.MotionMagicVoltage
-import frc.robot.subsystems.elevator.ElevatorConstants
-import frc.robot.subsystems.elevator.ElevatorIO
+import frc.robot.subsystems.superstructure.elevator.ElevatorConstants
+import frc.robot.subsystems.superstructure.elevator.ElevatorIO
 
 class ElevatorIOReal : ElevatorIO {
 

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

@@ -1,11 +1,12 @@
-package frc.robot.subsystems.intake
+package frc.robot.subsystems.superstructure.intake
 
 object IntakeConstants {
     const val coralMotorId = 40
     const val algaeMotorId = 41
     const val coralLaserCANId = 42
     const val algaeLaserCANId = 43
 
+    const val coralIntakeTimeoutSeconds = 0.1
     const val coralScoreTimeoutSeconds = 0.5
     const val algaeScoreTimeoutSeconds = 0.5
 }

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

@@ -1,4 +1,4 @@
-package frc.robot.subsystems.intake
+package frc.robot.subsystems.superstructure.intake
 
 interface IntakeIO {
 //    fun getIntakeEncoder(): Double

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

@@ -0,0 +1,26 @@
+package frc.robot.subsystems.superstructure.intake
+
+// Setting the motor positive will both intake coral from substation and score onto reef
+enum class CoralIntakeState(val speed: Double) {
+    Intaking(0.75),
+    Idle(0.0),
+    Scoring(0.5)
+}
+
+// Positive is scoring and negative is intaking
+enum class AlgaeIntakeState(val speed: Double) {
+    Intaking(-0.8),
+    Idle(0.0),
+    Scoring(0.5),
+    Holding(-0.05)
+}
+
+enum class CoralState {
+    Empty,
+    Stored
+}
+
+enum class AlgaeState {
+    Empty,
+    Stored
+}

src/main/java/frc/robot/subsystems/intake/IntakeSystem.kt → src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt

@@ -1,9 +1,9 @@
-package frc.robot.subsystems.intake
+package frc.robot.subsystems.superstructure.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
+import frc.robot.subsystems.superstructure.intake.*
 
 // 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.
@@ -17,6 +17,7 @@ class IntakeSystem(private val io: IntakeIO) : SubsystemBase() {
     fun coralIntake() = Commands.sequence(
         setCoralIntakeState(CoralIntakeState.Intaking),
         awaitCoralState(CoralState.Stored),
+        Commands.waitSeconds(IntakeConstants.coralIntakeTimeoutSeconds),
         setCoralIntakeState(CoralIntakeState.Idle)
     )
 
@@ -46,6 +47,5 @@ class IntakeSystem(private val io: IntakeIO) : SubsystemBase() {
         RobotState.algaeState = io.getAlgaeState()
     }
 
-    override fun simulationPeriodic() {
-    }
+    override fun simulationPeriodic() {}
 }

src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt

@@ -0,0 +1,14 @@
+package frc.robot.subsystems.superstructure.intake.implementation
+
+import frc.robot.subsystems.superstructure.intake.AlgaeIntakeState
+import frc.robot.subsystems.superstructure.intake.AlgaeState
+import frc.robot.subsystems.superstructure.intake.CoralIntakeState
+import frc.robot.subsystems.superstructure.intake.CoralState
+import frc.robot.subsystems.superstructure.intake.IntakeIO
+
+class IntakeIOEmpty() : IntakeIO {
+    override fun setAlgaeIntakeState(state: AlgaeIntakeState) {}
+    override fun setCoralIntakeState(state: CoralIntakeState) {}
+    override fun getCoralState() = CoralState.Empty
+    override fun getAlgaeState() = AlgaeState.Empty
+}