Merge pull simulation and auto from dev

improvements in simulation and auto

example/5516-2024-OffSeason/AdvantageScope-Simulation.json

@@ -0,0 +1,212 @@
+{
+  "hubs": [
+    {
+      "x": 302,
+      "y": 184,
+      "width": 1106,
+      "height": 650,
+      "state": {
+        "sidebar": {
+          "width": 300,
+          "expanded": [
+            "/LiveWindow",
+            "/AdvantageKit",
+            "/AdvantageKit/RealOutputs",
+            "/AdvantageKit/RealOutputs/Field",
+            "/AdvantageKit/RealOutputs/Shooter",
+            "/AdvantageKit/RealOutputs/SwerveStates",
+            "/AdvantageKit/RealOutputs/Odometry",
+            "/AdvantageKit/RealOutputs/FieldSimulation",
+            "/SmartDashboard",
+            "/SmartDashboard/MaplePhysicsSimulation",
+            "/SmartDashboard/MapleArenaSimulation",
+            "/AdvantageKit/RealOutputs/Odometry/Robot"
+          ]
+        },
+        "tabs": {
+          "selected": 2,
+          "tabs": [
+            {
+              "type": 0,
+              "title": "",
+              "controller": null,
+              "controllerUUID": "t651a9wcsm4uv2nn7i1w8unwiw3a5n84",
+              "renderer": "#/",
+              "controlsHeight": 0
+            },
+            {
+              "type": 1,
+              "title": "Line Graph",
+              "controller": {
+                "leftSources": [],
+                "rightSources": [],
+                "discreteSources": [],
+                "leftLockedRange": null,
+                "rightLockedRange": null,
+                "leftUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "rightUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "leftFilter": 0,
+                "rightFilter": 0
+              },
+              "controllerUUID": "a40mbm1463684stb0lw9q99k08q674jn",
+              "renderer": null,
+              "controlsHeight": 200
+            },
+            {
+              "type": 3,
+              "title": "3D Field",
+              "controller": {
+                "sources": [
+                  {
+                    "type": "robot",
+                    "logKey": "NT:/AdvantageKit/RealOutputs//Field/Robot",
+                    "logType": "Pose2d",
+                    "visible": true,
+                    "options": {
+                      "model": "11:59"
+                    }
+                  },
+                  {
+                    "type": "component",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Shooter/MechanismPose",
+                    "logType": "Pose3d[]",
+                    "visible": true,
+                    "options": {}
+                  },
+                  {
+                    "type": "swerveStates",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured",
+                    "logType": "SwerveModuleState[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#ff0000",
+                      "arrangement": "0,1,2,3"
+                    }
+                  },
+                  {
+                    "type": "robot",
+                    "logKey": "NT:/AdvantageKit/RealOutputs//Field/OpponentRobot",
+                    "logType": "Pose3d[]",
+                    "visible": true,
+                    "options": {
+                      "model": "2024 KitBot"
+                    }
+                  },
+                  {
+                    "type": "gamePiece",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Shooter/NoteInShooter",
+                    "logType": "Pose3d",
+                    "visible": true,
+                    "options": {
+                      "variant": "Note"
+                    }
+                  },
+                  {
+                    "type": "gamePiece",
+                    "logKey": "NT:/AdvantageKit/RealOutputs//Field/Note",
+                    "logType": "Pose3d[]",
+                    "visible": true,
+                    "options": {
+                      "variant": "Note"
+                    }
+                  },
+                  {
+                    "type": "trajectory",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Trajectory",
+                    "logType": "Pose2d[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ffff",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "robot",
+                    "logKey": "NT:/AdvantageKit/RealOutputs//Field/AlliancePartnerRobot",
+                    "logType": "Pose3d[]",
+                    "visible": true,
+                    "options": {
+                      "model": "2024 KitBot"
+                    }
+                  }
+                ],
+                "game": "2024 Field",
+                "origin": "blue"
+              },
+              "controllerUUID": "z31ichy013czxpxjwwhbvvdciqbfun51",
+              "renderer": {
+                "cameraIndex": -1,
+                "orbitFov": 50,
+                "cameraPosition": [
+                  -13.099515367064587,
+                  5.622905314106006,
+                  -1.1187082198216616
+                ],
+                "cameraTarget": [
+                  2.1564592708968564,
+                  -3.4497661190395785,
+                  1.6037949750703593
+                ]
+              },
+              "controlsHeight": 200
+            },
+            {
+              "type": 4,
+              "title": "Table",
+              "controller": [
+                "NT:/SmartDashboard/MapleArenaSimulation/difference",
+                "NT:/SmartDashboard/MapleArenaSimulation/chassisFrictionForce"
+              ],
+              "controllerUUID": "lx7skwjvbdch6kn71vxrmu6561rbc2q3",
+              "renderer": null,
+              "controlsHeight": 0
+            },
+            {
+              "type": 9,
+              "title": "Swerve",
+              "controller": {
+                "sources": [
+                  {
+                    "type": "states",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured",
+                    "logType": "SwerveModuleState[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#ff0000",
+                      "arrangement": "0,1,2,3"
+                    }
+                  },
+                  {
+                    "type": "states",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Setpoints",
+                    "logType": "SwerveModuleState[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#0000ff",
+                      "arrangement": "0,1,2,3"
+                    }
+                  }
+                ],
+                "maxSpeed": 5,
+                "sizeX": 0.65,
+                "sizeY": 0.65,
+                "orientation": 1
+              },
+              "controllerUUID": "x7ibv6cg5jcq5xxna17n6t1e1gzc18bu",
+              "renderer": null,
+              "controlsHeight": 200
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "satellites": [],
+  "version": "4.0.0-beta-1"
+}

example/5516-2024-OffSeason/build.gradle

@@ -58,7 +58,6 @@ wpi.java.debugJni = false
 // Set this to true to enable desktop support.
 def includeDesktopSupport = true
 
-// Configuration for AdvantageKit
 repositories {
     maven {
         url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit")
@@ -67,6 +66,15 @@ repositories {
             password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051"
         }
     }
+
+    maven {
+        url = uri("https://maven.pkg.github.com/Shenzhen-Robotics-Alliance/maple-sim")
+        credentials {
+            username = "Shenzhen-Robotics-Alliance"
+            password = "\u0067\u0069\u0074\u0068\u0075\u0062\u005F\u0070\u0061\u0074\u005F\u0031\u0031\u0041\u0052\u0037\u0033\u0059\u004C\u0049\u0030\u0034\u0030\u0044\u004E\u006B\u0032\u006C\u0038\u004F\u004A\u006E\u0059\u005F\u0036\u0045\u0030\u006F\u0037\u004D\u0052\u004D\u0053\u0065\u006D\u0044\u0072\u0056\u006B\u0079\u0041\u006F\u0048\u004F\u0064\u0052\u007A\u0056\u0062\u0054\u0058\u0046\u004A\u0062\u0067\u006F\u0032\u0032\u0055\u0056\u0064\u0058\u0069\u004F\u0037\u0079\u0041\u004F\u0053\u0052\u004F\u005A\u0032\u0032\u0054\u0079\u006E\u0031\u0056\u0054\u004B\u006C\u0042"
+        }
+    }
+
     mavenLocal()
 }
 

example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AmpSideSixNotesFast.java

@@ -12,6 +12,8 @@
 import frc.robot.commands.shooter.AimAtSpeakerFactory;
 import frc.robot.commands.shooter.FollowPathGrabAndShootStill;
 
+import java.util.HashSet;
+
 
 public class AmpSideSixNotesFast implements Auto {
     @Override
@@ -24,7 +26,6 @@ public Command getAutoCommand(RobotContainer robot) {
                 robot.drive, robot.intake, robot.pitch, robot.flyWheels, robot.shooterOptimization, robot.ledStatusLight,
                 robot.visualizerForShooter
         ));
-        commands.addCommands(AutoUtils.setIdleForSuperStructureCommand(robot));
 
         /* grab second */
         final Command chassisMoveToSecond = AutoBuilder.followPath(PathPlannerPath.fromPathFile("move to second and grab fast"))
@@ -34,7 +35,7 @@ public Command getAutoCommand(RobotContainer robot) {
                         .finallyDo(() -> noteFightFailed[0] = !robot.intake.isNotePresent())
         );
         commands.addCommands(grabSecondWhenClose.deadlineWith(
-                chassisMoveToSecond.alongWith(robot.pitch.getPitchDefaultCommand())
+                chassisMoveToSecond.alongWith(AutoUtils.getSuperStructureIdleCommand(robot))
         ));
 
         /* retry grab second */
@@ -54,7 +55,6 @@ public Command getAutoCommand(RobotContainer robot) {
                 robot.drive, robot.intake, robot.pitch, robot.flyWheels, robot.shooterOptimization, robot.ledStatusLight,
                 robot.visualizerForShooter
         ));
-        commands.addCommands(AutoUtils.setIdleForSuperStructureCommand(robot));
 
 
         /* move to third and grab */
@@ -63,7 +63,7 @@ public Command getAutoCommand(RobotContainer robot) {
         final Command moveToThirdAlter = AutoBuilder.followPath(PathPlannerPath.fromPathFile("move to third fast alter"))
                 .andThen(Commands.runOnce(robot.drive::stop, robot.drive));
         final Command moveToThirdDecisive = new CommandOnFly(() -> noteFightFailed[0] ? moveToThirdAlter : moveToThirdNormal)
-                .deadlineWith(robot.pitch.getPitchDefaultCommand());
+                .deadlineWith(AutoUtils.getSuperStructureIdleCommand(robot));
         final Command intakeThird = Commands.waitSeconds(1)
                 .andThen(robot.intake.suckNoteUntilTouching().withTimeout(4));
         commands.addCommands(intakeThird.deadlineWith(moveToThirdDecisive));
@@ -104,6 +104,7 @@ public Command getAutoCommand(RobotContainer robot) {
     }
     @Override
     public Pose2d getStartingPoseAtBlueAlliance() {
+        // return new Pose2d(1.47, 6.32, Rotation2d.fromDegrees(180));
         return new Pose2d(1.47, 6.32, Rotation2d.fromDegrees(-150));
     }
 }

example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java

@@ -5,12 +5,8 @@
 import frc.robot.RobotContainer;
 
 public class AutoUtils {
-    public static Command setIdleForSuperStructureCommand(RobotContainer robot) {
-        return Commands.runOnce(() -> setIdleForSuperStructuer(robot), robot.intake, robot.pitch, robot.flyWheels);
-    }
-    public static void setIdleForSuperStructuer(RobotContainer robot) {
-        robot.intake.runIdle();
-        robot.flyWheels.runIdle();
-        robot.pitch.setIdle();
+    public static Command getSuperStructureIdleCommand(RobotContainer robot) {
+        return robot.flyWheels.getFlyWheelsDefaultCommand()
+                .alongWith(robot.pitch.run(robot.pitch::runIdle));
     }
 }

example/5516-2024-OffSeason/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java

@@ -8,6 +8,7 @@
 import frc.robot.constants.DriveControlLoops;
 import frc.robot.subsystems.drive.HolonomicDriveSubsystem;
 import frc.robot.utils.CustomPIDs.MaplePIDController;
+import org.littletonrobotics.junction.Logger;
 
 import java.util.function.Supplier;
 
@@ -37,6 +38,7 @@ public void initialize() {
     public void execute() {
         final double rotationFeedBack = chassisRotationController.calculate(driveSubsystem.getFacing().getRadians(), targetRotationSupplier.get().getRadians());
         driveSubsystem.runRobotCentricChassisSpeeds(new ChassisSpeeds(0, 0, rotationFeedBack));
+        Logger.recordOutput("FaceToRotationCommand/ErrorDegrees", getError().getDegrees());
     }
 
     @Override
@@ -46,7 +48,11 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        return driveSubsystem.getFacing().minus(targetRotationSupplier.get()).getRadians() < tolerance.getRadians();
+        return Math.abs(getError().getRadians()) < tolerance.getRadians();
+    }
+
+    private Rotation2d getError() {
+        return driveSubsystem.getFacing().minus(targetRotationSupplier.get());
     }
 
     public static ChassisFaceToRotation faceToTarget(HolonomicDriveSubsystem driveSubsystem, Supplier<Translation2d> targetPositionSupplier) {

example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java

@@ -60,10 +60,6 @@ public AimAndShootSequence(
         super.addRequirements(pitch, flyWheels, intake);
         super.addCommands(Commands.runOnce(intake::runIdle));
 
-//        super.addCommands(
-//                new PrepareToAim(flyWheels, pitch, shooterOptimization, ledStatusLight,  robotScoringPositionSupplier, targetPositionSupplier)
-//                        .untilReady()
-//        );
         final AimAtSpeakerContinuously aimAtSpeakerContinuously = new AimAtSpeakerContinuously(
                 flyWheels, pitch, ledStatusLight, shooterOptimization, drive, targetPositionSupplier, externalShootCondition
         );
@@ -81,7 +77,7 @@ public AimAndShootSequence(
         final Command visualizeNoteFullCommand = Commands
                 .waitUntil(() -> !intake.isNotePresent())
                 .andThen(visualizeNote)
-                .onlyIf(() -> intake.isNotePresent());
+                .onlyIf(intake::isNotePresent);
         final Command waitForRightTimingAndShoot = Commands.waitUntil(aimAtSpeakerContinuously::readyToShoot)
                 .andThen(intake.executeLaunch().alongWith(visualizeNoteFullCommand));
         super.addCommands(aimAtSpeakerContinuously.raceWith(waitForRightTimingAndShoot));

example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAtSpeakerFactory.java

@@ -28,6 +28,10 @@ public static Command shootAtSpeakerStill(HolonomicDriveSubsystem drive, Intake
         final Command aimAtSpeakerStill = semiAutoAimAndShoot.deadlineWith(chassisAimAtSpeaker);
         aimAtSpeakerStill.addRequirements(drive, pitch, flyWheels);
 
-        return aimAtSpeakerStill.onlyIf(intake::isNotePresent);
+        return aimAtSpeakerStill.onlyIf(intake::isNotePresent).finallyDo(() -> {
+            pitch.runIdle();
+            intake.runIdle();
+            flyWheels.runIdle();
+        });
     }
 }

example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/FollowPathGrabAndShootStill.java

@@ -26,6 +26,8 @@ public FollowPathGrabAndShootStill(PathPlannerPath pathAtBlueAlliance, double di
                 () -> MaplePathPlannerLoader.getEndingRobotPoseInCurrentAllianceSupplier(pathAtBlueAlliance).get().getTranslation()
                         .getDistance(driveSubsystem.getPose().getTranslation()) < distanceToTargetMetersStartPreparing
                 )
+                .deadlineWith(pitch.run(pitch::runIdle))
+                .deadlineWith(flyWheels.getFlyWheelsDefaultCommand())
                 .andThen(new PrepareToAim(
                         flyWheels, pitch, shooterOptimization, statusLight,
                         () -> MaplePathPlannerLoader.getEndingRobotPoseInCurrentAllianceSupplier(pathAtBlueAlliance).get().getTranslation(),
@@ -39,5 +41,11 @@ public FollowPathGrabAndShootStill(PathPlannerPath pathAtBlueAlliance, double di
         super.addCommands(AimAtSpeakerFactory.shootAtSpeakerStill(
                 driveSubsystem, intake, pitch, flyWheels, shooterOptimization, statusLight, visualizer
         ));
+
+        super.addCommands(Commands.runOnce(() -> {
+            intake.runIdle();
+            flyWheels.runIdle();
+            pitch.runIdle();
+        }, intake, pitch, flyWheels));
     }
 }

example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveControlLoops.java

@@ -40,8 +40,8 @@ public class DriveControlLoops {
             12/DriveTrainConstants.CHASSIS_MAX_VELOCITY
     );
     public static final MaplePIDController.MaplePIDConfig DRIVE_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig(
-            5,
-            2,
+            3,
+            3,
             0,
             0,
             0,