Merge branch 'simulation' of https://github.com/MrTinker64/Quake into…

… simulation

src/main/java/frc/robot/Constants.java

@@ -13,7 +13,7 @@ public static final class ModuleConstants {
     public static final double kTurningEncoderRot2Rad = kTurningMotorGearRatio * 2 * Math.PI;
     public static final double kDriveEncoderRPM2MeterPerSec = kDriveEncoderRot2Meter / 60;
     public static final double kTurningEncoderRPM2RadPerSec = kTurningEncoderRot2Rad / 60;
-    public static final double kPTurning = 0.5; // *
+    public static final double kPTurning = 0.25; // *
   }
 
   public static final class DriveConstants {
@@ -46,39 +46,32 @@ public static final class DriveConstants {
     // RoboRioPort = 0
 
     public static final boolean kFrontLeftTurningEncoderReversed = true;
-    public static final boolean kBackLeftTurningEncoderReversed = true;
     public static final boolean kFrontRightTurningEncoderReversed = true;
+    public static final boolean kBackLeftTurningEncoderReversed = true;
     public static final boolean kBackRightTurningEncoderReversed = true;
 
     public static final boolean kFrontLeftDriveEncoderReversed = true;
-    public static final boolean kBackLeftDriveEncoderReversed = true;
     public static final boolean kFrontRightDriveEncoderReversed = false;
+    public static final boolean kBackLeftDriveEncoderReversed = true;
     public static final boolean kBackRightDriveEncoderReversed = false;
 
-    public static final int kFrontLeftDriveAbsoluteEncoderPort = 0;
+    public static final int kFrontLeftDriveAbsoluteEncoderPort = 1;
+    public static final int kFrontRightDriveAbsoluteEncoderPort = 0;
     public static final int kBackLeftDriveAbsoluteEncoderPort = 2;
-    public static final int kFrontRightDriveAbsoluteEncoderPort = 1;
     public static final int kBackRightDriveAbsoluteEncoderPort = 3;
 
     public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = false;
-    public static final boolean kBackLeftDriveAbsoluteEncoderReversed = false;
     public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false;
+    public static final boolean kBackLeftDriveAbsoluteEncoderReversed = false;
     public static final boolean kBackRightDriveAbsoluteEncoderReversed = false;
 
-    public static final double kFrontLeftDriveAbsoluteEncoderOffset = -0.254;
-    public static final double kBackLeftDriveAbsoluteEncoderOffset = -1.252;
-    public static final double kFrontRightDriveAbsoluteEncoderOffset = -1.816;
-    public static final double kBackRightDriveAbsoluteEncoderOffset = -4.811;
+    public static final double kFrontLeftDriveAbsoluteEncoderOffset = 3.204;
+    public static final double kBackLeftDriveAbsoluteEncoderOffset = 3.141;
+    public static final double kFrontRightDriveAbsoluteEncoderOffset = 5.969;
+    public static final double kBackRightDriveAbsoluteEncoderOffset = 4.649;
     /* */
 
     public static final double kPhysicalMaxSpeedMetersPerSecond = 13.00;
-    public static final double kPhysicalMaxAngularSpeedRadiansPerSecond = 2 * 2 * Math.PI;
-
-    // TODO Should tweak these
-    public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond / 4;
-    public static final double kTeleDriveMaxAngularSpeedRadiansPerSecond = kPhysicalMaxAngularSpeedRadiansPerSecond / 4;
-    public static final double kTeleDriveMaxAccelerationUnitsPerSecond = 3;
-    public static final double kTeleDriveMaxAngularAccelerationUnitsPerSecond = 3;
   }
 
   public static final class OperatorConstants {

src/main/java/frc/robot/Robot.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.subsystems.SwerveSubsystem;
+import edu.wpi.first.wpilibj.RobotBase;
 
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
@@ -18,7 +19,11 @@ public class Robot extends TimedRobot {
   @Override
   public void robotInit() {
     // Logging stuff
-    DataLogManager.start("logs");
+    if (RobotBase.isReal()) {
+      DataLogManager.start("/home/lvuser");
+    } else {
+      DataLogManager.start("logs");
+    }
     DataLog log = DataLogManager.getLog();
     DriverStation.startDataLog(log);
 

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

@@ -52,9 +52,9 @@ public RobotContainer() {
         configureBindings();
 
         // Add commands to the autonomous command chooser
-        m_chooser.setDefaultOption("Complex Auto", m_complexAuto);
+        m_chooser.setDefaultOption("Straight Auto", m_straightAuto);
 
-        m_chooser.addOption("Straight Auto", m_straightAuto);
+        m_chooser.addOption("Complex Auto", m_complexAuto);
 
         m_chooser.addOption("Circle Auto", m_circleAuto);
 

src/main/java/frc/robot/commands/SwerveJoystickCmd.java

@@ -43,7 +43,7 @@ public void execute() {
         ySpeed = this.applyDeadBand(ySpeed);
         turningSpeed = this.applyDeadBand(turningSpeed);
 
-        ChassisSpeeds chassisSpeeds = speedsToChassisSpeeds(xSpeed, ySpeed, turningSpeed, fieldOrientedFunction.getAsBoolean());
+        ChassisSpeeds chassisSpeeds = speedsToChassisSpeeds(xSpeed, ySpeed, turningSpeed, false);
 
         SwerveModuleState[] moduleStates = 
         DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds);

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

@@ -4,8 +4,8 @@
 
 import java.util.List;
 
-import com.ctre.phoenix.sensors.BasePigeonSimCollection;
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.ctre.phoenix6.sim.Pigeon2SimState;
+import com.ctre.phoenix6.hardware.Pigeon2;
 
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -64,8 +64,8 @@ public class SwerveSubsystem extends SubsystemBase implements AutoCloseable {
 
     private final List<SwerveModule> modules = List.of(m_frontLeft, m_frontRight, m_backLeft, m_backRight);
 
-    private WPI_Pigeon2 m_gyro = new WPI_Pigeon2(0);
-    private final BasePigeonSimCollection gyroSim = m_gyro.getSimCollection();
+    private Pigeon2 m_gyro = new Pigeon2(0);
+    private final Pigeon2SimState gyroSim = new Pigeon2SimState(m_gyro);
 
     SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(
         DriveConstants.kDriveKinematics,
@@ -81,6 +81,7 @@ public class SwerveSubsystem extends SubsystemBase implements AutoCloseable {
 
     private double[] desiredModuleStates = { 0, 0, 0, 0, 0, 0, 0, 0 };
     private double[] currentStates = { 0, 0, 0, 0, 0, 0, 0, 0 };
+    private double[] encoderOffsets = { 0, 0, 0, 0 };
 
     private final Field2d field2d = new Field2d();
     private final FieldObject2d[] modules2d = new FieldObject2d[modules.size()];
@@ -93,6 +94,9 @@ public class SwerveSubsystem extends SubsystemBase implements AutoCloseable {
     private DoubleArrayTopic m_desiredStatesTopic = inst.getDoubleArrayTopic("desired module states");
     private DoubleArrayPublisher m_desiredStatesPublisher = m_desiredStatesTopic.publish();
 
+    private DoubleArrayTopic m_encoderOffsetTopic = inst.getDoubleArrayTopic("encoder offsets");
+    private DoubleArrayPublisher m_encoderOffsetPublisher = m_encoderOffsetTopic.publish();
+
     public SwerveSubsystem() {
         /* Threads are units of code. These threads call the zeroHeading method 1 sec 
         after the robot starts without interfering with the rest of the code */
@@ -112,7 +116,7 @@ public SwerveSubsystem() {
     }
 
     // ! For testing purposes only
-    public SwerveSubsystem(WPI_Pigeon2 gyro) {
+    public SwerveSubsystem(Pigeon2 gyro) {
         m_gyro = gyro;
 
         new Thread(() -> {
@@ -167,6 +171,13 @@ public void updatePoseEstimator(){
 
         m_statesPublisher.set(currentStates);
 
+        encoderOffsets[0] = m_frontLeft.getAbsoluteEncoderRad();
+        encoderOffsets[1] = m_frontRight.getAbsoluteEncoderRad();
+        encoderOffsets[2] = m_backLeft.getAbsoluteEncoderRad();
+        encoderOffsets[3] = m_backRight.getAbsoluteEncoderRad();
+
+        m_encoderOffsetPublisher.set(encoderOffsets);
+
         for (int i = 0; i < modules.size(); i++) {
             var transform = new Transform2d(DriveConstants.kModuleOffset[i], modules.get(i).getPosition().angle);
             modules2d[i].setPose(getPose().transformBy(transform));
@@ -235,7 +246,7 @@ public void simulate(){
         SmartDashboard.putData("Module 2", m_frontRight);
         SmartDashboard.putData("Module 3", m_backLeft);
         SmartDashboard.putData("Module 4", m_backRight);
-        gyroSim.addHeading(Units.radiansToDegrees(DriveConstants.kDriveKinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond) 
+        gyroSim.addYaw(Units.radiansToDegrees(DriveConstants.kDriveKinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond) 
         * (DriveConstants.kGyroReversed ? -1.0 : 1.0) * 0.02);
         SmartDashboard.putNumber("Heading", getHeading());
     }
@@ -266,6 +277,6 @@ public void close() throws Exception {
         m_frontRight.close();
         m_backLeft.close();
         m_backRight.close();
-        m_gyro.DestroyObject();
+        m_gyro.close();
     }
 }

src/main/java/frc/robot/subsystems/modules/FakeSwerveModule.java

@@ -31,6 +31,11 @@ public SwerveModuleState getDesiredState() {
     return state;
   }
 
+  @Override
+  public double getAbsoluteEncoderRad(){
+    return 0.0;
+  }
+
   @Override
   public void resetEncoders() {}
 

src/main/java/lib/SwerveModule.java

@@ -35,7 +35,7 @@ public static SwerveModule create(int kDriveMotorId, int kTurningMotorId, boolea
 
   // double getTurningVelocity();
 
-  // double getAbsoluteEncoderRad();
+  double getAbsoluteEncoderRad();
 
   public default void stopMotors() {}