create rest of subsystems (state space flywheel and pivot)

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

@@ -37,7 +37,7 @@ class RobotLoop : TimedRobot(), Logged {
   @Log.NT
   private val field = robot.field
   private var autoCommand: Command? = null
-  private var routineMap = hashMapOf<String,Command>()
+  private var routineMap = hashMapOf<String, Command>()
   private val controllerBinder = ControllerBindings(robot.driveController, robot.mechController, robot)
 
   override fun robotInit() {

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

@@ -7,7 +7,7 @@ import frc.team449.RobotBase
 import frc.team449.control.holonomic.SwerveDrive
 import frc.team449.control.holonomic.SwerveOrthogonalCommand
 import frc.team449.robot2024.constants.RobotConstants
-import frc.team449.robot2024.subsystems.ProtoUndertaker.Companion.createProtoUndertaker
+import frc.team449.robot2024.subsystems.Undertaker.Companion.createProtoUndertaker
 import frc.team449.system.AHRS
 import frc.team449.system.light.Light
 import monologue.Annotations.Log

src/main/kotlin/frc/team449/robot2024/constants/field/FieldConstants.kt

@@ -1,6 +1,11 @@
 package frc.team449.robot2024.constants.field
 
+import edu.wpi.first.math.geometry.Translation2d
+
 object FieldConstants {
-  val fieldLength = 16.54175
-  val fieldWidth = 8.21055
+  const val fieldLength = 16.54175
+  const val fieldWidth = 8.21055
+
+  /** Find these constants in meters for the blue alliance */
+  val SUBWOOFER_POSE = Translation2d(1.0, 4.0)
 }

src/main/kotlin/frc/team449/robot2024/constants/subsystem/ClimberConstants.kt

@@ -16,4 +16,4 @@ object ClimberConstants {
   const val KP = 1.0
   const val KI = 0.0
   const val KD = 0.0
-}
+}

src/main/kotlin/frc/team449/robot2024/constants/subsystem/PivotConstants.kt

@@ -0,0 +1,37 @@
+package frc.team449.robot2024.constants.subsystem
+
+import edu.wpi.first.math.util.Units
+import kotlin.math.PI
+
+object PivotConstants {
+
+  const val MOTOR_ID = 46
+  const val INVERTED = false
+  const val CURRENT_LIM = 40
+  const val GEARING = 1.0 / 75.0
+  const val UPR = 2 * PI
+  const val FOLLOWER_ID = 47
+  const val FOLLOWER_INVERTED = true
+
+  const val NUM_MOTORS = 2
+  const val MOMENT_OF_INERTIA = 2.0
+
+  /** Deviations for Kalman filter in units of radians or radians / seconds */
+  val MODEL_POS_DEVIATION = Units.degreesToRadians(10.0)
+  val MODEL_VEL_DEVIATION = Units.degreesToRadians(20.0)
+  val ENCODER_POS_DEVIATION = Units.degreesToRadians(0.25)
+
+  /** LQR Position and Velocity tolerances */
+  val POS_TOLERANCE = Units.degreesToRadians(0.25)
+  val VEL_TOLERANCE = Units.degreesToRadians(10.0)
+  val CONTROL_EFFORT_VOLTS = 12.0
+
+  val MAX_VOLTAGE = 12.0
+
+  /** Profile Constraints */
+  val MAX_VELOCITY = Units.degreesToRadians(100.0)
+  val MAX_ACCEL = Units.degreesToRadians(75.0)
+
+  val AMP_ANGLE = Units.degreesToRadians(75.0)
+  val SUBWOOFER_ANGLE = Units.degreesToRadians(30.0)
+}

src/main/kotlin/frc/team449/robot2024/constants/subsystem/ShooterConstants.kt

@@ -0,0 +1,64 @@
+package frc.team449.robot2024.constants.subsystem
+
+import edu.wpi.first.math.InterpolatingMatrixTreeMap
+import edu.wpi.first.math.MatBuilder
+import edu.wpi.first.math.Nat
+import edu.wpi.first.math.numbers.N1
+import edu.wpi.first.math.numbers.N3
+
+object ShooterConstants {
+  const val RIGHT_MOTOR_ID = 41
+  const val RIGHT_MOTOR_INVERTED = false
+  const val LEFT_MOTOR_ID = 42
+  const val LEFT_MOTOR_INVERTED = false
+  const val CURRENT_LIMIT = 40
+  const val UPR = 1.0
+
+  const val SUBWOOFER_LEFT_SPEED = 100.0
+  const val SUBWOOFER_RIGHT_SPEED = 125.0
+
+  val SHOOTING_MAP = InterpolatingMatrixTreeMap<Double, N3, N1>()
+
+  const val LEFT_KS = 0.0
+  const val RIGHT_KS = 0.0
+
+  const val AMP_SCORE_VOLTAGE = 4.0
+  const val DURING_INTAKE_VOLTAGE = -3.0
+
+  /** In meters from the ground */
+  const val SHOOTER_HEIGHT = 0.25
+
+  /** These constants are PER SIDE of the shooter */
+  const val MOMENT_OF_INERTIA = 2.50
+  const val NUM_MOTORS = 1
+  const val GEARING = 1.0 / 1.0
+
+  const val MODEL_VEL_STDDEV = 3.0
+  const val ENCODER_VEL_STDDEV = 0.01
+  const val LQR_VEL_TOL = 10.0 / 60.0
+  const val LQR_MAX_VOLTS = 12.0
+  const val MAX_VOLTAGE = 12.0
+
+  init {
+    /**
+     * Fill with values of optimized left/right and pivot angles
+     *  for a given distance to the Speaker
+     *
+     * It may be better to mathematically calculate pivot angle,
+     *  this is something to test
+     *
+     * Data is entered as following:
+     *  Right shooter speed, left shooter speed, pivot angle
+     */
+    SHOOTING_MAP.put(
+      0.0,
+      MatBuilder.fill(
+        Nat.N3(),
+        Nat.N1(),
+        0.0,
+        0.0,
+        0.0
+      )
+    )
+  }
+}

src/main/kotlin/frc/team449/robot2024/constants/subsystem/ProtoIntakeConstants.kt → src/main/kotlin/frc/team449/robot2024/constants/subsystem/UndertakerConstants.kt

@@ -1,6 +1,6 @@
 package frc.team449.robot2024.constants.subsystem
 
-object ProtoIntakeConstants {
+object UndertakerConstants {
   const val MOTOR_ID = 7
   const val FOLLOLWER_ID = 55
   const val FOLLOWER_INV = false

src/main/kotlin/frc/team449/robot2024/subsystems/Climber.kt

@@ -18,26 +18,26 @@ class Climber(
   private val controller: PIDController
 ) : SubsystemBase() {
 
-    fun levelClimb() : Command {
-      return PIDCommand(
-        controller,
-        { robot.drive.roll.degrees },
-        { 0.0 },
-        { value ->
-          rightMotor.setVoltage(ClimberConstants.DEFAULT_PID_RETRACT + value)
-          leftMotor.setVoltage(ClimberConstants.DEFAULT_PID_RETRACT - value)
-        },
-        this
-      )
-    }
-
-    fun extend(): Command {
-      return this.runOnce {
-        rightMotor.setVoltage(ClimberConstants.EXTEND_VOLTAGE)
-        leftMotor.setVoltage(ClimberConstants.EXTEND_VOLTAGE)
-      }
+  fun levelClimb(): Command {
+    return PIDCommand(
+      controller,
+      { robot.drive.roll.degrees },
+      { 0.0 },
+      { value ->
+        rightMotor.setVoltage(ClimberConstants.DEFAULT_PID_RETRACT + value)
+        leftMotor.setVoltage(ClimberConstants.DEFAULT_PID_RETRACT - value)
+      },
+      this
+    )
+  }
 
+  fun extend(): Command {
+    return this.runOnce {
+      rightMotor.setVoltage(ClimberConstants.EXTEND_VOLTAGE)
+      leftMotor.setVoltage(ClimberConstants.EXTEND_VOLTAGE)
     }
+  }
+
   fun retract(): Command {
     return this.runOnce {
       rightMotor.setVoltage(ClimberConstants.RETRACT_VOLTAGE)
@@ -82,7 +82,6 @@ class Climber(
         currentLimit = ClimberConstants.CURRENT_LIM,
       )
 
-
       val controller = PIDController(ClimberConstants.KP, ClimberConstants.KI, ClimberConstants.KD)
 
       return Climber(robot, rightMotor, leftMotor, controller)

src/main/kotlin/frc/team449/robot2024/subsystems/Pivot.kt

@@ -0,0 +1,161 @@
+package frc.team449.robot2024.subsystems
+
+import edu.wpi.first.math.Nat
+import edu.wpi.first.math.VecBuilder
+import edu.wpi.first.math.controller.LinearQuadraticRegulator
+import edu.wpi.first.math.estimator.KalmanFilter
+import edu.wpi.first.math.numbers.N1
+import edu.wpi.first.math.numbers.N2
+import edu.wpi.first.math.system.LinearSystemLoop
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.math.system.plant.LinearSystemId
+import edu.wpi.first.math.trajectory.TrapezoidProfile
+import edu.wpi.first.util.sendable.SendableBuilder
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+import frc.team449.robot2024.Robot
+import frc.team449.robot2024.constants.MotorConstants
+import frc.team449.robot2024.constants.RobotConstants
+import frc.team449.robot2024.constants.field.FieldConstants
+import frc.team449.robot2024.constants.subsystem.PivotConstants
+import frc.team449.robot2024.constants.subsystem.ShooterConstants
+import frc.team449.system.encoder.NEOEncoder
+import frc.team449.system.motor.WrappedMotor
+import frc.team449.system.motor.createSparkMax
+import java.util.function.Supplier
+
+open class Pivot(
+  private val motor: WrappedMotor,
+  private val loop: LinearSystemLoop<N2, N1, N1>,
+  private val profile: TrapezoidProfile,
+  private val robot: Robot
+) : SubsystemBase() {
+
+  open val positionSupplier: Supplier<Double> =
+    Supplier { motor.position }
+
+  private var lastProfileReference = TrapezoidProfile.State(motor.position, motor.velocity)
+
+  init {
+    loop.reset(VecBuilder.fill(motor.position, motor.velocity))
+    this.defaultCommand = hold()
+  }
+
+  fun hold(): Command {
+    return this.run {
+      loop.setNextR(lastProfileReference.position, lastProfileReference.velocity)
+      loop.correct(VecBuilder.fill(positionSupplier.get()))
+      loop.predict(RobotConstants.LOOP_TIME)
+
+      motor.setVoltage(loop.getU(0))
+    }
+  }
+
+  fun moveAmp(): Command {
+    return this.run {
+      moveToAngle(PivotConstants.AMP_ANGLE)
+    }
+  }
+
+  fun moveSubwoofer(): Command {
+    return this.run {
+      moveToAngle(PivotConstants.SUBWOOFER_ANGLE)
+    }
+  }
+
+  fun pivotShootAnywhere(): Command {
+    return this.run {
+      val distance = FieldConstants.SUBWOOFER_POSE.getDistance(robot.drive.pose.translation)
+      val goal = ShooterConstants.SHOOTING_MAP.get(distance).get(2, 0)
+
+      loop.setNextR(goal, 0.0)
+      loop.correct(VecBuilder.fill(motor.position))
+      loop.predict(RobotConstants.LOOP_TIME)
+
+      motor.setVoltage(loop.getU(0))
+    }
+  }
+
+  private fun moveToAngle(goal: Double) {
+    lastProfileReference = profile.calculate(RobotConstants.LOOP_TIME, lastProfileReference, TrapezoidProfile.State(goal, 0.0))
+
+    loop.setNextR(lastProfileReference.position, lastProfileReference.velocity)
+    loop.correct(VecBuilder.fill(motor.position))
+    loop.predict(RobotConstants.LOOP_TIME)
+
+    motor.setVoltage(loop.getU(0))
+  }
+
+  fun stop(): Command {
+    return this.runOnce {
+      motor.stopMotor()
+    }
+  }
+
+  override fun initSendable(builder: SendableBuilder) {
+    builder.publishConstString("1.0", "Motor Voltages")
+    builder.addDoubleProperty("1.1 Last Voltage", { motor.lastVoltage }, null)
+  }
+
+  companion object {
+    fun createShooter(robot: Robot): Pivot {
+      val motor = createSparkMax(
+        "Shooter Right Motor",
+        PivotConstants.MOTOR_ID,
+        NEOEncoder.creator(
+          PivotConstants.GEARING,
+          PivotConstants.UPR
+        ),
+        inverted = PivotConstants.INVERTED,
+        currentLimit = PivotConstants.CURRENT_LIM,
+        slaveSparks = mapOf(Pair(PivotConstants.FOLLOWER_ID, PivotConstants.FOLLOWER_INVERTED))
+      )
+
+      val plant = LinearSystemId.createSingleJointedArmSystem(
+        DCMotor(
+          MotorConstants.NOMINAL_VOLTAGE,
+          MotorConstants.STALL_TORQUE,
+          MotorConstants.STALL_CURRENT,
+          MotorConstants.FREE_CURRENT,
+          MotorConstants.FREE_SPEED,
+          PivotConstants.NUM_MOTORS
+        ),
+        PivotConstants.MOMENT_OF_INERTIA,
+        1 / PivotConstants.GEARING
+      )
+
+      val observer = KalmanFilter(
+        Nat.N2(),
+        Nat.N1(),
+        plant,
+        VecBuilder.fill(PivotConstants.MODEL_POS_DEVIATION, PivotConstants.MODEL_VEL_DEVIATION),
+        VecBuilder.fill(PivotConstants.ENCODER_POS_DEVIATION),
+        RobotConstants.LOOP_TIME
+      )
+
+      val controller = LinearQuadraticRegulator(
+        plant,
+        VecBuilder.fill(PivotConstants.POS_TOLERANCE, PivotConstants.VEL_TOLERANCE),
+        VecBuilder.fill(PivotConstants.CONTROL_EFFORT_VOLTS),
+        RobotConstants.LOOP_TIME
+      )
+
+      val loop = LinearSystemLoop(
+        plant,
+        controller,
+        observer,
+        PivotConstants.MAX_VOLTAGE,
+        RobotConstants.LOOP_TIME
+      )
+
+      val profile = TrapezoidProfile(
+        TrapezoidProfile.Constraints(
+          PivotConstants.MAX_VELOCITY,
+          PivotConstants.MAX_ACCEL
+        )
+      )
+
+      return Pivot(motor, loop, profile, robot)
+    }
+  }
+}