Merge pull request #83 from 4201VitruvianBots/Dev-Auto1

Offsets/Auto updates

src/main/deploy/pathplanner/autos/Test1.auto

@@ -7,7 +7,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Test1"
+            "pathName": "4PieceL"
           }
         }
       ]

src/main/deploy/pathplanner/settings.json

@@ -1,5 +1,5 @@
 {
-  "robotWidth": 0.9398,
+  "robotWidth": 0.94,
   "robotLength": 0.9398,
   "holonomicMode": true,
   "pathFolders": [],
@@ -9,14 +9,14 @@
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 68.0,
+  "robotMass": 52.163,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.0508,
-  "driveGearing": 5.36,
+  "driveWheelRadius": 0.051,
+  "driveGearing": 5.357142857142857,
   "maxDriveSpeed": 5.96,
   "driveMotorType": "krakenX60",
-  "driveCurrentLimit": 60.0,
+  "driveCurrentLimit": 79.0,
   "wheelCOF": 1.2,
   "flModuleX": 0.34925,
   "flModuleY": 0.34925,

src/main/java/frc/robot/RobotContainer.java

@@ -196,8 +196,8 @@ private void initAutoChooser() {
     SmartDashboard.putData("Auto Mode", m_chooser);
     m_chooser.setDefaultOption("Do Nothing", new WaitCommand(0));
 
-    m_chooser.addOption("DriveForward", new DriveForward(m_swerveDrive));
-    m_chooser.addOption("TestAuto1", new TestAuto1(m_swerveDrive));
+    m_chooser.addOption("DriveForward", new DriveForward(m_swerveDrive, m_fieldSim));
+    m_chooser.addOption("TestAuto1", new TestAuto1(m_swerveDrive, m_fieldSim));
   }
 
   private void initSmartDashboard() {

src/main/java/frc/robot/commands/autos/DriveForward.java

@@ -13,10 +13,11 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import org.team4201.codex.simulation.FieldSim;
 
 public class DriveForward extends SequentialCommandGroup {
   /** Creates a new DriveForward. */
-  public DriveForward(CommandSwerveDrivetrain swerveDrive /* TODO: Add field sim */) {
+  public DriveForward(CommandSwerveDrivetrain swerveDrive, FieldSim fieldSim) {
     try {
       PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward");
 
@@ -31,6 +32,7 @@ public DriveForward(CommandSwerveDrivetrain swerveDrive /* TODO: Add field sim *
       // Add your commands in the addCommands() call, e.g.
       // addCommands(new FooCommand(), new BarCommand());
       addCommands(
+          new PlotAutoPath(swerveDrive, fieldSim, path),
           new InstantCommand( // Reset the pose of the robot to the starting pose of the path
               () -> swerveDrive.resetPose(starting_pose)),
           new InstantCommand(

src/main/java/frc/robot/commands/autos/PlotAutoPath.java

@@ -0,0 +1,69 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.autos;
+
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.util.FileVersionException;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import java.io.IOException;
+import java.util.ArrayList;
+import org.json.simple.parser.ParseException;
+import org.team4201.codex.simulation.FieldSim;
+
+public class PlotAutoPath extends Command {
+  private final CommandSwerveDrivetrain m_swerveDrive;
+  private final FieldSim m_fieldSim;
+  private final ArrayList<PathPlannerPath> m_paths = new ArrayList<>();
+  private ArrayList<Trajectory.State> m_pathPoints;
+  private final PathPlannerPath m_path;
+
+  public PlotAutoPath(CommandSwerveDrivetrain swerveDrive, FieldSim fieldSim, String pathName)
+      throws FileVersionException, IOException, ParseException {
+    this(swerveDrive, fieldSim, PathPlannerPath.fromPathFile(pathName));
+  }
+
+  public PlotAutoPath(
+      CommandSwerveDrivetrain swerveDrive, FieldSim fieldSim, PathPlannerPath path) {
+    m_swerveDrive = swerveDrive;
+    m_fieldSim = fieldSim;
+    m_path = path;
+
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(m_fieldSim);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    try {
+
+      var trajectory = m_swerveDrive.getTrajectoryUtils().getTrajectoryFromPathPlanner(m_path);
+      m_fieldSim.addTrajectory(trajectory);
+    } catch (Exception e) {
+      System.out.println("Could not plot auto path ");
+    }
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/commands/autos/TestAuto1.java

@@ -13,12 +13,18 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import org.team4201.codex.simulation.FieldSim;
 
 public class TestAuto1 extends SequentialCommandGroup {
-  /** Creates a new DriveForward. */
-  public TestAuto1(CommandSwerveDrivetrain swerveDrive /* TODO: Add field sim */) {
+  /**
+   * Creates a new DriveForward.
+   *
+   * @param m_fieldSim
+   */
+  public TestAuto1(CommandSwerveDrivetrain swerveDrive, FieldSim m_fieldSim) {
+    // TODO implement field sim
     try {
-      PathPlannerPath path = PathPlannerPath.fromPathFile("Test1");
+      PathPlannerPath path = PathPlannerPath.fromPathFile("4PieceL");
 
       var m_ppCommand = AutoBuilder.followPath(path);
 

src/main/java/frc/robot/generated/V2Constants.java

@@ -154,7 +154,7 @@ public class V2Constants {
   private static final int kFrontRightDriveMotorId = CAN.frontRightDriveMotor;
   private static final int kFrontRightSteerMotorId = CAN.frontRightTurnMotor;
   private static final int kFrontRightEncoderId = CAN.frontRightCanCoder;
-  private static final Angle kFrontRightEncoderOffset = Rotations.of(0.158203125);
+  private static final Angle kFrontRightEncoderOffset = Rotations.of(0.168212890625);
   private static final boolean kFrontRightSteerMotorInverted = true;
   private static final boolean kFrontRightEncoderInverted = false;
 
@@ -176,7 +176,7 @@ public class V2Constants {
   private static final int kBackRightDriveMotorId = CAN.backRightDriveMotor;
   private static final int kBackRightSteerMotorId = CAN.backRightTurnMotor;
   private static final int kBackRightEncoderId = CAN.backRightCanCoder;
-  private static final Angle kBackRightEncoderOffset = Rotations.of(-0.048095703125);
+  private static final Angle kBackRightEncoderOffset = Rotations.of(-0.037841796875);
   private static final boolean kBackRightSteerMotorInverted = true;
   private static final boolean kBackRightEncoderInverted = false;
 

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -34,6 +34,7 @@
 import java.util.function.Supplier;
 import org.team4201.codex.subsystems.SwerveSubsystem;
 import org.team4201.codex.utils.ModuleMap;
+import org.team4201.codex.utils.TrajectoryUtils;
 
 /**
  * Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily
@@ -65,6 +66,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Sw
   /* Keep track if we've ever applied the operator perspective before or not */
   private boolean m_hasAppliedOperatorPerspective = false;
 
+  private TrajectoryUtils m_trajectoryUtils;
+
   /** Swerve request to apply during robot-centric path following */
   private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds =
       new SwerveRequest.ApplyRobotSpeeds();
@@ -145,6 +148,17 @@ public CommandSwerveDrivetrain(
       startSimThread();
     }
     configureAutoBuilder();
+
+    try {
+      m_trajectoryUtils =
+          new TrajectoryUtils(
+              this,
+              RobotConfig.fromGUISettings(),
+              new PIDConstants(10, 0, 0),
+              new PIDConstants(7, 0, 0));
+    } catch (Exception ex) {
+      DriverStation.reportError("Failed to configure TrajectoryUtils", false);
+    }
   }
 
   public void initDriveSysid() {
@@ -341,6 +355,10 @@ private void configureAutoBuilder() {
     }
   }
 
+  public TrajectoryUtils getTrajectoryUtils() {
+    return m_trajectoryUtils;
+  }
+
   /**
    * Returns a command that applies the specified control request to this swerve drivetrain.
    *

src/main/java/frc/robot/subsystems/Vision.java

@@ -20,7 +20,6 @@
 import frc.robot.Robot;
 import frc.robot.constants.ROBOT;
 import frc.robot.constants.VISION;
-// import frc.robot.simulation.FieldSim;
 import java.util.List;
 import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;