Change led length to 12 per strip, add one piece taxi auto

Note: The leds have 36 physical leds but I think one led controller controls 3 leds

simgui.json

@@ -109,6 +109,9 @@
               "open": true
             }
           },
+          "driveCommand": {
+            "open": true
+          },
           "intake": {
             "double[]##v_/Monologuing/robot/intake/3.1 Intake 3D Position": {
               "open": true

src/main/kotlin/frc/team449/control/holonomic/SwerveOrthogonalCommand.kt

@@ -37,14 +37,16 @@ class SwerveOrthogonalCommand(
 
   private var atGoal = true
 
-  private var rotRamp = SlewRateLimiter(RobotConstants.ROT_RATE_LIMIT, RobotConstants.NEG_ROT_RATE_LIM, 0.0)
+  private var rotRamp = SlewRateLimiter(RobotConstants.ROT_RATE_LIMIT)
 
   private val timer = Timer()
 
   private val rotCtrl = RobotConstants.ORTHOGONAL_CONTROLLER
 
   private var skewConstant = 0.5
 
+  private var desiredVel = doubleArrayOf(0.0, 0.0, 0.0)
+
   init {
     addRequirements(drive)
     rotCtrl.enableContinuousInput(-PI, PI)
@@ -65,7 +67,7 @@ class SwerveOrthogonalCommand(
 
     rotRamp = SlewRateLimiter(
       RobotConstants.ROT_RATE_LIMIT,
-      -RobotConstants.NEG_ROT_RATE_LIM,
+      RobotConstants.NEG_ROT_RATE_LIM,
       drive.currentSpeeds.omegaRadiansPerSecond
     )
 
@@ -131,14 +133,20 @@ class SwerveOrthogonalCommand(
        * by rotating it with offset proportional to how much we are rotating
        **/
       vel.rotateBy(Rotation2d(-rotScaled * dt * skewConstant))
+
+      val desVel = ChassisSpeeds.fromFieldRelativeSpeeds(
+        vel.x * directionCompensation.invoke(),
+        vel.y * directionCompensation.invoke(),
+        rotScaled,
+        drive.heading
+      )
       drive.set(
-        ChassisSpeeds.fromFieldRelativeSpeeds(
-          vel.x * directionCompensation.invoke(),
-          vel.y * directionCompensation.invoke(),
-          rotScaled,
-          drive.heading
-        )
+        desVel
       )
+
+      desiredVel[0] = desVel.vxMetersPerSecond
+      desiredVel[1] = desVel.vyMetersPerSecond
+      desiredVel[2] = desVel.omegaRadiansPerSecond
     } else {
       drive.set(
         ChassisSpeeds(
@@ -168,5 +176,8 @@ class SwerveOrthogonalCommand(
 
     builder.publishConstString("4.0", "Turning Skew")
     builder.addDoubleProperty("4.1 skew constant", { skewConstant }, { k: Double -> skewConstant = k })
+
+    builder.publishConstString("5.0", "Given Speeds")
+    builder.addDoubleArrayProperty("Chassis Speed", { desiredVel }, null)
   }
 }

src/main/kotlin/frc/team449/robot2023/auto/routines/OnePieceTaxi.kt

@@ -0,0 +1,43 @@
+package frc.team449.robot2023.auto.routines
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
+import edu.wpi.first.wpilibj2.command.WaitCommand
+import frc.team449.control.auto.ChoreoRoutine
+import frc.team449.control.auto.ChoreoRoutineStructure
+import frc.team449.control.auto.ChoreoTrajectory
+import frc.team449.robot2023.Robot
+import frc.team449.robot2023.auto.AutoUtil
+
+class OnePieceTaxi(
+  robot: Robot,
+  isRed: Boolean
+) : ChoreoRoutineStructure {
+
+  override val routine =
+    ChoreoRoutine(
+      drive = robot.drive,
+      parallelEventMap = hashMapOf(
+        0 to robot.intake.extend().andThen(
+          robot.elevator.high()
+        ),
+        1 to robot.elevator.stow()
+      ),
+      stopEventMap = hashMapOf(
+        1 to SequentialCommandGroup(
+          WaitCommand(1.0),
+          robot.manipulator.outtake(),
+          WaitCommand(1.0),
+          robot.manipulator.stop()
+        )
+      )
+    )
+
+  override val trajectory: MutableList<ChoreoTrajectory> =
+    if (isRed) {
+      AutoUtil.transformForRed(
+        ChoreoTrajectory.createTrajectory("1PieceTaxi")
+      )
+    } else {
+      ChoreoTrajectory.createTrajectory("1PieceTaxi")
+    }
+}

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

@@ -11,8 +11,8 @@ class RoutineChooser(private val robot: Robot) : SendableChooser<String>() {
       "DoNothing" to DoNothing(robot).createCommand(),
       "BlueOnePiecePick" to OnePiecePick(robot, false).createCommand(),
       "RedOnePiecePick" to OnePiecePick(robot, true).createCommand(),
-      "BlueOnePieceDrop" to OnePieceDrop(robot, false).createCommand(),
-      "RedOnePieceDrop" to OnePieceDrop(robot, true).createCommand()
+      "BlueOnePieceTaxi" to OnePieceTaxi(robot, false).createCommand(),
+      "RedOnePieceTaxi" to OnePieceTaxi(robot, true).createCommand()
     )
   }
 
@@ -34,11 +34,11 @@ class RoutineChooser(private val robot: Robot) : SendableChooser<String>() {
     )
 
     this.addOption(
-      "One Piece and Drop",
+      "One Piece and Taxi",
       if (isRed) {
-        "RedOnePieceDrop"
+        "RedOnePieceTaxi"
       } else {
-        "BlueOnePieceDrop"
+        "BlueOnePieceTaxi"
       }
     )
   }

src/main/kotlin/frc/team449/robot2023/commands/light/BlairChasing.kt

@@ -24,21 +24,21 @@ class BlairChasing(
   override fun execute() {
     for (i in LightConstants.SECTION_1_START..LightConstants.SECTION_1_END) {
       // This number is related to how many lights will show up between the high and low intensity (which technically also affects how fast itll cycle)
-      val saturation = MathUtil.inputModulus(firstSaturation1 + i * 300.0 / led.buffer.length, 0.0, 255.0)
+      val saturation = MathUtil.inputModulus(firstSaturation1 + i * 200.0 / led.buffer.length, 0.0, 255.0)
       led.setHSV(i, 0, saturation.toInt(), 255)
 
       // The i * 255.0 relates to how fast it will cycle in between the high and low intensity
-      firstSaturation1 += 0.0375
+      firstSaturation1 += 0.135
       firstSaturation1 = MathUtil.inputModulus(firstSaturation1, 0.0, 255.0)
     }
 
     for (i in LightConstants.SECTION_2_START..LightConstants.SECTION_2_END) {
       // This number is related to how many lights will show up between the high and low intensity (which technically also affects how fast itll cycle)
-      val saturation = MathUtil.inputModulus(firstSaturation2 + (i - LightConstants.SECTION_2_START) * 300.0 / led.buffer.length, 0.0, 255.0)
+      val saturation = MathUtil.inputModulus(firstSaturation2 + (i - LightConstants.SECTION_2_START) * 200.0 / led.buffer.length, 0.0, 255.0)
       led.setHSV(i, 0, saturation.toInt(), 255)
 
       // The i * 255.0 relates to how fast it will cycle in between the high and low intensity
-      firstSaturation2 += 0.0375
+      firstSaturation2 += 0.135
       firstSaturation2 = MathUtil.inputModulus(firstSaturation2, 0.0, 255.0)
     }
   }

src/main/kotlin/frc/team449/robot2023/commands/light/BreatheHue.kt

@@ -26,22 +26,22 @@ class BreatheHue(
   override fun execute() {
     for (i in LightConstants.SECTION_1_START..LightConstants.SECTION_1_END) {
       // This number is related to how many lights will show up between the high and low intensity
-      val intensity = MathUtil.inputModulus(firstIntensity1 + i * 32.5 / led.buffer.length, 175.0, 255.0)
+      val intensity = MathUtil.inputModulus(firstIntensity1 + i * 32.5 / led.buffer.length, 125.0, 255.0)
       led.setHSV(i, hue, 255, intensity.toInt())
 
       // The i * 255.0 relates to how fast it will cycle in between the high and low intensity
-      firstIntensity1 += 0.045
-      firstIntensity1 = MathUtil.inputModulus(firstIntensity1, 175.0, 255.0)
+      firstIntensity1 += 0.1
+      firstIntensity1 = MathUtil.inputModulus(firstIntensity1, 125.0, 255.0)
     }
 
     for (i in LightConstants.SECTION_2_START..LightConstants.SECTION_2_END) {
       // This number is related to how many lights will show up between the high and low intensity
-      val intensity = MathUtil.inputModulus(firstIntensity2 + (i - LightConstants.SECTION_2_START) * 32.5 / led.buffer.length, 175.0, 255.0)
+      val intensity = MathUtil.inputModulus(firstIntensity2 + (i - LightConstants.SECTION_2_START) * 32.5 / led.buffer.length, 125.0, 255.0)
       led.setHSV(i, hue, 255, intensity.toInt())
 
       // The i * 255.0 relates to how fast it will cycle in between the high and low intensity
-      firstIntensity2 += 0.045
-      firstIntensity2 = MathUtil.inputModulus(firstIntensity2, 175.0, 255.0)
+      firstIntensity2 += 0.1
+      firstIntensity2 = MathUtil.inputModulus(firstIntensity2, 125.0, 255.0)
     }
   }
 }

src/main/kotlin/frc/team449/robot2023/constants/subsystem/LightConstants.kt

@@ -2,10 +2,10 @@ package frc.team449.robot2023.constants.subsystem
 
 object LightConstants {
   const val LIGHT_PORT = 9
-  const val LIGHT_LENGTH = 72
+  const val LIGHT_LENGTH = 24
 
   const val SECTION_1_START = 0
-  const val SECTION_1_END = 35
-  const val SECTION_2_START = 36
-  const val SECTION_2_END = 71
+  const val SECTION_1_END = 11
+  const val SECTION_2_START = 12
+  const val SECTION_2_END = 23
 }