4 piece shooting far auto (double check parallel and stop command str…

…ucture still)

simgui-ds.json

@@ -94,8 +94,7 @@
       "useGamepad": true
     },
     {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
+      "guid": "Keyboard0"
     }
   ]
 }

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

@@ -13,24 +13,55 @@ import kotlin.math.PI
 object AutoUtil {
 
   fun autoIntake(robot: Robot): Command {
-    return ParallelDeadlineGroup(
+    return ParallelCommandGroup(
+      SequentialCommandGroup(
+        robot.undertaker.intake(),
+        robot.feeder.autoIntake(),
+        WaitUntilCommand { !robot.infrared.get() },
+        robot.undertaker.stop(),
+        robot.feeder.outtake(),
+        WaitUntilCommand { robot.infrared.get() },
+        robot.feeder.stop(),
+      ),
       robot.shooter.shootSubwoofer(),
+    )
+  }
+
+  fun autoIntakeAway(robot: Robot): Command {
+    return ParallelCommandGroup(
       SequentialCommandGroup(
+        robot.pivot.moveStow(),
         robot.undertaker.intake(),
         robot.feeder.autoIntake(),
         WaitUntilCommand { !robot.infrared.get() },
         robot.undertaker.stop(),
         robot.feeder.outtake(),
+        robot.pivot.autoAngle(),
         WaitUntilCommand { robot.infrared.get() },
         robot.feeder.stop(),
-      )
+      ),
+      robot.shooter.shootAuto(),
+    )
+  }
+
+  fun autoShootAway(robot: Robot): Command {
+    return ParallelDeadlineGroup(
+      SequentialCommandGroup(
+        robot.pivot.autoAngle(),
+        WaitUntilCommand { robot.shooter.atAutoSetpoint() && robot.pivot.inTolerance() },
+        WaitCommand(AutoConstants.SHOOT_AWAY_WAIT),
+        robot.feeder.intake(),
+        robot.undertaker.intake(),
+        WaitCommand(AutoConstants.SHOOT_INTAKE_TIME)
+      ),
+      robot.shooter.shootAuto()
     )
   }
 
   fun autoShoot(robot: Robot): Command {
     return ParallelDeadlineGroup(
       SequentialCommandGroup(
-        WaitUntilCommand { robot.shooter.atSetpoint() },
+        WaitUntilCommand { robot.shooter.atAutoSetpoint() },
         robot.feeder.intake(),
         robot.undertaker.intake(),
         WaitCommand(AutoConstants.SHOOT_INTAKE_TIME)
@@ -71,10 +102,10 @@ object AutoUtil {
           Nat.N3(),
           FieldConstants.fieldLength - currentMatrix[0, 0],
           currentMatrix[0, 1],
-          MathUtil.angleModulus(PI + currentMatrix[0, 2]),
+          MathUtil.angleModulus(PI - currentMatrix[0, 2]),
           -currentMatrix[1, 0],
           currentMatrix[1, 1],
-          currentMatrix[1, 2]
+          -currentMatrix[1, 2]
         )
 
         pathGroup[index].stateMap.put(time, newMatrix)

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

@@ -0,0 +1,39 @@
+package frc.team449.robot2024.auto.routines
+
+import frc.team449.control.auto.ChoreoRoutine
+import frc.team449.control.auto.ChoreoRoutineStructure
+import frc.team449.control.auto.ChoreoTrajectory
+import frc.team449.robot2024.Robot
+import frc.team449.robot2024.auto.AutoUtil
+
+class FourPieceAway(
+  robot: Robot,
+  isRed: Boolean
+) : ChoreoRoutineStructure {
+  override val routine =
+    ChoreoRoutine(
+      drive = robot.drive,
+      parallelEventMap = hashMapOf(
+        0 to AutoUtil.autoIntakeAway(robot),
+        1 to AutoUtil.autoIntakeAway(robot),
+        2 to AutoUtil.autoIntakeAway(robot),
+        3 to AutoUtil.autoIntakeAway(robot),
+      ),
+      stopEventMap = hashMapOf(
+        0 to AutoUtil.autoShoot(robot),
+        1 to AutoUtil.autoShootAway(robot),
+        2 to AutoUtil.autoShootAway(robot),
+        3 to AutoUtil.autoShootAway(robot)
+      ),
+      debug = false
+    )
+
+  override val trajectory: MutableList<ChoreoTrajectory> =
+    if (isRed) {
+      AutoUtil.transformForRed(
+        ChoreoTrajectory.createTrajectory("4_Piece_Shoot_Anywhere")
+      )
+    } else {
+      ChoreoTrajectory.createTrajectory("4_Piece_Shoot_Anywhere")
+    }
+}

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

@@ -6,7 +6,7 @@ import frc.team449.control.auto.ChoreoTrajectory
 import frc.team449.robot2024.Robot
 import frc.team449.robot2024.auto.AutoUtil
 
-class Experimental4Piece(
+class FourPieceSubwoofer(
   robot: Robot,
   isRed: Boolean
 ) : ChoreoRoutineStructure {

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

@@ -9,14 +9,16 @@ class RoutineChooser(private val robot: Robot) : SendableChooser<String>() {
   fun routineMap(): HashMap<String, Command> {
     return hashMapOf(
       "DoNothing" to DoNothing(robot).createCommand(),
-      "Red4Piece" to Experimental4Piece(robot, true).createCommand(),
-      "Blue4Piece" to Experimental4Piece(robot, false).createCommand(),
+      "Red4Piece" to FourPieceSubwoofer(robot, true).createCommand(),
+      "Blue4Piece" to FourPieceSubwoofer(robot, false).createCommand(),
       "Red3PieceMid" to Experimental3PieceMid(robot, true).createCommand(),
       "Blue3PieceMid" to Experimental3PieceMid(robot, false).createCommand(),
       "Red1PieceTaxi" to Experimental1PieceTaxi(robot, true).createCommand(),
       "Blue1PieceTaxi" to Experimental1PieceTaxi(robot, false).createCommand(),
       "RedTaxi" to ExperimentalTaxi(robot, true).createCommand(),
       "BlueTaxi" to ExperimentalTaxi(robot, false).createCommand(),
+      "RedFar4Piece" to FourPieceAway(robot, true).createCommand(),
+      "BlueFar4Piece" to FourPieceAway(robot, false).createCommand()
     )
   }
 
@@ -29,10 +31,15 @@ class RoutineChooser(private val robot: Robot) : SendableChooser<String>() {
     this.setDefaultOption("Do Nothing", "DoNothing")
 
     this.addOption(
-      "Experimental 4 Piece",
+      "4 Piece Subwoofer",
       if (isRed) "Red4Piece" else "Blue4Piece"
     )
 
+    this.addOption(
+      "4 Piece Away",
+      if (isRed) "RedFar4Piece" else "BlueFar4Piece"
+    )
+
     this.addOption(
       "3 Piece Mid",
       if (isRed) "Red3PieceMid" else "Blue3PieceMid"

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

@@ -10,4 +10,6 @@ object AutoConstants {
 
   const val SHOOT_INTAKE_TIME = 0.075
   const val FEEDER_REVERSE_TIME = 0.35
+
+  const val SHOOT_AWAY_WAIT = 0.25
 }

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

@@ -38,6 +38,8 @@ object PivotConstants {
   val VEL_TOLERANCE = Units.degreesToRadians(10.0)
   const val CONTROL_EFFORT_VOLTS = 12.0
 
+  val MAX_VEL_TOL = Units.degreesToRadians(5.0)
+
   const val MAX_VOLTAGE = 12.0
 
   /** Profile Constraints */
@@ -50,5 +52,5 @@ object PivotConstants {
   val MAX_ANGLE = Units.degreesToRadians(100.0)
   val AMP_ANGLE = Units.degreesToRadians(90.0)
   val STOW_ANGLE = Units.degreesToRadians(-2.0)
-  val SUBWOOFER_ANGLE = Units.degreesToRadians(30.0)
+  val AUTO_ANGLE = Units.degreesToRadians(17.5)
 }

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

@@ -20,6 +20,9 @@ object ShooterConstants {
   val SUBWOOFER_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(5460.0)
   val SUBWOOFER_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(5460.0)
 
+  val AUTO_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(5500.0)
+  const val AUTO_SHOOT_TOL = 25.0
+
   val BRAKE_RATE_LIMIT = Units.rotationsPerMinuteToRadiansPerSecond(2850.0)
 
   val SHOOTING_MAP = InterpolatingMatrixTreeMap<Double, N3, N1>()
@@ -42,7 +45,7 @@ object ShooterConstants {
 
   const val MODEL_VEL_STDDEV = 3.0
   const val ENCODER_VEL_STDDEV = 0.5
-  const val LQR_VEL_TOL = 45.0
+  const val LQR_VEL_TOL = 10.0
   const val LQR_MAX_VOLTS = 12.0
   const val MAX_VOLTAGE = 12.0
 

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

@@ -26,6 +26,7 @@ import frc.team449.system.motor.WrappedMotor
 import frc.team449.system.motor.createSparkMax
 import java.util.function.DoubleSupplier
 import java.util.function.Supplier
+import kotlin.math.abs
 import kotlin.math.sign
 
 open class Pivot(
@@ -66,6 +67,23 @@ open class Pivot(
     }
   }
 
+  fun autoAngle(): Command {
+    return this.run {
+      moveToAngle(PivotConstants.AUTO_ANGLE)
+    }.until(::inTolerance)
+  }
+
+  fun autoStow(): Command {
+    return this.run {
+      moveToAngle(PivotConstants.STOW_ANGLE)
+    }.until(::inTolerance)
+  }
+
+  fun inTolerance(): Boolean {
+    return abs(positionSupplier.get() - lastProfileReference.position) < PivotConstants.POS_TOLERANCE &&
+      abs(velocitySupplier.get()) < PivotConstants.MAX_VEL_TOL
+  }
+
   fun manualMovement(axisSupplier: DoubleSupplier): Command {
     val cmd = this.run {
       moveToAngle(
@@ -80,12 +98,6 @@ open class Pivot(
     return cmd
   }
 
-  fun moveSubwoofer(): Command {
-    return this.run {
-      moveToAngle(PivotConstants.SUBWOOFER_ANGLE)
-    }
-  }
-
   fun moveStow(): Command {
     return this.run {
       moveToAngle(PivotConstants.STOW_ANGLE)

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

@@ -73,6 +73,17 @@ open class Shooter(
     return cmd
   }
 
+  fun shootAuto(): Command {
+    val cmd = this.run {
+      shootPiece(
+        ShooterConstants.AUTO_SPEED,
+        ShooterConstants.AUTO_SPEED
+      )
+    }
+    cmd.name = "shooting subwoofer auto side"
+    return cmd
+  }
+
   fun shootAnywhere(): Command {
     val cmd = this.run {
       val distance = FieldConstants.SUBWOOFER_POSE.getDistance(robot.drive.pose.translation)
@@ -95,6 +106,13 @@ open class Shooter(
       desiredVels.second != 0.0
   }
 
+  fun atAutoSetpoint(): Boolean {
+    return abs(leftVelocity.get() - desiredVels.first) < ShooterConstants.AUTO_SHOOT_TOL &&
+      abs(rightVelocity.get() - desiredVels.second) < ShooterConstants.AUTO_SHOOT_TOL &&
+      desiredVels.first != 0.0 &&
+      desiredVels.second != 0.0
+  }
+
   fun scoreAmp(): Command {
     val cmd = this.runOnce {
       leftMotor.setVoltage(ShooterConstants.AMP_SCORE_VOLTAGE)