refactor and instantiate intake class

src/main/kotlin/frc/team449/ControllerBindings.kt

@@ -26,17 +26,17 @@ class ControllerBindings(
     /** Driver: https://docs.google.com/drawings/d/13W3qlIxzIh5MTraZGWON7IqwJvovVr8eNBvjq8_vYZI/edit
      * Operator: https://docs.google.com/drawings/d/1lF4Roftk6932jMCQthgKfoJVPuTVSgnGZSHs5j68uo4/edit
      */
-    SCORE_L1()
-    SCORE_L2()
-    SCORE_L3()
-    SCORE_L4()
+    score_l1()
+    score_l2()
+    score_l3()
+    score_l4()
 
-    PREMOVE_L1()
-    PREMOVE_L2()
-    PREMOVE_L3()
-    PREMOVE_L4()
+    premove_l1()
+    premove_l2()
+    premove_l3()
+    premove_l4()
 
-    STOW()
+    stow()
   }
 
   private fun nonRobotBindings() {
@@ -47,59 +47,59 @@ class ControllerBindings(
     resetGyro()
   }
 
-  private fun STOW() {
+  private fun stow() {
     mechanismController.rightBumper().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.STOW)
     )
   }
 
-  private fun SCORE_L1() {
+  private fun score_l1() {
     driveController.a().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L1)
         .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
     )
   }
 
-  private fun SCORE_L2() {
+  private fun score_l2() {
     driveController.x().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L2)
         .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
     )
   }
 
-  private fun SCORE_L3() {
+  private fun score_l3() {
     driveController.b().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L3)
         .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
     )
   }
 
-  private fun SCORE_L4() {
+  private fun score_l4() {
     driveController.y().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L4)
         .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
     )
   }
 
-  private fun PREMOVE_L1() {
+  private fun premove_l1() {
     mechanismController.a().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L1_PREMOVE)
     )
   }
 
-  private fun PREMOVE_L2() {
+  private fun premove_l2() {
     mechanismController.x().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L2_PREMOVE)
     )
   }
 
-  private fun PREMOVE_L3() {
+  private fun premove_l3() {
     mechanismController.b().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L3_PREMOVE)
     )
   }
 
-  private fun PREMOVE_L4() {
+  private fun premove_l4() {
     mechanismController.y().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L4_PREMOVE)
     )

src/main/kotlin/frc/team449/Robot.kt

@@ -6,17 +6,19 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import frc.team449.subsystems.RobotConstants
 import frc.team449.subsystems.drive.swerve.SwerveDrive
 import frc.team449.subsystems.drive.swerve.SwerveOrthogonalCommand
-import frc.team449.subsystems.elevator.Elevator
-import frc.team449.subsystems.elevator.Elevator.Companion.createElevator
 import frc.team449.subsystems.light.Light.Companion.createLight
-import frc.team449.subsystems.pivot.Pivot
-import frc.team449.subsystems.pivot.Pivot.Companion.createPivot
 import frc.team449.subsystems.superstructure.SuperstructureManager
 import frc.team449.subsystems.superstructure.SuperstructureManager.Companion.createSuperstructureManager
+import frc.team449.subsystems.superstructure.elevator.Elevator
+import frc.team449.subsystems.superstructure.elevator.Elevator.Companion.createElevator
+import frc.team449.subsystems.superstructure.intake.Intake
+import frc.team449.subsystems.superstructure.intake.Intake.Companion.createIntake
+import frc.team449.subsystems.superstructure.pivot.Pivot
+import frc.team449.subsystems.superstructure.pivot.Pivot.Companion.createPivot
+import frc.team449.subsystems.superstructure.wrist.Wrist
+import frc.team449.subsystems.superstructure.wrist.Wrist.Companion.createWrist
 import frc.team449.subsystems.vision.PoseSubsystem
 import frc.team449.subsystems.vision.PoseSubsystem.Companion.createPoseSubsystem
