Merge branch 'main' of https://github.com/KHS-Robotics/Demonator13

KHS-Robotics · Jan 31, 2024 · b1a3055 · b1a3055
2 parents 6f1053b + cffc78a
commit b1a3055
Show file tree

Hide file tree

Showing 8 changed files with 138 additions and 64 deletions.
diff --git a/.gitignore b/.gitignore
@@ -4,6 +4,7 @@
 # Robot Simulation
 simgui-ds.json
 simgui.json
+networktables.json
 
 ### C++ ###
 # Prerequisites

diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -1,12 +1,12 @@
 {
-  "robotWidth": 0.71,
-  "robotLength": 0.71,
+  "robotWidth": 0.864,
+  "robotLength": 0.86,
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 4.5,
-  "defaultMaxAccel": 9.0,
-  "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 720.0,
+  "defaultMaxVel": 3.5,
+  "defaultMaxAccel": 6.0,
+  "defaultMaxAngVel": 360.0,
+  "defaultMaxAngAccel": 540.0,
   "maxModuleSpeed": 4.5
 }
diff --git a/networktables.json b/networktables.json
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -5,12 +5,12 @@
 
 package frc.robot;
 
+import edu.wpi.first.util.datalog.StringLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.subsystems.drive.SwerveDrive;
 
 /**
  * The VM is configured to automatically run this class, and to call the methods
@@ -25,6 +25,10 @@ public class Robot extends TimedRobot {
   private RobotContainer robotContainer;
   private Command autonomousRoutine;
 
+  private StringLogEntry LogCmdStarted = new StringLogEntry(DataLogManager.getLog(), "/commands/started");
+  private StringLogEntry LogCmdInterrupted = new StringLogEntry(DataLogManager.getLog(), "/commands/interrupted");
+  private StringLogEntry LogCmdEnded = new StringLogEntry(DataLogManager.getLog(), "/commands/ended");
+
   /**
    * This method is run when the robot is first started up and should be used for
    * any
@@ -44,11 +48,21 @@ public void robotInit() {
     robotContainer = RobotContainer.getInstance();
 
     // for debugging
-    CommandScheduler.getInstance()
-        .onCommandInitialize((command) -> System.out.println(command.getName() + " starting..."));
-    CommandScheduler.getInstance()
-        .onCommandInterrupt((command) -> System.out.println(command.getName() + " interrupted!"));
-    CommandScheduler.getInstance().onCommandFinish((command) -> System.out.println(command.getName() + " ended."));
+    CommandScheduler.getInstance().onCommandInitialize((command) -> {
+      var cmdName = command.getName();
+      System.out.println(cmdName + " started.");
+      LogCmdStarted.append(cmdName);
+    });
+    CommandScheduler.getInstance().onCommandInterrupt((command) -> {
+      var cmdName = command.getName();
+      System.out.println(cmdName + " interrupted.");
+      LogCmdInterrupted.append(cmdName);
+    });
+    CommandScheduler.getInstance().onCommandFinish((command) -> {
+      var cmdName = command.getName();
+      System.out.println(cmdName + " ended.");
+      LogCmdEnded.append(cmdName);
+    });
   }
 
   /**

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -16,6 +16,8 @@
 import com.pathplanner.lib.util.ReplanningConfig;
 
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.SerialPort.Port;
@@ -37,7 +39,6 @@
 public class RobotContainer {
   private static RobotContainer instance;
   public static AutoBuilder autoBuilder;
-  private static SendableChooser<Command> autoChooser;
 
   public static RobotContainer getInstance() {
     if (instance == null) {
@@ -46,6 +47,11 @@ public static RobotContainer getInstance() {
     return instance;
   }
 
+  private static SendableChooser<Command> autoChooser;
+  public Command getAutonomousCommand() {
+    return autoChooser.getSelected();
+  }
+
   public static final AHRS navx = new AHRS(Port.kMXP);
 
   /**
@@ -64,6 +70,10 @@ public static double getRobotPitch() {
     return navx.getPitch();
   }
 
+  /**
+   * Returns the roll angle of the robot in degrees. This tracks the
+   * left/right tilt of the robot.
+   */
   public static double getRobotRoll() {
     return navx.getRoll();
   }
