EOL for drive

team4099 · Apr 7, 2024 · 0667957 · 0667957
1 parent c6f8ef2
commit 0667957
Show file tree

Hide file tree

Showing 6 changed files with 41 additions and 50 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -80,7 +80,7 @@ object RobotContainer {
 
       drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal)
       vision = Vision(object : CameraIO {}, CameraIOPhotonvision("parakeet_2"))
-      limelight = LimelightVision(LimelightVisionIOReal)
+      limelight = LimelightVision(object : LimelightVisionIO {})
       intake = Intake(IntakeIOFalconNEO)
       feeder = Feeder(FeederIONeo)
       elevator = Elevator(ElevatorIONEO)
@@ -169,19 +169,7 @@ object RobotContainer {
 
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))
     ControlBoard.intake.whileTrue(
-      ParallelCommandGroup(
-        superstructure.groundIntakeCommand(),
-        TargetNoteCommand(
-          driver = Ryan(),
-          { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-          { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-          { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
-          { ControlBoard.slowMode },
-          drivetrain,
-          limelight,
-          feeder
-        )
-      )
+        superstructure.groundIntakeCommand()
     )
 
     ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand())
@@ -197,9 +185,13 @@ object RobotContainer {
         if (DriverStation.getAlliance().isPresent &&
           DriverStation.getAlliance().get() == DriverStation.Alliance.Red
         )
-        206.degrees + 180.degrees
+        225.degrees + 180.degrees
+        else if (DriverStation.getAlliance().isPresent &&
+          DriverStation.getAlliance().get() == DriverStation.Alliance.Blue
+        )
+          135.degrees
         else
-        154.degrees
+        135.degrees
       },
     ))
 

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt
@@ -37,18 +37,13 @@ class ThreeNoteCenterlineFromAmpAutoPath(
                 startingPose.rotation.inRotation2ds
               ),
               FieldWaypoint(
-                Translation2d(2.24.meters, 7.68.meters).translation2d,
-                0.degrees.inRotation2ds,
-                180.degrees.inRotation2ds
-              ),
-              FieldWaypoint(
-                Translation2d(5.0.meters, 7.68.meters).translation2d,
+                Translation2d(1.90.meters, 6.76.meters).translation2d,
                 null,
-                180.degrees.inRotation2ds
+                210.degrees.inRotation2ds
               ),
               FieldWaypoint(
-                Translation2d(6.0.meters, 7.45.meters).translation2d,
-                0.degrees.inRotation2ds,
+                Translation2d(2.87.meters, 6.27.meters).translation2d,
+                null,
                 180.degrees.inRotation2ds
               ),
               FieldWaypoint(
@@ -59,7 +54,7 @@ class ThreeNoteCenterlineFromAmpAutoPath(
             )
           }
         ),
-        WaitCommand(2.0).andThen(superstructure.groundIntakeCommand())
+        WaitCommand(2.25).andThen(superstructure.groundIntakeCommand())
       ),
       ParallelCommandGroup(
         DrivePathCommand.createPathInFieldFrame(
@@ -79,7 +74,7 @@ class ThreeNoteCenterlineFromAmpAutoPath(
             )
           }
         ),
-        WaitCommand(1.3)
+        WaitCommand(1.0)
           .andThen(
             superstructure
               .prepManualSpeakerCommand(
@@ -100,10 +95,10 @@ class ThreeNoteCenterlineFromAmpAutoPath(
                 (180 + 13.856).degrees.inRotation2ds
               ),
               FieldWaypoint(
-                Translation2d(((3.9 + 8.27).meters) / 2, (6.45 + 7.45).meters / 2)
+                Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2)
                   .translation2d,
                 null,
-                ((180 + 13.856 + 160).degrees / 2).inRotation2ds
+                ((180 + 13.856 + 165).degrees / 2).inRotation2ds
               ),
               FieldWaypoint(
                 Translation2d(8.27.meters, 5.78.meters).translation2d,
@@ -127,7 +122,7 @@ class ThreeNoteCenterlineFromAmpAutoPath(
                 160.degrees.inRotation2ds
               ),
               FieldWaypoint(
-                Translation2d(((3.9 + 8.27).meters) / 2, (6.45 + 7.45).meters / 2)
+                Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2)
                   .translation2d,
                 null,
                 ((180 + 13.856 + 160).degrees / 2).inRotation2ds
@@ -140,7 +135,7 @@ class ThreeNoteCenterlineFromAmpAutoPath(
             )
           }
         ),
-        WaitCommand(1.3)
+        WaitCommand(1.0)
           .andThen(
             superstructure.prepManualSpeakerCommand(-7.degrees, 4000.rotations.perMinute)
           )
@@ -157,10 +152,10 @@ class ThreeNoteCenterlineFromAmpAutoPath(
                 (180 + 13.856).degrees.inRotation2ds
               ),
               FieldWaypoint(
-                Translation2d(((3.9 + 8.27).meters) / 2, (6.45 + 7.45).meters / 2)
+                Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2)
                   .translation2d,
                 null,
