remove velocity limit on 4 piece, put back quad encoder, coast instea…

…d of rampStop by default, fix ir logic

simgui.json

@@ -355,7 +355,7 @@
             0.0,
             0.8500000238418579
           ],
-          "height": 647,
+          "height": 599,
           "series": [
             {
               "color": [

src/main/kotlin/frc/team449/robot2024/auto/AutoUtil.kt

@@ -13,28 +13,29 @@ import kotlin.math.PI
 object AutoUtil {
 
   fun autoIntake(robot: Robot): Command {
-    return SequentialCommandGroup(
-      robot.undertaker.intake(),
-      robot.feeder.intake(),
-      WaitUntilCommand { !robot.infrared.get() },
-      robot.undertaker.stop(),
-      robot.feeder.outtake(),
-      robot.shooter.duringIntake(),
-      WaitUntilCommand { robot.infrared.get() },
-      robot.feeder.stop(),
-      robot.shooter.coast()
+    return ParallelDeadlineGroup(
+      robot.shooter.shootSubwoofer(),
+      SequentialCommandGroup(
+        robot.undertaker.intake(),
+        robot.feeder.autoIntake(),
+        WaitUntilCommand { !robot.infrared.get() },
+        robot.undertaker.stop(),
+        robot.feeder.outtake(),
+        WaitUntilCommand { robot.infrared.get() },
+        robot.feeder.stop(),
+      )
     )
   }
 
   fun autoShoot(robot: Robot): Command {
-    return ParallelCommandGroup(
-      robot.shooter.shootSubwoofer(),
+    return ParallelDeadlineGroup(
       SequentialCommandGroup(
         WaitUntilCommand { robot.shooter.atSetpoint() },
         robot.feeder.intake(),
         robot.undertaker.intake(),
         WaitCommand(AutoConstants.SHOOT_INTAKE_TIME)
-      )
+      ),
+      robot.shooter.shootSubwoofer()
     )
   }
   fun transformForPos2(pathGroup: MutableList<ChoreoTrajectory>): MutableList<ChoreoTrajectory> {

src/main/kotlin/frc/team449/robot2024/auto/routines/Experimental4Piece.kt

@@ -10,8 +10,6 @@ class Experimental4Piece(
   robot: Robot,
   isRed: Boolean
 ) : ChoreoRoutineStructure {
-
-  /** TODO STILL AUTO SUBSYSTEM CMD PARALLEL */
   override val routine =
     ChoreoRoutine(
       drive = robot.drive,
@@ -27,7 +25,7 @@ class Experimental4Piece(
         2 to AutoUtil.autoShoot(robot),
         3 to AutoUtil.autoShoot(robot)
       ),
-      debug = true
+      debug = false
     )
 
   override val trajectory: MutableList<ChoreoTrajectory> =

src/main/kotlin/frc/team449/robot2024/constants/auto/AutoConstants.kt

@@ -4,10 +4,10 @@ object AutoConstants {
   /** PID gains */
   const val DEFAULT_X_KP = 1.875
   const val DEFAULT_Y_KP = 1.875
-  const val DEFAULT_ROTATION_KP = 2.0
+  const val DEFAULT_ROTATION_KP = 2.25
 
   const val ORBIT_KP = 2.0
 
-  const val SHOOT_INTAKE_TIME = 0.5
+  const val SHOOT_INTAKE_TIME = 0.075
   const val FEEDER_REVERSE_TIME = 0.35
 }

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

@@ -8,7 +8,8 @@ object FeederConstants {
   const val GEARING = 1.0 / 9.0
 
   const val INTAKE_VOLTAGE = 8.0
-  const val REVERSE_VOLTAGE = -2.0
+  const val AUTO_INTAKE_VOLTAGE = 2.0
+  const val REVERSE_VOLTAGE = -3.5
 
   const val BRAKE_MODE = false
 }

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

@@ -20,16 +20,16 @@ object ShooterConstants {
   val SUBWOOFER_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(5460.0)
   val SUBWOOFER_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(5460.0)
 
-  val BRAKE_RATE_LIMIT = Units.rotationsPerMinuteToRadiansPerSecond(3500.0)
+  val BRAKE_RATE_LIMIT = Units.rotationsPerMinuteToRadiansPerSecond(2850.0)
 
   val SHOOTING_MAP = InterpolatingMatrixTreeMap<Double, N3, N1>()
 
   const val LEFT_KS = 0.0
   const val RIGHT_KS = 0.0
-  const val LEFT_KV = 0.02018878773
-  const val RIGHT_KV = 0.02018878773
-  const val LEFT_KA = 0.032004
-  const val RIGHT_KA = 0.032004
+  const val LEFT_KV = 0.00965199999
+  const val RIGHT_KV = 0.00965199999
+  const val LEFT_KA = 0.05638799999
+  const val RIGHT_KA = 0.05638799999
 
   const val AMP_SCORE_VOLTAGE = 5.460
   const val DURING_INTAKE_VOLTAGE = -1.0
@@ -42,20 +42,23 @@ object ShooterConstants {
 
   const val MODEL_VEL_STDDEV = 3.0
   const val ENCODER_VEL_STDDEV = 0.5
-  const val LQR_VEL_TOL = 30.0
+  const val LQR_VEL_TOL = 45.0
   const val LQR_MAX_VOLTS = 12.0
   const val MAX_VOLTAGE = 12.0
 
+  const val MIN_RAMP_VEL = 50.0
+
   /** Encoder stuff */
   const val LEFT_CHANNEL_A = 1
   const val LEFT_CHANNEL_B = 2
-  const val RIGHT_CHANNEL_A = 3
-  const val RIGHT_CHANNEL_B = 4
+  const val RIGHT_CHANNEL_A = 4
+  const val RIGHT_CHANNEL_B = 3
   const val CPR = 2048
   const val UPR = 2 * PI
   const val GEARING = 1.0 / 1.0
   const val LEFT_ENCODER_INVERTED = false
   const val RIGHT_ENCODER_INVERTED = false
+  const val SAMPLES_TO_AVERAGE = 127
 
   init {
     /**

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

@@ -45,7 +45,7 @@ class ControllerBindings(
   private fun stopAll(): Command {
     return ParallelCommandGroup(
       robot.undertaker.stop(),
-      robot.shooter.rampStop(),
+      robot.shooter.coast(),
       robot.feeder.stop()
     )
   }
@@ -123,7 +123,8 @@ class ControllerBindings(
       )
     ).onFalse(
       SequentialCommandGroup(
-        stopAll(),
+        robot.feeder.stop(),
+        robot.undertaker.stop(),
         robot.shooter.rampStop()
       )
     )
@@ -179,17 +180,18 @@ class ControllerBindings(
   }
 
   private fun outtakeToNotePosition(): Command {
-    return ConditionalCommand(
+    val cmd = ConditionalCommand(
       SequentialCommandGroup(
         robot.undertaker.stop(),
         robot.feeder.outtake(),
-        robot.shooter.duringIntake(),
         WaitUntilCommand { robot.infrared.get() },
-        robot.feeder.stop(),
-        robot.shooter.rampStop()
+        robot.feeder.stop()
       ),
-      InstantCommand()
+      stopAll()
     ) { !robot.infrared.get() }
+
+    cmd.name = "outtake to note pos"
+    return cmd
   }
 
   private fun nonRobotBindings() {

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

@@ -18,6 +18,12 @@ class Feeder(
     }
   }
 
+  fun autoIntake(): Command {
+    return this.runOnce {
+      motor.setVoltage(FeederConstants.AUTO_INTAKE_VOLTAGE)
+    }
+  }
+
   fun outtake(): Command {
     return this.runOnce {
       motor.setVoltage(FeederConstants.REVERSE_VOLTAGE)

src/main/kotlin/frc/team449/robot2024/subsystems/shooter/Shooter.kt

@@ -10,19 +10,17 @@ import edu.wpi.first.math.system.LinearSystemLoop
 import edu.wpi.first.math.system.plant.LinearSystemId
 import edu.wpi.first.util.sendable.SendableBuilder
 import edu.wpi.first.wpilibj.DriverStation
+import edu.wpi.first.wpilibj.Encoder
 import edu.wpi.first.wpilibj.RobotBase
-import edu.wpi.first.wpilibj2.command.Command
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
-import edu.wpi.first.wpilibj2.command.SubsystemBase
+import edu.wpi.first.wpilibj2.command.*
 import frc.team449.robot2024.Robot
 import frc.team449.robot2024.constants.RobotConstants
 import frc.team449.robot2024.constants.field.FieldConstants
 import frc.team449.robot2024.constants.subsystem.ShooterConstants
-import frc.team449.system.encoder.NEOEncoder
+import frc.team449.system.encoder.QuadEncoder
 import frc.team449.system.motor.WrappedMotor
 import frc.team449.system.motor.createSparkMax
 import java.util.function.Supplier
-import kotlin.math.PI
 import kotlin.math.abs
 
 open class Shooter(
@@ -137,7 +135,7 @@ open class Shooter(
   }
 
   fun coast(): Command {
-    val cmd = this.run {
+    val cmd = this.runOnce {
       desiredVels = Pair(0.0, 0.0)
       leftMotor.setVoltage(0.0)
       rightMotor.setVoltage(0.0)
@@ -152,19 +150,33 @@ open class Shooter(
       shootPiece(0.0, 0.0)
     }
     cmd.name = "force stop"
-    return cmd
+    return ParallelDeadlineGroup(
+      WaitUntilCommand {
+        abs(leftVelocity.get()) < ShooterConstants.LQR_VEL_TOL &&
+          abs(rightVelocity.get()) < ShooterConstants.LQR_VEL_TOL
+      },
+      cmd
+    ).andThen(
+      coast()
+    )
   }
 
   fun rampStop(): Command {
     val cmd = SequentialCommandGroup(
       this.runOnce {
+        println("hola im here")
         leftRateLimiter.reset(leftVelocity.get())
         rightRateLimiter.reset(rightVelocity.get())
       },
       this.run {
         desiredVels = Pair(leftRateLimiter.calculate(0.0), rightRateLimiter.calculate(0.0))
         shootPiece(desiredVels.first, desiredVels.second)
-      }
+      }.until {
+        abs(leftVelocity.get()) < ShooterConstants.MIN_RAMP_VEL &&
+          abs(rightVelocity.get()) < ShooterConstants.MIN_RAMP_VEL
+      }.andThen(
+        forceStop()
+      )
     )
     cmd.name = "active stop"
     return cmd
@@ -175,25 +187,35 @@ open class Shooter(
     builder.addDoubleProperty("1.1 Last Right Voltage", { rightMotor.lastVoltage }, null)
     builder.addDoubleProperty("1.2 Last Left Voltage", { leftMotor.lastVoltage }, null)
     builder.publishConstString("2.0", "Current and Desired Velocities")
-    builder.addDoubleProperty("2.1 Left Current Speed", { leftVelocity.get() * 60 / (2 * PI) }, null)
-    builder.addDoubleProperty("2.2 Right Current Speed", { rightVelocity.get() * 60 / (2 * PI) }, null)
+    builder.addDoubleProperty("2.1 Left Current Speed", { leftVelocity.get() }, null)
+    builder.addDoubleProperty("2.2 Right Current Speed", { rightVelocity.get() }, null)
     builder.addDoubleProperty("2.3 Left Desired Speed", { desiredVels.first }, null)
     builder.addDoubleProperty("2.4 Right Desired Speed", { desiredVels.second }, null)
     builder.publishConstString("3.0", "Velocity Errors")
     builder.addDoubleProperty("3.1 Left Vel Error Pred", { leftLoop.error.get(0, 0) }, null)
     builder.addDoubleProperty("3.2 Right Vel Error Pred", { rightLoop.error.get(0, 0) }, null)
     builder.addDoubleProperty("3.3 Left Vel Error", { leftVelocity.get() - desiredVels.first }, null)
     builder.addDoubleProperty("3.4 Right Vel Error", { rightVelocity.get() - desiredVels.second }, null)
+    builder.publishConstString("4.0", "Encoder Positions")
+    builder.addDoubleProperty("4.1 Left Enc Pos", { leftMotor.position }, null)
+    builder.addDoubleProperty("4.2 Left Enc Pos", { rightMotor.position }, null)
   }
 
   companion object {
     fun createShooter(robot: Robot): Shooter {
       val rightMotor = createSparkMax(
         "Shooter Right Motor",
         ShooterConstants.RIGHT_MOTOR_ID,
-        encCreator = NEOEncoder.creator(
+        encCreator = QuadEncoder.creator(
+          Encoder(
+            ShooterConstants.RIGHT_CHANNEL_A,
+            ShooterConstants.RIGHT_CHANNEL_B
+          ),
+          ShooterConstants.CPR,
           ShooterConstants.UPR,
-          ShooterConstants.GEARING
+          ShooterConstants.GEARING,
+          ShooterConstants.RIGHT_ENCODER_INVERTED,
+          ShooterConstants.SAMPLES_TO_AVERAGE
         ),
         inverted = ShooterConstants.RIGHT_MOTOR_INVERTED,
         currentLimit = ShooterConstants.CURRENT_LIMIT,
@@ -203,9 +225,16 @@ open class Shooter(
       val leftMotor = createSparkMax(
         "Shooter Right Motor",
         ShooterConstants.LEFT_MOTOR_ID,
-        encCreator = NEOEncoder.creator(
+        encCreator = QuadEncoder.creator(
+          Encoder(
+            ShooterConstants.LEFT_CHANNEL_A,
+            ShooterConstants.LEFT_CHANNEL_B
+          ),
+          ShooterConstants.CPR,
           ShooterConstants.UPR,
-          ShooterConstants.GEARING
+          ShooterConstants.GEARING,
+          ShooterConstants.LEFT_ENCODER_INVERTED,
+          ShooterConstants.SAMPLES_TO_AVERAGE
         ),
         inverted = ShooterConstants.LEFT_MOTOR_INVERTED,
         currentLimit = ShooterConstants.CURRENT_LIMIT,

src/main/kotlin/frc/team449/system/encoder/QuadEncoder.kt

@@ -9,12 +9,13 @@ class QuadEncoder(
   encoderCPR: Int,
   unitPerRotation: Double,
   gearing: Double,
-  pollTime: Double = .02
+  pollTime: Double = .01,
+  samplesToAverage: Int = 5
 ) : Encoder(name, 1, 1.0, 1.0, pollTime) {
   init {
     // Let the WPI encoder handle the distance scaling
     encoder.distancePerPulse = unitPerRotation * gearing / encoderCPR
-    encoder.samplesToAverage = 5
+    encoder.samplesToAverage = samplesToAverage
   }
 
   override fun getPositionNative() = encoder.distance
@@ -27,11 +28,12 @@ class QuadEncoder(
       encoderCPR: Int,
       unitPerRotation: Double,
       gearing: Double,
-      inverted: Boolean
+      inverted: Boolean,
+      samplesAverage: Int = 5
     ): EncoderCreator<T> =
       EncoderCreator { name, _, _ ->
         encoder.setReverseDirection(inverted)
-        QuadEncoder(name, encoder, encoderCPR, unitPerRotation, gearing)
+        QuadEncoder(name, encoder, encoderCPR, unitPerRotation, gearing, samplesToAverage = samplesAverage)
       }
   }
 }