fried at worlds but its over?

team4099 · May 13, 2024 · a430254 · a430254
1 parent 68ee786
commit a430254
Show file tree

Hide file tree

Showing 7 changed files with 22 additions and 80 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
@@ -142,7 +142,7 @@ object AutonomousSelector {
   }
 
   val waitTime: Time
-    get() = 0.0.seconds
+    get() = waitBeforeCommandSlider.getDouble(0.0).seconds
 
   val secondaryWaitTime: Time
     get() = secondaryWaitInAuto.getDouble(0.0).seconds

diff --git a/...ain/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt b/...ain/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt
@@ -67,7 +67,7 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath(
           )
         }
       ),
-      superstructure.prepManualSpeakerCommand(-2.5.degrees, 4400.rotations.perMinute),
+      superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute),
       superstructure.scoreCommand(),
       ParallelCommandGroup(
         DrivePathCommand.createPathInFieldFrame(
@@ -113,7 +113,7 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath(
         WaitCommand(1.0)
           .andThen(superstructure.groundIntakeCommand())
       ),
-      superstructure.prepManualSpeakerCommand(-2.5.degrees, 44000.rotations.perMinute),
+      superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute),
       superstructure.scoreCommand(),
       ParallelCommandGroup(
         DrivePathCommand.createPathInFieldFrame(

diff --git a/...eam4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt b/...eam4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt
@@ -40,11 +40,11 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath(
                 null,
                 180.degrees.inRotation2ds
               ),
-              FieldWaypoint(
-                Translation2d(8.32.meters, 0.78.meters).translation2d,
-                null,
-                180.degrees.inRotation2ds
-              )
+            FieldWaypoint(
+              Translation2d(8.32.meters, 2.44.meters).translation2d,
+              null,
+              220.degrees.inRotation2ds
+            )
             )
           }
         ),
@@ -56,9 +56,9 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath(
             {
               listOf(
                 FieldWaypoint(
-                  Translation2d(8.32.meters, 0.78.meters).translation2d,
+                  Translation2d(8.32.meters, 2.44.meters).translation2d,
                   null,
-                  180.degrees.inRotation2ds
+                  220.degrees.inRotation2ds
                 ),
                 FieldWaypoint(
                   Translation2d(6.89.meters, 1.73.meters).translation2d,
@@ -73,68 +73,7 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath(
               )
             }
           ),
-          superstructure.prepManualSpeakerCommand(-2.2.degrees, 4000.rotations.perMinute)
-        )
-          .andThen(superstructure.scoreCommand())
-          .andThen(
-            ParallelCommandGroup(
-              DrivePathCommand.createPathInFieldFrame(
-                drivetrain,
-                {
-                  listOf(
-                    FieldWaypoint(
-                      Translation2d(4.33.meters, 1.67.meters).translation2d,
-                      null,
-                      (180 - 43.3758).degrees.inRotation2ds
-                    ),
-                    FieldWaypoint(
-                      Translation2d(
-                        (4.33.meters + 8.32.meters) / 2,
-                        (2.44 + 0.78).meters / 2 - 0.1.meters
-                      )
-                        .translation2d,
-                      null,
-                      ((180 - 43.3758 + 220) / 2 + 10).degrees.inRotation2ds,
-                    ),
-                    FieldWaypoint(
-                      Translation2d(8.32.meters, 2.44.meters).translation2d,
-                      null,
-                      220.degrees.inRotation2ds
-                    )
-                  )
-                }
-              ),
-              WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()),
-            )
-          ),
-        ParallelCommandGroup(
-          DrivePathCommand.createPathInFieldFrame(
-            drivetrain,
-            {
-              listOf(
-                FieldWaypoint(
-                  Translation2d(8.32.meters, 2.44.meters).translation2d,
-                  null,
-                  220.degrees.inRotation2ds
-                ),
-                FieldWaypoint(
-                  Translation2d(
-                    (4.33.meters + 8.32.meters) / 2,
-                    (2.44 + 0.78).meters / 2 + 0.1.meters
-                  )
-                    .translation2d,
-                  null,
-                  ((180 - 43.3758 + 220) / 2 + 10).degrees.inRotation2ds,
-                ),
-                FieldWaypoint(
-                  Translation2d(4.33.meters, 1.67.meters).translation2d,
-                  null,
-                  (180 - 43.3758).degrees.inRotation2ds
-                )
-              )
-            }
-          ),
-          superstructure.prepManualSpeakerCommand(-2.2.degrees, 4000.rotations.perMinute)
+          superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute)
         )
           .andThen(superstructure.scoreCommand())
           .andThen(
@@ -164,9 +103,9 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath(
                       180.degrees.inRotation2ds
                     ),
                     FieldWaypoint(
-                      Translation2d(5.5.meters, 1.85.meters).translation2d,
+                      Translation2d(4.33.meters, 1.67.meters).translation2d,
                       null,
-                      (180 - 35.02902884839783).degrees.inRotation2ds
+                      (180 - 43.37583640633171).degrees.inRotation2ds
                     )
                   )
                 }
@@ -176,7 +115,7 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath(
                 .andThen(WaitCommand(2.0))
                 .andThen(
                   superstructure.prepManualSpeakerCommand(
-                    -2.0.degrees, 4400.rotations.perMinute
+                    -4.5.degrees, 4000.rotations.perMinute
                   )
                 )
             )

diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
@@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.GenericHID
 import edu.wpi.first.wpilibj2.command.button.Trigger
 import org.team4099.lib.joystick.XboxOneGamepad
 import java.util.function.Consumer
+import kotlin.math.absoluteValue
 
 /**
  * Maps buttons on the driver and operator controllers to specific actions with meaningful variable
@@ -29,7 +30,7 @@ object ControlBoard {
 
   val turn: Double
     get() {
-      return if (driver.rightXAxis < 0.85) {
+      return if (driver.rightXAxis.absoluteValue < 0.80) {
         driver.rightXAxis * DrivetrainConstants.TELEOP_TURNING_SPEED_PERCENT
       } else {
         driver.rightXAxis

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
@@ -33,7 +33,7 @@ object DrivetrainConstants {
   const val FOC_ENABLED = true
   const val MINIMIZE_SKEW = false
 
-  const val TELEOP_TURNING_SPEED_PERCENT = 0.7
+  const val TELEOP_TURNING_SPEED_PERCENT = 0.5
 
   const val OMOMETRY_UPDATE_FREQUENCY = 250.0
 

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
@@ -37,9 +37,9 @@ object WristConstants {
   val ABSOLUTE_ENCODER_OFFSET =
     (
       97.72227856659904.degrees - 35.degrees + 1.90.degrees -
-        0.5.degrees -
-        1.0.degrees - // add to drop angle
-        1.degrees -
+        0.55.degrees -
+        0.6.degrees - // add to drop angle
+        1.degrees - 0.5.degrees -
         96.3.degrees
       ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO
   val WRIST_LENGTH = 18.6.inches

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt
@@ -23,6 +23,8 @@ interface MotorType
 
 typealias REVNeo = MotorType
 
+typealias Kraken = MotorType
+
 typealias Falcon = MotorType
 
 typealias SimMotor = MotorType