Merge branch 'main' into dev-arm

# Conflicts: # src/main/java/frc/robot/RobotContainer.java # src/main/java/frc/robot/subsystems/drive/Drive.java
Mechanical-Advantage · Feb 6, 2024 · 4f9c949 · 4f9c949
2 parents be7b87f + d1e91d6
commit 4f9c949
Show file tree

Hide file tree

Showing 26 changed files with 1,344 additions and 521 deletions.
diff --git a/Earthfile b/Earthfile
@@ -0,0 +1,37 @@
+VERSION 0.7
+FROM debian:bookworm-20240110
+RUN apt-get update -y
+WORKDIR /RobotCode2024
+
+javabase:
+    RUN apt-get install -y openjdk-17-jdk-headless git
+
+ccbase:
+    RUN apt-get install -y build-essential git cmake clang lld
+
+ccbase-mingw-w64:
+    FROM +ccbase
+    RUN apt-get install -y mingw-w64
+
+osxcross:
+    FROM crazymax/osxcross:13.1-r0-debian
+    SAVE ARTIFACT /osxcross
+
+robotcode2024-dependencies:
+    CACHE /root/.gradle
+    FROM +javabase
+    COPY gradle/ gradle/
+    COPY settings.gradle settings.gradle
+    COPY build.gradle build.gradle
+    COPY .wpilib .wpilib
+    COPY vendordeps vendordeps
+    COPY gradlew gradlew
+    RUN ./gradlew build --no-daemon
+    SAVE IMAGE --cache-hint
+
+robotcode2024-build:
+    CACHE /root/.gradle
+    FROM +robotcode2024-dependencies
+    COPY src src
+    RUN ./gradlew build --no-daemon
+    SAVE IMAGE --cache-hint
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -21,7 +21,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.FeedForwardCharacterization;
 import frc.robot.subsystems.apriltagvision.AprilTagVision;
@@ -56,7 +55,7 @@
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-  // Load robot state as field
+  // Load robot state
   private final RobotState robotState = RobotState.getInstance();
 
   // Subsystems
@@ -85,12 +84,14 @@ public RobotContainer() {
                     new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]),
                     new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]),
                     new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]),
-                    new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]));
-            aprilTagVision =
-                new AprilTagVision(
-                    new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0]));
-            shooter = new Shooter(new ShooterIOSparkMax());
-            intake = new Intake(new IntakeIOSparkMax());
+                    new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]),
+                    false);
+            //            aprilTagVision =
+            //                new AprilTagVision(
+            //                    new
+            // AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0]));
+            //            shooter = new Shooter(new ShooterIOSparkMax());
+            //            intake = new Intake(new IntakeIOSparkMax());
             arm = new Arm(new ArmIOKrakenFOC());
           }
         }
@@ -103,7 +104,8 @@ public RobotContainer() {
                 new ModuleIOSim(DriveConstants.moduleConfigs[0]),
                 new ModuleIOSim(DriveConstants.moduleConfigs[1]),
                 new ModuleIOSim(DriveConstants.moduleConfigs[2]),
-                new ModuleIOSim(DriveConstants.moduleConfigs[3]));
+                new ModuleIOSim(DriveConstants.moduleConfigs[3]),
+                false);
         shooter = new Shooter(new ShooterIOSim());
         intake = new Intake(new IntakeIOSim());
         arm = new Arm(new ArmIOSim());
@@ -116,7 +118,8 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {},
                 new ModuleIO() {},
-                new ModuleIO() {});
+                new ModuleIO() {},
+                false);
         shooter = new Shooter(new ShooterIO() {});
         intake = new Intake(new IntakeIO() {});
         arm = new Arm(new ArmIO() {});
@@ -130,7 +133,8 @@ public RobotContainer() {
               new ModuleIO() {},
               new ModuleIO() {},
               new ModuleIO() {},
-              new ModuleIO() {});
+              new ModuleIO() {},
+              false);
     }
 
     if (aprilTagVision == null) {
@@ -167,10 +171,7 @@ public RobotContainer() {
         new FeedForwardCharacterization(
             shooter,
             shooter::runRightCharacterizationVolts,
-            shooter::getRightCharacterizationVelocity));
-    autoChooser.addOption(
-        "Arm Quasistatic Forward", arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-    autoChooser.addOption("Arm Dynamic Forward", arm.sysIdDynamic(SysIdRoutine.Direction.kForward));
+            shooter::getRightCharacterizationVelocity));\
     autoChooser.addOption("Arm get static current", arm.getStaticCurrent());
 
     // Testing autos paths
