improved scoring logic, fixed the bug in intake

Shenzhen-Robotics-Alliance · Sep 13, 2024 · 3f069aa · 3f069aa
1 parent 5d4a171
commit 3f069aa
Show file tree

Hide file tree

Showing 5 changed files with 21 additions and 17 deletions.
diff --git a/...e/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to second and grab fast.path b/...e/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to second and grab fast.path
@@ -61,11 +61,6 @@
     }
   ],
   "rotationTargets": [
-    {
-      "waypointRelativePos": 1,
-      "rotationDegrees": 180.0,
-      "rotateFast": false
-    },
     {
       "waypointRelativePos": 0.25,
       "rotationDegrees": 180.0,
@@ -75,6 +70,11 @@
       "waypointRelativePos": 2.8,
       "rotationDegrees": -171.25383773744474,
       "rotateFast": true
+    },
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 180.0,
+      "rotateFast": true
     }
   ],
   "constraintZones": [
@@ -114,9 +114,9 @@
     "rotateFast": false
   },
   "reversed": false,
-  "folder": "Amp Side Six Note",
+  "folder": null,
   "previewStartingState": {
-    "rotation": -151.29115678153866,
+    "rotation": -155.55562093631892,
     "velocity": 0
   },
   "useDefaultConstraints": true

diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java
@@ -413,7 +413,7 @@ public void configureButtonBindings() {
                 driveInput, drive,
                 FieldConstants.SPEAKER_POSITION_SUPPLIER,
                 shooterOptimization,
-                0.5
+                0.3
         );
         final Command semiAutoAimAndShoot = new AimAndShootSequence(
                 pitch, flyWheels, intake, shooterOptimization, drive,
@@ -422,7 +422,10 @@ public void configureButtonBindings() {
                 ledStatusLight,
                 visualizerForShooter
         ).ifNotePresent();
-        driverXBox.rightTrigger(0.5).whileTrue(semiAutoAimAndShoot.deadlineWith(faceTargetWhileDrivingLowSpeed));
+        driverXBox.rightTrigger(0.5).whileTrue(semiAutoAimAndShoot
+                        .deadlineWith(faceTargetWhileDrivingLowSpeed)
+                        .finallyDo(() -> joystickDrive.setCurrentRotationalMaintenance(drive.getFacing()))
+        );
 
         driverXBox.rightBumper().whileTrue(new PathFindToPoseAndShootSequence(
                 intake, pitch, flyWheels, shooterOptimization, drive,

diff --git a/...ple/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java b/...ple/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java
@@ -60,10 +60,10 @@ public AimAndShootSequence(
         super.addRequirements(pitch, flyWheels, intake);
         super.addCommands(Commands.runOnce(intake::runIdle));
 
-        super.addCommands(
-                new PrepareToAim(flyWheels, pitch, shooterOptimization, ledStatusLight,  robotScoringPositionSupplier, targetPositionSupplier)
-                        .untilReady()
-        );
+//        super.addCommands(
+//                new PrepareToAim(flyWheels, pitch, shooterOptimization, ledStatusLight,  robotScoringPositionSupplier, targetPositionSupplier)
+//                        .untilReady()
+//        );
         final AimAtSpeakerContinuously aimAtSpeakerContinuously = new AimAtSpeakerContinuously(
                 flyWheels, pitch, ledStatusLight, shooterOptimization, drive, targetPositionSupplier, externalShootCondition
         );

diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveTrainConstants.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveTrainConstants.java
@@ -93,8 +93,8 @@ public class DriveTrainConstants {
 
     /* for collision detection in simulation */
     public static final double
-            BUMPER_WIDTH_METERS = Units.inchesToMeters(34.5),
-            BUMPER_LENGTH_METERS = Units.inchesToMeters(36),
+            BUMPER_WIDTH_METERS = Units.inchesToMeters(32),
+            BUMPER_LENGTH_METERS = Units.inchesToMeters(32),
             /* https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction */
             BUMPER_COEFFICIENT_OF_FRICTION = 0.65,
             /* https://simple.wikipedia.org/wiki/Coefficient_of_restitution */

diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.geometry.Translation2d;
 import frc.robot.Robot;
+import frc.robot.constants.DriveTrainConstants;
 import frc.robot.utils.CompetitionFieldUtils.Simulations.IntakeSimulation;
 
 public class IntakeIOSim extends IntakeSimulation implements IntakeIO {
@@ -17,8 +18,8 @@ public class IntakeIOSim extends IntakeSimulation implements IntakeIO {
     private boolean shooterRunning = false;
     public IntakeIOSim() {
         super(
-                new Translation2d(-0.9144/2-0.01, 0.3),
-                new Translation2d(-0.9144/2-0.01, -0.3),
+                new Translation2d(-DriveTrainConstants.BUMPER_LENGTH_METERS/2, 0.3),
+                new Translation2d(-DriveTrainConstants.BUMPER_LENGTH_METERS/2, -0.3),
                 1
         );
     }