-                ((180 + 13.856 + 140).degrees / 2).inRotation2ds
+                ((180 + 13.856 + 165).degrees / 2).inRotation2ds
               ),
               FieldWaypoint(
                 Translation2d(8.27.meters, 4.11.meters).translation2d,
@@ -183,10 +178,10 @@ class ThreeNoteCenterlineFromAmpAutoPath(
                 140.degrees.inRotation2ds
               ),
               FieldWaypoint(
-                Translation2d(((3.9 + 8.27).meters) / 2, (6.45 + 7.45).meters / 2)
+                Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2)
                   .translation2d,
                 null,
-                ((180 + 13.856 + 140).degrees / 2).inRotation2ds
+                ((180 + 13.856 + 160).degrees / 2).inRotation2ds
               ),
               FieldWaypoint(
                 Translation2d(3.9.meters, 6.45.meters).translation2d,
@@ -196,7 +191,7 @@ class ThreeNoteCenterlineFromAmpAutoPath(
             )
           }
         ),
-        WaitCommand(1.3)
+        WaitCommand(1.0)
           .andThen(
             superstructure.prepManualSpeakerCommand(-5.5.degrees, 4000.rotations.perMinute)
           )
@@ -208,4 +203,4 @@ class ThreeNoteCenterlineFromAmpAutoPath(
   companion object {
     val startingPose = Pose2d(0.75.meters, 6.70.meters, 240.degrees)
   }
-}
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
@@ -20,7 +20,7 @@ object WristConstants {
   // val ROLLER_GEAR_RATIO = 0.0
   // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts
   // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps
-  val UNDER_STAGE_SHOT = -33.5.degrees
+  val UNDER_STAGE_SHOT = 25.5.degrees
   val PUSH_DOWN_VOLTAGE = -0.5.volts
 
   val EJECT_ANGLE = -15.degrees
@@ -36,9 +36,9 @@ object WristConstants {
   val VOLTAGE_COMPENSATION = 12.0.volts
   val ABSOLUTE_ENCODER_OFFSET =
     (
-      97.72227856659904.degrees - 35.degrees + 1.65.degrees -
+      97.72227856659904.degrees - 35.degrees + 1.90.degrees -
         0.5.degrees -
-        1.8.degrees -
+        2.2.degrees - // add to drop angle
         1.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/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt
@@ -212,7 +212,7 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
     io.updateInputs(inputs)
 
     Logger.processInputs("Flywheel", inputs)
-    DebugLogger.recordDebugOutput(
+    Logger.recordOutput(
       "Flywheel/targetDifference",
       (inputs.leftFlywheelVelocity - flywheelLeftTargetVelocity)
         .absoluteValue
@@ -227,13 +227,14 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
     Logger.recordOutput("Flywheel/FlywheelRightTargetVoltage", flywheelTargetVoltage.inVolts)
     Logger.recordOutput("Flywheel/FlywheelLeftTargetVoltage", flywheelTargetVoltage.inVolts)
 
+    Logger.recordOutput(
+      "Flywheel/FlywheelRightTargetVelocity", flywheelRightTargetVelocity.inRotationsPerMinute
+    )
+    Logger.recordOutput(
+      "Flywheel/FlywheelLeftTargetVelocity", flywheelLeftTargetVelocity.inRotationsPerMinute
+    )
     if (Constants.Tuning.DEBUGING_MODE) {
-      Logger.recordOutput(
-        "Flywheel/FlywheelRightTargetVelocity", flywheelRightTargetVelocity.inRotationsPerMinute
-      )
-      Logger.recordOutput(
-        "Flywheel/FlywheelLeftTargetVelocity", flywheelLeftTargetVelocity.inRotationsPerMinute
-      )
+
       Logger.recordOutput("Flywheel/FlywheelLastVoltage", lastRightFlywheelVoltage.inVolts)
     }
 

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -360,6 +360,9 @@ class Superstructure(
           is Request.SuperstructureRequest.PassingShot -> {
             nextState = SuperstructureStates.PASSING_SHOT_PREP
           }
+          is Request.SuperstructureRequest.UnderStageShot -> {
+            nextState = SuperstructureStates.UNDER_STAGE_SHOT_PREP
+          }
         }
       }
       SuperstructureStates.GROUND_INTAKE_PREP -> {
@@ -900,7 +903,7 @@ class Superstructure(
   fun underStageCommand(): Command {
     val returnCommand =
       run { currentRequest = Request.SuperstructureRequest.UnderStageShot() }.until {
-        currentState == SuperstructureStates.PASSING_SHOT_PREP
+        currentState == SuperstructureStates.UNDER_STAGE_SHOT_PREP
       }
 
     returnCommand.name = "UnderStageShotCommand"

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt
@@ -96,8 +96,8 @@ class Wrist(val io: WristIO) : SubsystemBase() {
 
     val passingShotAngle =
       LoggedTunableValue(
-        "Wrist/underStageShotAngle",
-        WristConstants.UNDER_STAGE_SHOT,
+        "Wrist/passingShotAngle",
+        WristConstants.PASSING_SHOT_ANGLE,
         Pair({ it.inDegrees }, { it.degrees })
       )