@@ -181,7 +182,7 @@ public RobotContainer() {
               traj ->
                   Commands.runOnce(
                           () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose())))
-                      .andThen(drive.setTrajectoryCommand(traj)));
+                      .andThen(drive.followTrajectory(traj)));
         };
     final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo");
     for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) {
@@ -226,6 +227,16 @@ private void configureButtonBindings() {
                                 robotState.getEstimatedPose().getTranslation(), new Rotation2d())))
                 .ignoringDisable(true));
     controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d())));
+    controller
+        .a()
+        .whileTrue(
+            drive.autoAlignToPose(
+                () ->
+                    AllianceFlipUtil.apply(
+                        new Pose2d(
+                            FieldConstants.ampCenter.getX(),
+                            FieldConstants.ampCenter.getY() - 0.6,
+                            new Rotation2d(Math.PI / 2.0)))));
   }
 
   /**

diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
@@ -58,7 +58,6 @@ private RobotState() {
 
   /** Add odometry observation */
   public void addOdometryObservation(OdometryObservation observation) {
-    Pose2d lastOdometryPose = odometryPose;
     Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions());
     lastWheelPositions = observation.wheelPositions();
     // Check gyro connected
@@ -74,7 +73,7 @@ public void addOdometryObservation(OdometryObservation observation) {
     // Add pose to buffer at timestamp
     poseBuffer.addSample(observation.timestamp(), odometryPose);
     // Calculate diff from last odometry pose and add onto pose estimate
-    estimatedPose = estimatedPose.exp(lastOdometryPose.log(odometryPose));
+    estimatedPose = estimatedPose.exp(twist);
   }
 
   public void addVisionObservation(VisionObservation observation) {
@@ -141,6 +140,7 @@ public void resetPose(Pose2d initialPose) {
     poseBuffer.clear();
   }
 
+  @AutoLogOutput(key = "Odometry/FieldVelocity")
   public Twist2d fieldVelocity() {
     Translation2d linearFieldVelocity =
         new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation());

diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -22,8 +22,8 @@
 import frc.robot.RobotState;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveConstants;
-import frc.robot.subsystems.superstructure.ShotCalculator;
 import frc.robot.util.AllianceFlipUtil;
+import frc.robot.util.shooting.ShotCalculator;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
@@ -64,9 +64,10 @@ public static Command joystickDrive(
                   linearVelocity.getX() * DriveConstants.driveConfig.maxLinearVelocity(),
                   linearVelocity.getY() * DriveConstants.driveConfig.maxLinearVelocity(),
                   omega * DriveConstants.driveConfig.maxAngularVelocity());
-          drive.setDriveVelocity(
+          speeds =
               ChassisSpeeds.fromFieldRelativeSpeeds(
-                  speeds, RobotState.getInstance().getEstimatedPose().getRotation()));
+                  speeds, RobotState.getInstance().getEstimatedPose().getRotation());
+          drive.setVelocity(speeds);
         });
   }
 

diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.java b/src/main/java/frc/robot/commands/auto/AutoCommands.java
@@ -7,9 +7,9 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.FieldConstants;
 import frc.robot.RobotState;
-import frc.robot.subsystems.superstructure.ShotCalculator;
 import frc.robot.subsystems.superstructure.intake.Intake;
 import frc.robot.util.AllianceFlipUtil;
+import frc.robot.util.shooting.ShotCalculator;
 import java.util.function.Supplier;
 
 public class AutoCommands {

diff --git a/src/main/java/frc/robot/commands/auto/TestAutos.java b/src/main/java/frc/robot/commands/auto/TestAutos.java
@@ -65,8 +65,7 @@ public static Command fourNoteDavis(Drive drive, Intake intake) {
 
     return Commands.sequence(
         resetPoseCommand(trajectory.getStartPose()),
-        drive.setTrajectoryCommand(trajectory),
-        Commands.parallel(sequenceIntake));
+        Commands.parallel(drive.followTrajectory(trajectory), sequenceIntake));
   }
 
   private static Command resetPoseCommand(Pose2d pose) {

diff --git a/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java b/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java
@@ -22,7 +22,7 @@ public class AprilTagVisionConstants {
                       Units.inchesToMeters(9.974),
                       Units.inchesToMeters(6.839),
                       new Rotation3d(0.0, Units.degreesToRadians(-28.125), 0.0))
-                  .rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0))) // offset?
+                  .rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0)))
             };
         default -> new Pose3d[] {};
       };