Began intake file

src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt

@@ -18,4 +18,12 @@ object IntakeConstants {
   const val ROLLER_GEAR_RATIO = 36.0 / 18.0
   const val ARM_GEAR_RATIO = ((60.0 / 12.0) * (80.0 / 18.0) * (32.0 / 16.0))
   const val ARM_ENCODER_GEAR_RATIO = 36.0 / 18.0
+
+  // TODO: Enter values
+  const val ENABLE_ARM = 0.0
+  const val ENABLE_ROTATION = 0.0
+  val STOWED_UP_ANGLE = 0.0.degrees
+  val STOWED_DOWN_ANGLE = 0.0.degrees
+  val INTAKE_VOLTAGE = 0.0.volts
+  val NEUTRAL_VOLTAGE = 0.0.volts
 }

src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt

@@ -1,3 +1,77 @@
 package com.team4099.robot2023.subsystems.intake
 
-class Intake
+import com.team4099.lib.hal.Clock
+import com.team4099.lib.logging.LoggedTunableNumber
+import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.IntakeConstants
+import com.team4099.robot2023.subsystems.superstructure.Request
+import edu.wpi.first.wpilibj.RobotBase
+import org.littletonrobotics.junction.Logger
+import org.team4099.lib.controller.ArmFeedforward
+import org.team4099.lib.controller.TrapezoidProfile
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.Radian
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.inVoltsPerDegree
+import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond
+import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds
+import org.team4099.lib.units.derived.perDegree
+import org.team4099.lib.units.derived.perDegreePerSecond
+import org.team4099.lib.units.derived.perDegreeSeconds
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inDegreesPerSecond
+import org.team4099.lib.units.perSecond
+
+class Intake(private val io: IntakeIO) {
+    val inputs = IntakeIO.IntakeIOInputs()
+
+    lateinit var armFeedforward: ArmFeedforward
+
+    private val kP = LoggedTunableValue("Intake/kP", Pair({it.inVoltsPerDegree}, {it.volts.perDegree}))
+    private val kI = LoggedTunableValue("Intake/kI", Pair({it.inVoltsPerDegreeSeconds}, {it.volts.perDegreeSeconds}))
+    private val kD = LoggedTunableValue("Intake/kD", Pair({it.inVoltsPerDegreePerSecond}, {it.volts.perDegreePerSecond}))
+
+    object TunableGroundIntakeStates {
+        val enableArm =
+            LoggedTunableNumber(
+                "Intake/enableArmIntake",
+                IntakeConstants.ENABLE_ARM
+            )
+        val enableRotation =
+            LoggedTunableNumber(
+                "Intake/enableRotationIntake",
+                IntakeConstants.ENABLE_ROTATION
+            )
+        val stowedUpAngle =
+            LoggedTunableValue(
+                "Intake/stowedUpAngle",
+                IntakeConstants.STOWED_UP_ANGLE,
+                Pair({ it.inDegrees }, { it.degrees })
+            )
+        val stowedDownAngle =
+            LoggedTunableValue(
+                "Intake/stowedDownAngle",
+                IntakeConstants.STOWED_DOWN_ANGLE,
+                Pair({ it.inDegrees }, { it.degrees })
+            )
+        val intakeVoltage =
+            LoggedTunableValue(
+                "Intake/intakeVoltage",
+                IntakeConstants.INTAKE_VOLTAGE,
+                Pair({ it.inVolts }, { it.volts })
+            )
+        val neutralVoltage =
+            LoggedTunableValue(
+                "Intake/neutralVoltage",
+                IntakeConstants.NEUTRAL_VOLTAGE,
+                Pair({ it.inVolts }, { it.volts })
+            )
+    }
+}