@@ -130,7 +140,7 @@ private void configureXboxControllerBindings() {
     }));
     slowDrive.onFalse(new InstantCommand(() -> {
       SwerveDrive.kMaxAngularSpeedRadiansPerSecond = 2 * Math.PI;
-      SwerveDrive.kMaxSpeedMetersPerSecond = 3.5;
+      SwerveDrive.kMaxSpeedMetersPerSecond = 4.5;
     }));
 
     Trigger pointToNote = driverController.leftBumper();
@@ -185,16 +195,32 @@ private void configureAutonmousChooser() {
   }
 
   private void configureFieldLogging() {
+    var logPPCurrentPoseX = new DoubleLogEntry(DataLogManager.getLog(), "/pathplanner/current/x");
+    var logPPCurrentPoseY = new DoubleLogEntry(DataLogManager.getLog(), "/pathplanner/current/y");
+    var logPPCurrentPoseTheta = new DoubleLogEntry(DataLogManager.getLog(), "/pathplanner/current/theta");
+
     // Logging callback for current robot pose
     PathPlannerLogging.setLogCurrentPoseCallback((pose) -> {
       // Do whatever you want with the pose here
       field.setRobotPose(pose);
+
+      logPPCurrentPoseX.append(pose.getX());
+      logPPCurrentPoseY.append(pose.getY());
+      logPPCurrentPoseTheta.append(pose.getRotation().getDegrees());
     });
 
+    var logPPTargetPoseX = new DoubleLogEntry(DataLogManager.getLog(), "/pathplanner/target/x");
+    var logPPTargetPoseY = new DoubleLogEntry(DataLogManager.getLog(), "/pathplanner/target/y");
+    var logPPTargetPoseTheta = new DoubleLogEntry(DataLogManager.getLog(), "/pathplanner/target/theta");
+
     // Logging callback for target robot pose
     PathPlannerLogging.setLogTargetPoseCallback((pose) -> {
       // Do whatever you want with the pose here
       field.getObject("target pose").setPose(pose);
+
+      logPPTargetPoseX.append(pose.getX());
+      logPPTargetPoseY.append(pose.getY());
+      logPPTargetPoseTheta.append(pose.getRotation().getDegrees());
     });
 
     // Logging callback for the active path, this is sent as a list of poses
@@ -204,10 +230,6 @@ private void configureFieldLogging() {
     });
   }
 
