BB pose estimator(from 6328) and many other stuffs

.vscode/settings.json

@@ -56,5 +56,6 @@
     "edu.wpi.first.math.proto.*",
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
-  ]
+  ],
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
 }

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

@@ -4,10 +4,19 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-import com.revrobotics.spark.config.SoftLimitConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import frc.FSLib2025.swerve.SwerveModuleConstants;
 
 public class Constants {
@@ -17,6 +26,7 @@ public static final class RobotConstants {
         public static final String CANBUS_NAME = "GTX7130";
         public static final int DRIVE_CONTROLLER_PORT = 0;
         public static final int OPERATOR_BUTTONBOX_PORT = 1;
+        public static final double DRIVE_CONTROLLER_DEADBAND = 0.1;
     }
 
     public static final class SwerveConstants {
@@ -26,47 +36,60 @@ public static final class SwerveConstants {
 
         public static final int PIGEON_ID = 40;
 
-        public static final double STEER_MOTOR_KP = 0.008;
-        public static final double STEER_MOTOR_KI = 0.05;
-        public static final double STEER_MOTOR_KD = 0.005;
+        public static final double DRIVE_MOTOR_KP = 0.0;
+        public static final double DRIVE_MOTOR_KI = 0.0;
+        public static final double DRIVE_MOTOR_KD = 0.0;
+        public static final double DRIVE_MOTOR_KS = 0.0;
+        public static final double DRIVE_MOTOR_KV = 0.0;
+        public static final double DRIVE_MOTOR_KA = 0.0;
+
+        public static final double STEER_MOTOR_KP = 0.0005;
+        public static final double STEER_MOTOR_KI = 0.000015;
+        public static final double STEER_MOTOR_KD = 0.000001;
         public static final double STEER_MOTOR_WINDUP = 0.0;
         public static final int STEER_MOTOR_LIMIT = 0;
 
         public static final double DRIVE_MOTOR_GEAR_RATIO = 6.122449;
-        public static final double DRIVE_WHEEL_DIAMETERS = 4 * 0.0254; // meters
+        public static final double STEER_MOTOR_GEAR_RATIO = 150.0 / 7.0;
+
+        public static final boolean IS_STEER_MOTOR_INVERTED = false;
+
+        public static final double DRIVE_WHEEL_DIAMETERS = 0.0964511800486235; // meters
         public static final double DRIVE_WHEEL_PERIMETER = Math.PI * DRIVE_WHEEL_DIAMETERS; // meters
 
-        public static final Translation2d[] MODULE_TRANSLATOIN_METERS = new Translation2d[] {
+        public static Rotation2d gyroYaw = new Rotation2d();
+
+        public static final Translation2d[] MODULE_TRANSLATION_METERS = new Translation2d[] {
                 new Translation2d(WHEEL_BASE / 2.0, WHEEL_BASE / 2.0),
                 new Translation2d(WHEEL_BASE / 2.0, -WHEEL_BASE / 2.0),
                 new Translation2d(-WHEEL_BASE / 2.0, -WHEEL_BASE / 2.0),
                 new Translation2d(-WHEEL_BASE / 2.0, WHEEL_BASE / 2.0)
         };
-
+        
         public static final SwerveModuleConstants MOD0_CONSTANTS = new SwerveModuleConstants();
         static {
             MOD0_CONSTANTS.DriveMotorId = 1;
             MOD0_CONSTANTS.SteerMotorId = 2;
             MOD0_CONSTANTS.CANcoderId = 0;
             MOD0_CONSTANTS.CANcoderOffset = -0.272461;
         }
-
+        
         public static final SwerveModuleConstants MOD1_CONSTANTS = new SwerveModuleConstants();
         static {
             MOD1_CONSTANTS.DriveMotorId = 11;
             MOD1_CONSTANTS.SteerMotorId = 12;
             MOD1_CONSTANTS.CANcoderId = 1;
             MOD1_CONSTANTS.CANcoderOffset = -0.123047;
         }
-
+        
         public static final SwerveModuleConstants MOD2_CONSTANTS = new SwerveModuleConstants();
         static {
             MOD2_CONSTANTS.DriveMotorId = 21;
             MOD2_CONSTANTS.SteerMotorId = 22;
             MOD2_CONSTANTS.CANcoderId = 2;
             MOD2_CONSTANTS.CANcoderOffset = 0.205322;
         }
-
+        
         public static final SwerveModuleConstants MOD3_CONSTANTS = new SwerveModuleConstants();
         static {
             MOD3_CONSTANTS.DriveMotorId = 31;
@@ -75,15 +98,26 @@ public static final class SwerveConstants {
             MOD3_CONSTANTS.CANcoderOffset = -0.119141;
         }
 
+        public static final SwerveModuleConstants[] SWERVE_MODULE_CONSTANTS = new SwerveModuleConstants[] {
+            MOD0_CONSTANTS,
+            MOD1_CONSTANTS,
+            MOD2_CONSTANTS,
+            MOD3_CONSTANTS
+        };
+
         // motor configuration
         public static final TalonFXConfiguration DRIVE_MOTOR_CONFIGURATION = new TalonFXConfiguration();
         static {
+            DRIVE_MOTOR_CONFIGURATION.CurrentLimits.SupplyCurrentLimit = 40;
+            DRIVE_MOTOR_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true;
             DRIVE_MOTOR_CONFIGURATION.Feedback.SensorToMechanismRatio = SwerveConstants.DRIVE_MOTOR_GEAR_RATIO;
         }
 
         public static final SparkMaxConfig STEER_MOTOR_CONFIGURATION = new SparkMaxConfig();
         static {
-            STEER_MOTOR_CONFIGURATION.inverted(false);
+            STEER_MOTOR_CONFIGURATION.smartCurrentLimit(30);
+            STEER_MOTOR_CONFIGURATION.voltageCompensation(12.0);
+            STEER_MOTOR_CONFIGURATION.inverted(IS_STEER_MOTOR_INVERTED);
             STEER_MOTOR_CONFIGURATION.idleMode(IdleMode.kBrake);
         }
     }
@@ -233,4 +267,14 @@ public static final class SuperstructureConstants {
     public static final class JoystickConstants {
 
     }
+
+    public static final class VisionConstants {
+        public static final String CHASSIS_CAM_NAME = "mydickisbig";
+        public static final Transform3d ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0));
+        public static final AprilTagFieldLayout TAG_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
+
+        // placeholder values
+        public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
+        public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
+    }
 }

src/main/java/frc/robot/IO/GyroIO.java

@@ -0,0 +1,28 @@
+package frc.robot.IO;
+
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+
+public interface GyroIO {
+
+  public static class GyroIOInputs implements LoggableInputs {
+    public boolean connected = false;
+    public Rotation2d yawPosition = new Rotation2d();
+    public double yawVelocityRadPerSec = 0.0;
+    @Override
+    public void toLog(LogTable table) {
+      // TODO Auto-generated method stub
+      throw new UnsupportedOperationException("Unimplemented method 'toLog'");
+    }
+    @Override
+    public void fromLog(LogTable table) {
+      // TODO Auto-generated method stub
+      throw new UnsupportedOperationException("Unimplemented method 'fromLog'");
+    }
+  }
+
+  public default void updateInputs(GyroIOInputs inputs) {}
+}

src/main/java/frc/robot/IO/GyroIOPigeon2.java

@@ -0,0 +1,33 @@
+package frc.robot.IO;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+
+/** IO implementation for Pigeon2 */
+public class GyroIOPigeon2 implements GyroIO {
+  private final Pigeon2 pigeon = new Pigeon2(40,"GTX7130");
+  private final StatusSignal<Angle> yaw = pigeon.getYaw();
+  private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
+
+  public GyroIOPigeon2() {
+    pigeon.getConfigurator().apply(new Pigeon2Configuration());
+    pigeon.getConfigurator().setYaw(0.0);
+    yaw.setUpdateFrequency(100.0);
+    yawVelocity.setUpdateFrequency(100.0);
+    pigeon.optimizeBusUtilization();
+  }
+
+  @Override
+  public void updateInputs(GyroIOInputs inputs) {
+    inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
+    inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
+    inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
+  }
+}

src/main/java/frc/robot/IO/ModuleIO.java

@@ -0,0 +1,53 @@
+// 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.IO;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public interface ModuleIO {
+    @AutoLog
+    public static class ModuleIOInputs implements LoggableInputs {
+        public double drivePositionRad = 0.0;
+        public double driveVelocityRadPerSec = 0.0;
+        public double driveAppliedVolts = 0.0;
+        public double[] driveCurrentAmps = new double[] {};
+
+        public Rotation2d turnAbsolutePosition = new Rotation2d();
+        public Rotation2d turnPosition = new Rotation2d();
+        public double turnVelocityRadPerSec = 0.0;
+        public double turnAppliedVolts = 0.0;
+        public double[] turnCurrentAmps = new double[] {};
+        @Override
+        public void toLog(LogTable table) {
+            // TODO Auto-generated method stub
+            throw new UnsupportedOperationException("Unimplemented method 'toLog'");
+        }
+
+        @Override
+        public void fromLog(LogTable table) {
+            // TODO Auto-generated method stub
+            throw new UnsupportedOperationException("Unimplemented method 'fromLog'");
+        }
+    }
+
+    /** Updates the set of loggable inputs. */
+    public default void updateInputs(ModuleIOInputs inputs) {}
+
+    /** Run the drive motor at the specified voltage. */
+    public default void setDriveVoltage(double volts) {}
+
+    /** Run the turn motor at the specified voltage. */
+    public default void setTurnVoltage(double volts) {}
+
+    /** Enable or disable brake mode on the drive motor. */
+    public default void setDriveBrakeMode(boolean enable) {}
+
+    /** Enable or disable brake mode on the turn motor. */
+    public default void setTurnBrakeMode(boolean enable) {}
+} 

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

@@ -8,30 +8,37 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.RobotConstants;
+import frc.robot.IO.GyroIOPigeon2;
+import frc.robot.commands.PathPlannerCommand;
 import frc.robot.commands.TeleopSuperstructure;
 import frc.robot.commands.TeleopSwerve;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Grabber;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.LED;
+import frc.robot.subsystems.PathPlanner;
 import frc.robot.subsystems.Superstructure;
 import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.SwerveDrive;
+import frc.robot.subsystems.SwervePoseEstimator;
 
 public class RobotContainer {
 
     private CommandXboxController m_Controller;
     private Joystick joystick;
     private static ButtonBox m_buttonbox;
+    // private final Elevator m_elevator;
+    // private final Grabber m_grabber;
+    // private final Intake m_intake;
+    // private final LED m_led;
+    // private final Superstructure m_superstructure;
+    private final SwerveDrive m_swerveD;
+    private final SwervePoseEstimator m_swervePE;
+    private final PathPlanner m_pathplanner;
 
-    private final Swerve m_swerve;
-    private final Elevator m_elevator;
-    private final Grabber m_grabber;
-    private final Intake m_intake;
-    private final LED m_led;
-    private final Superstructure m_superstructure;
-
-    private final TeleopSwerve teleopSwerve;
-    private final TeleopSuperstructure teleopSuperstructure;
+    // private final TeleopSuperstructure teleopSuperstructure;
+    private final PathPlannerCommand pathPlannerCommand;
+    private static Command driveCommand;
 
     private final SendableChooser<Command> autoChooser;
 
@@ -40,18 +47,33 @@ public RobotContainer() {
         joystick = new Joystick(RobotConstants.OPERATOR_BUTTONBOX_PORT);
         m_buttonbox = new ButtonBox(joystick);
 
-        m_swerve = Swerve.getInstance();
-        m_elevator = Elevator.getInstance();
-        m_grabber = Grabber.getInstance();
-        m_intake = Intake.getInstance();
-        m_led = LED.getInstance();
-        m_superstructure = Superstructure.getInstance();
+        m_swerveD = SwerveDrive.getInstance(
+            Swerve.getInstance(0),
+            Swerve.getInstance(1),
+            Swerve.getInstance(2),
+            Swerve.getInstance(3)
+        );
+        m_swervePE = SwervePoseEstimator.getInstance(new GyroIOPigeon2());
+        m_pathplanner = PathPlanner.getInstance();
+        // m_elevator = Elevator.getInstance();
+        // m_grabber = Grabber.getInstance();
+        // m_intake = Intake.getInstance();
+        // m_led = LED.getInstance();
+        // m_superstructure = Superstructure.getInstance();
 
-        teleopSwerve = new TeleopSwerve(m_swerve, m_Controller);
-        teleopSuperstructure = new TeleopSuperstructure();
+        // teleopSuperstructure = new TeleopSuperstructure();
+        pathPlannerCommand = new PathPlannerCommand(m_pathplanner);
+        driveCommand = TeleopSwerve.joystickDrive(
+            m_swerveD, 
+            m_swervePE, 
+            () -> -m_Controller.getLeftX(), 
+            () -> -m_Controller.getLeftY(), 
+            () -> -m_Controller.getRightX());
 
-        m_swerve.setDefaultCommand(teleopSwerve);
-        m_superstructure.setDefaultCommand(teleopSuperstructure);
+        m_swerveD.setDefaultCommand(driveCommand);
+        m_swervePE.setDefaultCommand(driveCommand);
+        // m_superstructure.setDefaultCommand(teleopSuperstructure);
+        m_pathplanner.setDefaultCommand(pathPlannerCommand);
 
         configureButtonBindings();
         autoChooser = AutoBuilder.buildAutoChooser();

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

@@ -0,0 +1,37 @@
+// 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;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.PathPlanner;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class PathPlannerCommand extends Command {
+
+  PathPlanner pathPlanner;
+
+  public PathPlannerCommand(PathPlanner pathPlanner) {
+    this.pathPlanner = pathPlanner;
+    addRequirements(pathPlanner);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // 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 false;
+  }
+}