-import frc.team449.subsystems.wrist.Wrist
-import frc.team449.subsystems.wrist.Wrist.Companion.createWrist
 import frc.team449.system.AHRS
 
 @Logged
@@ -47,6 +49,8 @@ class Robot : RobotBase() {
 
   val wrist: Wrist = createWrist()
 
+  val intake: Intake = createIntake()
+
   val superstructureManager: SuperstructureManager = createSuperstructureManager(this)
 
   val light = createLight()

src/main/kotlin/frc/team449/RobotLoop.kt

@@ -18,9 +18,9 @@ import frc.team449.commands.light.BreatheHue
 import frc.team449.commands.light.Rainbow
 import frc.team449.subsystems.FieldConstants
 import frc.team449.subsystems.drive.swerve.SwerveSim
-import frc.team449.subsystems.elevator.ElevatorConstants
-import frc.team449.subsystems.elevator.ElevatorFeedForward.Companion.createElevatorFeedForward
-import frc.team449.subsystems.pivot.PivotFeedForward.Companion.createPivotFeedForward
+import frc.team449.subsystems.superstructure.elevator.ElevatorConstants
+import frc.team449.subsystems.superstructure.elevator.ElevatorFeedForward.Companion.createElevatorFeedForward
+import frc.team449.subsystems.superstructure.pivot.PivotFeedForward.Companion.createPivotFeedForward
 import frc.team449.subsystems.vision.VisionConstants
 import org.littletonrobotics.urcl.URCL
 import kotlin.jvm.optionals.getOrDefault

src/main/kotlin/frc/team449/commands/driveAlign/SimpleReefAlign.kt

@@ -24,10 +24,10 @@ import kotlin.math.min
 class SimpleReefAlign(
   private val drive: SwerveDrive,
   private val poseSubsystem: PoseSubsystem,
-  translationSpeedLim: Double = 0.8 * RobotConstants.MAX_LINEAR_SPEED,
-  translationAccelLim: Double = 7.0,
+  translationSpeedLim: Double = 0.825 * RobotConstants.MAX_LINEAR_SPEED,
+  translationAccelLim: Double = 5.75,
   headingSpeedLim: Double = PI,
-  headingAccelLim: Double = 5 * PI,
+  headingAccelLim: Double = 6 * PI,
   translationPID: Triple<Double, Double, Double> = Triple(7.5, 0.0, 0.0),
   headingPID: Triple<Double, Double, Double> = Triple(7.5, 0.0, 0.0),
   private val translationTolerance: Double = Units.inchesToMeters(0.75),

src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt

@@ -7,9 +7,9 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand
 import edu.wpi.first.wpilibj2.command.InstantCommand
 import frc.team449.Robot
 import frc.team449.subsystems.drive.swerve.SwerveDrive
-import frc.team449.subsystems.elevator.Elevator
-import frc.team449.subsystems.pivot.Pivot
-import frc.team449.subsystems.wrist.Wrist
+import frc.team449.subsystems.superstructure.elevator.Elevator
+import frc.team449.subsystems.superstructure.pivot.Pivot
+import frc.team449.subsystems.superstructure.wrist.Wrist
 
 class SuperstructureManager(
   private val elevator: Elevator,

src/main/kotlin/frc/team449/subsystems/elevator/Elevator.kt → src/main/kotlin/frc/team449/subsystems/superstructure/elevator/Elevator.kt

@@ -1,4 +1,4 @@
-package frc.team449.subsystems.elevator
+package frc.team449.subsystems.superstructure.elevator
 
 import com.ctre.phoenix6.BaseStatusSignal
 import com.ctre.phoenix6.configs.TalonFXConfiguration
@@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import frc.team449.subsystems.superstructure.SuperstructureGoal
-import frc.team449.subsystems.wrist.WristConstants
+import frc.team449.subsystems.superstructure.wrist.WristConstants
 import frc.team449.system.motor.KrakenDogLog
 import java.util.function.Supplier
 import kotlin.math.abs

src/main/kotlin/frc/team449/subsystems/elevator/ElevatorConstants.kt → src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorConstants.kt

@@ -1,4 +1,4 @@
-package frc.team449.subsystems.elevator
+package frc.team449.subsystems.superstructure.elevator
 
 import com.ctre.phoenix6.signals.InvertedValue
 import com.ctre.phoenix6.signals.NeutralModeValue

src/main/kotlin/frc/team449/subsystems/elevator/ElevatorFeedForward.kt → src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorFeedForward.kt

@@ -1,7 +1,7 @@
-package frc.team449.subsystems.elevator
+package frc.team449.subsystems.superstructure.elevator
 
 import edu.wpi.first.math.controller.ElevatorFeedforward
-import frc.team449.subsystems.pivot.Pivot
+import frc.team449.subsystems.superstructure.pivot.Pivot
 import kotlin.math.pow
 import kotlin.math.sign
 import kotlin.math.sin

src/main/kotlin/frc/team449/subsystems/elevator/ElevatorSim.kt → src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorSim.kt

@@ -1,4 +1,4 @@
-package frc.team449.subsystems.elevator
+package frc.team449.subsystems.superstructure.elevator
 
 import com.ctre.phoenix6.hardware.TalonFX
 import com.ctre.phoenix6.sim.TalonFXSimState

src/main/kotlin/frc/team449/subsystems/elevator/TiltedElevatorSim.kt → src/main/kotlin/frc/team449/subsystems/superstructure/elevator/TiltedElevatorSim.kt

@@ -1,4 +1,4 @@
-package frc.team449.subsystems.elevator
+package frc.team449.subsystems.superstructure.elevator
 
 import edu.wpi.first.math.Matrix
 import edu.wpi.first.math.VecBuilder

src/main/kotlin/frc/team449/subsystems/superstructure/intake/Intake.kt

@@ -0,0 +1,72 @@
+package frc.team449.subsystems.superstructure.intake
+
+import com.ctre.phoenix6.hardware.TalonFX
+import edu.wpi.first.units.Units.Seconds
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+import frc.team449.system.motor.createKraken
+
+// TODO(the entire class bru)
+class Intake(
+  private val frontMotor: TalonFX,
+  private val backMotor: TalonFX
+) : SubsystemBase() {
+
+  fun intakeCoral(): Command {
+    return runOnce {
+      frontMotor.setVoltage(IntakeConstants.CORAL_INTAKE_VOLTAGE)
+      backMotor.setVoltage(-IntakeConstants.CORAL_INTAKE_VOLTAGE)
+    }
+  }
+
+  fun outtakeCoral(): Command {
+    return runOnce {
+      frontMotor.setVoltage(IntakeConstants.CORAL_OUTTAKE_VOLTAGE)
+      backMotor.setVoltage(-IntakeConstants.CORAL_OUTTAKE_VOLTAGE)
+    }
+  }
+
+  fun intakeAlgae(): Command {
+    return runOnce {
+      frontMotor.setVoltage(-IntakeConstants.ALGAE_INTAKE_VOLTAGE)
+      backMotor.setVoltage(IntakeConstants.ALGAE_INTAKE_VOLTAGE)
+    }
+  }
+
+  fun outtakeAlgae(): Command {
+    return runOnce {
+      frontMotor.setVoltage(-IntakeConstants.ALGAE_OUTTAKE_VOLTAGE)
+      backMotor.setVoltage(IntakeConstants.ALGAE_OUTTAKE_VOLTAGE)
+    }
+  }
+
+  fun stop(): Command {
+    return runOnce {
+      frontMotor.stopMotor()
+      backMotor.stopMotor()
+    }
+  }
+
+  companion object {
+    fun createIntake(): Intake {
+      val frontMotor = createKraken(
+        id = IntakeConstants.FRONT_MOTOR_ID,
+        inverted = IntakeConstants.FRONT_MOTOR_INVERTED,
+        statorCurrentLimit = IntakeConstants.STATOR_LIMIT,
+        burstCurrentLimit = IntakeConstants.SUPPLY_LIMIT,
+        burstTimeLimit = Seconds.of(0.0),
+        updateFrequency = IntakeConstants.VALUE_UPDATE_FREQ
+      )
+
+      val backMotor = createKraken(
+        id = IntakeConstants.BACK_MOTOR_ID,
+        inverted = IntakeConstants.BACK_MOTOR_INVERTED,
+        statorCurrentLimit = IntakeConstants.STATOR_LIMIT,
+        burstCurrentLimit = IntakeConstants.SUPPLY_LIMIT,
+        burstTimeLimit = Seconds.of(0.0),
+        updateFrequency = IntakeConstants.VALUE_UPDATE_FREQ
+      )
+      return Intake(frontMotor, backMotor)
+    }
+  }
+}

src/main/kotlin/frc/team449/subsystems/superstructure/intake/IntakeConstants.kt

@@ -0,0 +1,22 @@
+package frc.team449.subsystems.superstructure.intake
+
+import edu.wpi.first.units.Units.Amps
+import edu.wpi.first.units.Units.Hertz
+
+object IntakeConstants {
+  const val FRONT_MOTOR_ID = 10 // TODO(Change motor ID.)
+  const val BACK_MOTOR_ID = 11 // TODO(Change motor ID.)
+
+  val STATOR_LIMIT = Amps.of(60.0)
+  val SUPPLY_LIMIT = Amps.of(30.0)
+
+  val VALUE_UPDATE_FREQ = Hertz.of(10.0)
+
+  const val FRONT_MOTOR_INVERTED = false
+  const val BACK_MOTOR_INVERTED = true
+
+  const val CORAL_INTAKE_VOLTAGE = 3.0
+  const val CORAL_OUTTAKE_VOLTAGE = -2.0
+  const val ALGAE_INTAKE_VOLTAGE = 7.0
+  const val ALGAE_OUTTAKE_VOLTAGE = -10.0
+}

src/main/kotlin/frc/team449/subsystems/pivot/LengthPivotSim.kt → src/main/kotlin/frc/team449/subsystems/superstructure/pivot/LengthPivotSim.kt

@@ -1,4 +1,4 @@
-package frc.team449.subsystems.pivot
+package frc.team449.subsystems.superstructure.pivot
 
 import edu.wpi.first.math.MathUtil
 import edu.wpi.first.math.Matrix

src/main/kotlin/frc/team449/subsystems/pivot/Pivot.kt → src/main/kotlin/frc/team449/subsystems/superstructure/pivot/Pivot.kt

@@ -1,4 +1,4 @@
-package frc.team449.subsystems.pivot
+package frc.team449.subsystems.superstructure.pivot
 
 import com.ctre.phoenix6.BaseStatusSignal
 import com.ctre.phoenix6.configs.TalonFXConfiguration

src/main/kotlin/frc/team449/subsystems/pivot/PivotConstants.kt → src/main/kotlin/frc/team449/subsystems/superstructure/pivot/PivotConstants.kt

@@ -1,4 +1,4 @@
-package frc.team449.subsystems.pivot
+package frc.team449.subsystems.superstructure.pivot
 
 import com.ctre.phoenix6.signals.InvertedValue
 import com.ctre.phoenix6.signals.NeutralModeValue

src/main/kotlin/frc/team449/subsystems/pivot/PivotFeedForward.kt → src/main/kotlin/frc/team449/subsystems/superstructure/pivot/PivotFeedForward.kt

@@ -1,8 +1,8 @@
-package frc.team449.subsystems.pivot
+package frc.team449.subsystems.superstructure.pivot
 
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap
-import frc.team449.subsystems.elevator.Elevator
-import frc.team449.subsystems.elevator.ElevatorConstants
+import frc.team449.subsystems.superstructure.elevator.Elevator
+import frc.team449.subsystems.superstructure.elevator.ElevatorConstants
 import kotlin.math.cos
 import kotlin.math.sign