-  public Command getAutonomousCommand() {
-    return autoChooser.getSelected();
-  }
-
   private void registerNamedCommands() {
     registerNamedCommand(new ShootSpeaker());
     registerNamedCommand("print", new PrintCommand("asd"));

diff --git a/src/main/java/frc/robot/subsystems/cameras/NoteDetectorCamera.java b/src/main/java/frc/robot/subsystems/cameras/NoteDetectorCamera.java
@@ -25,7 +25,7 @@ public NoteDetectorCamera(String cameraName, Transform3d cameraOffset) {
     this.cameraOffset = cameraOffset;
     notes = new ArrayList<>();
     notes.add(new Note(new Translation2d(0, 0)));
-    
+
   }
 
   public List<PhotonTrackedTarget> getTargets() {
@@ -54,13 +54,15 @@ public Translation2d estimateNotePose(PhotonTrackedTarget target) {
     double phi = -Math.toRadians(target.getYaw());
     double theta = (Math.PI / 2) - cameraOffset.getRotation().getY() - Math.toRadians(target.getPitch());
 
-    Translation3d targetVector = new Translation3d(Math.sin(theta) * Math.cos(phi), Math.sin(theta) * Math.sin(phi), Math.cos(theta));
+    Translation3d targetVector = new Translation3d(Math.sin(theta) * Math.cos(phi), Math.sin(theta) * Math.sin(phi),
+        Math.cos(theta));
     double z = cameraOffset.getZ();
     double t = -z / targetVector.getZ();
 
     Translation2d noteCameraRelative = new Translation2d(targetVector.getX() * t, targetVector.getY() * t);
     Translation2d noteRobotRelative = noteCameraRelative.plus(cameraOffset.getTranslation().toTranslation2d());
-    Translation2d noteFieldRelative = noteRobotRelative.rotateBy(robotPose.getRotation()).plus(robotPose.getTranslation());
+    Translation2d noteFieldRelative = noteRobotRelative.rotateBy(robotPose.getRotation())
+        .plus(robotPose.getTranslation());
 
     return noteFieldRelative;
   }
@@ -116,5 +118,4 @@ public void periodic() {
     RobotContainer.field.getObject("note").setPoses(allNotePoses);
   }
 
-
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java
@@ -24,13 +24,14 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotMap;
 import frc.robot.Constants;
-import frc.robot.Robot;
 import frc.robot.RobotContainer;
 
 /**
@@ -48,8 +49,14 @@ public class SwerveDrive extends SubsystemBase {
   private final Translation2d rearLeftLocation = new Translation2d(-0.2921, 0.2921);
   private final Translation2d rearRightLocation = new Translation2d(-0.2921, -0.2921);
 
-
-  private boolean loggedPoseError = false;
+  private static final DoubleLogEntry LogLinearSpeed = new DoubleLogEntry(DataLogManager.getLog(), "/subsystems/swerve/speed");
+  private static final DoubleLogEntry LogAngularSpeed = new DoubleLogEntry(DataLogManager.getLog(), "/subsystems/swerve/omega");
+  private static final DoubleLogEntry LogOdometryX = new DoubleLogEntry(DataLogManager.getLog(), "/odometry/pose/x");
+  private static final DoubleLogEntry LogOdometryY = new DoubleLogEntry(DataLogManager.getLog(), "/odometry/pose/y");
+  private static final DoubleLogEntry LogOdometryTheta = new DoubleLogEntry(DataLogManager.getLog(), "/odometry/pose/theta");
+  private static final DoubleLogEntry LogNavxYaw = new DoubleLogEntry(DataLogManager.getLog(), "/navx/yaw");
+  private static final DoubleLogEntry LogNavxRoll = new DoubleLogEntry(DataLogManager.getLog(), "/navx/roll");
+  private static final DoubleLogEntry LogNavxPitch = new DoubleLogEntry(DataLogManager.getLog(), "/navx/pitch");
 
   public static final SwerveModule frontLeft = new SwerveModule(
     "FL",
@@ -68,7 +75,7 @@ public class SwerveDrive extends SubsystemBase {
     Constants.FRONT_LEFT_PIVOT_OFFSET_DEGREES
   );
   public static final SwerveModule frontRight = new SwerveModule(
-      "FR",
+    "FR",
     RobotMap.FRONT_RIGHT_DRIVE,
     RobotMap.FRONT_RIGHT_PIVOT,
     Constants.PIVOT_P,
@@ -81,7 +88,8 @@ public class SwerveDrive extends SubsystemBase {
     Constants.DRIVE_KV,
     Constants.DRIVE_KA,
     RobotMap.FRONT_RIGHT_PIVOT_ENCODER,
-    Constants.FRONT_RIGHT_PIVOT_OFFSET_DEGREES);
+    Constants.FRONT_RIGHT_PIVOT_OFFSET_DEGREES
+  );
   public static final SwerveModule rearLeft = new SwerveModule(
     "RL",
     RobotMap.REAR_LEFT_DRIVE,
@@ -96,9 +104,10 @@ public class SwerveDrive extends SubsystemBase {
     Constants.DRIVE_KV,
     Constants.DRIVE_KA,
     RobotMap.REAR_LEFT_PIVOT_ENCODER,
-    Constants.REAR_LEFT_PIVOT_OFFSET_DEGREES);
+    Constants.REAR_LEFT_PIVOT_OFFSET_DEGREES
+  );
   public static final SwerveModule rearRight = new SwerveModule(
-      "RR",
+    "RR",
     RobotMap.REAR_RIGHT_DRIVE,
     RobotMap.REAR_RIGHT_PIVOT,
     Constants.PIVOT_P,
@@ -111,7 +120,8 @@ public class SwerveDrive extends SubsystemBase {
     Constants.DRIVE_KV,
     Constants.DRIVE_KA,
     RobotMap.REAR_RIGHT_PIVOT_ENCODER,
-    Constants.REAR_RIGHT_PIVOT_OFFSET_DEGREES);
+    Constants.REAR_RIGHT_PIVOT_OFFSET_DEGREES
+  );
 
   public final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(frontLeftLocation,
       frontRightLocation, rearLeftLocation, rearRightLocation);
@@ -252,20 +262,8 @@ public Pose2d getPose() {
    * 
    */
   public void updateOdometry() {
-    var hasValidMeasurements = true;
     var modulePositions = getSwerveModulePositions();
-    for (var i = 0; i < modulePositions.length; i++) {
-      var pos = modulePositions[i];
-      if (Double.isNaN(pos.distanceMeters) || Double.isNaN(pos.angle.getRadians())) {
-        DriverStation.reportError("Rejected module state! (index=" + i + ")", false);
-        hasValidMeasurements = false;
-        break;
-      }
-    }
-
-    if (hasValidMeasurements) {
-      poseEstimator.updateWithTime(Timer.getFPGATimestamp(), getAngle(), modulePositions);
-    }
+    poseEstimator.updateWithTime(Timer.getFPGATimestamp(), getAngle(), modulePositions);
 
     // Optional<EstimatedRobotPose> estimatedFrontPose = RobotContainer.frontAprilTagCamera.getEstimatedGlobalPose();
     // if (estimatedFrontPose.isPresent()) {
@@ -304,16 +302,6 @@ public void updateOdometry() {
     //     poseEstimator.addVisionMeasurement(estimatedRearPose.get().estimatedPose.toPose2d(), estimatedRearPose.get().timestampSeconds);
     //   }
 
-    var pose = poseEstimator.getEstimatedPosition();
-
-    if (!this.loggedPoseError && (Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()))) {
-      this.loggedPoseError = true;
-      for(var pos : modulePositions) {
-        DriverStation.reportError("Bad module state! Check output for details.", false);
-      }
-    }
-
-
   }
 
 
@@ -395,9 +383,30 @@ public Pose2d getPoseInverted() {
   @Override
   public void periodic() {
     updateOdometry();
-    RobotContainer.field.setRobotPose(getPose());
-    SmartDashboard.putNumber("Navx-Yaw", RobotContainer.getRobotYaw());
-    SmartDashboard.putNumber("Navx-Roll", RobotContainer.getRobotRoll());
-    SmartDashboard.putNumber("Navx-Pitch", RobotContainer.getRobotPitch());
+
+    var pose = getPose();
+    RobotContainer.field.setRobotPose(pose);
+
+    var yaw = RobotContainer.getRobotYaw();
+    var roll = RobotContainer.getRobotRoll();
+    var pitch = RobotContainer.getRobotPitch();
+    SmartDashboard.putNumber("Navx-Yaw", yaw);
+    SmartDashboard.putNumber("Navx-Roll", roll);
+    SmartDashboard.putNumber("Navx-Pitch", pitch);
+
+    if (RobotState.isEnabled()) {
+      var chassis = getChassisSpeeds();
+      var speed = Math.hypot(chassis.vxMetersPerSecond, chassis.vyMetersPerSecond);
+      LogLinearSpeed.append(speed);
+      var omega = chassis.omegaRadiansPerSecond;
+      LogAngularSpeed.append(omega);
+
+      LogOdometryX.append(pose.getX());
+      LogOdometryY.append(pose.getY());
+      LogOdometryTheta.append(pose.getRotation().getDegrees());
+      LogNavxYaw.append(yaw);
+      LogNavxRoll.append(roll);
+      LogNavxPitch.append(pitch);
+    }
   }
 }