Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Attempt to implement advantagekit for CTRE swerve

Browse Source
This commit is contained in:
Michael Mikovsky
2025-07-17 09:15:19 -06:00
parent 8e34bfe354
commit 2b7bb12241
20 changed files with 326 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+6 -6
View File
@@ -205,13 +205,13 @@ public class Robot extends LoggedRobot {
// }
@Override
public void simulationPeriodic() {
m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage());
// visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d());
// @Override
// public void simulationPeriodic() {
// m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage());
// // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d());
// m_robotContainer.m_robotSwerveDrive.
}
// // m_robotContainer.m_robotSwerveDrive.
// }
src/main/java/frc4388/robot/RobotContainer.java
+16 -12
View File
@@ -48,6 +48,7 @@ import frc4388.robot.constants.Constants;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.Constants.LiDARConstants;
import frc4388.robot.constants.Constants.OIConstants;
import frc4388.robot.constants.Constants.SimConstants.Mode;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
@@ -56,8 +57,8 @@ import frc4388.robot.subsystems.Elevator;
// Subsystems
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Elevator.CoordinationState;
// import frc4388.robot.subsystems.Endeffector;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
// Utilites
import frc4388.utility.DeferredBlock;
@@ -74,7 +75,7 @@ import frc4388.utility.compute.ReefPositionHelper.Side;
public class RobotContainer {
/* RobotMap */
public final RobotMap m_robotMap = new RobotMap();
public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
/* Subsystems */
public final LED m_robotLED = new LED();
@@ -83,6 +84,9 @@ public class RobotContainer {
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse");
/* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -129,7 +133,7 @@ public class RobotContainer {
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
new ParallelRaceGroup(
new WaitCommand(1),
@@ -292,7 +296,7 @@ public class RobotContainer {
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
new ParallelRaceGroup(
new WaitCommand(1),
@@ -339,7 +343,7 @@ public class RobotContainer {
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
// waitDebuger.asProxy(),
// new ParallelRaceGroup(
// new WaitCommand(1),
@@ -380,7 +384,7 @@ public class RobotContainer {
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -409,7 +413,7 @@ public class RobotContainer {
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
@@ -429,7 +433,7 @@ public class RobotContainer {
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -450,7 +454,7 @@ public class RobotContainer {
new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
new LidarAlign(m_robotSwerveDrive, reefLidar),
waitDebuger.asProxy(),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -558,7 +562,7 @@ public class RobotContainer {
NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive,
new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE));
new Translation2d(-1,0), new Translation2d(), reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE));
// NamedCommands.registerCommand("feed-driveback", Commands.none());
NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.isStopped()));
@@ -722,7 +726,7 @@ public class RobotContainer {
.onTrue(thrustIntake.asProxy());
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_robotMap.reefLidar));
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, reefLidar));
// ? /* Operator Buttons */
src/main/java/frc4388/robot/RobotMap.java
+95 -65
View File
@@ -19,14 +19,18 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DigitalInput;
import frc4388.robot.constants.Constants.ElevatorConstants;
import frc4388.robot.constants.Constants.LiDARConstants;
import frc4388.robot.constants.Constants.SimConstants;
import frc4388.robot.constants.Constants.VisionConstants;
import frc4388.robot.constants.DriveConstants;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.lidar.LidarIO;
import frc4388.robot.subsystems.lidar.LidarLiteV2;
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
import frc4388.robot.subsystems.swerve.SwerveIO;
import frc4388.robot.subsystems.swerve.SwervePhoenix;
import frc4388.robot.subsystems.vision.VisionIO;
import frc4388.robot.subsystems.vision.VisionPhotonvision;
import frc4388.utility.status.FaultCANCoder;
import frc4388.utility.status.FaultPhotonCamera;
import frc4388.utility.status.FaultPidgeon2;
import frc4388.utility.status.FaultTalonFX;
@@ -38,24 +42,20 @@ public class RobotMap {
// private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
// public RobotGyro gyro = new RobotGyro(m_pigeon2);
public final VisionIO leftCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME), VisionConstants.LEFT_CAMERA_POS);
public final VisionIO rightCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME), VisionConstants.RIGHT_CAMERA_POS);
public final VisionIO leftCamera;
public final VisionIO rightCamera;
// public final LiDAR lidar = new
public final LiDAR reefLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL), "Reef");
public final LiDAR reverseLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL), "Reverse");
public final LidarIO reefLidar;
public final LidarIO reverseLidar;
/* LED Subsystem */
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
/* Swreve Drive Subsystem */
public final SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrain = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
DriveConstants.DrivetrainConstants,
DriveConstants.FRONT_LEFT, DriveConstants.FRONT_RIGHT,
DriveConstants.BACK_LEFT, DriveConstants.BACK_RIGHT
);
public final SwerveIO swerveDrivetrain;
/* Elevator Subsystem */
public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
@@ -66,71 +66,101 @@ public class RobotMap {
public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
void configureDriveMotorControllers() {
// endeffector.saf
public RobotMap(SimConstants.Mode mode) {
switch (mode) {
case REAL:
// Configure cameras
PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
leftCamera = new VisionPhotonvision(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ;
rightCamera = new VisionPhotonvision(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
// Configure LiDAR
reefLidar = new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
reverseLidar = new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
// Configure swerve drive train
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
SwerveDriveConstants.DrivetrainConstants,
SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
);
swerveDrivetrain = new SwervePhoenix(swerveDrivetrainReal);
FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
FaultTalonFX.addDevice(elevator, "Elevator");
FaultTalonFX.addDevice(endeffector, "Endeffector");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
break;
// case SIM:
// break;
default:
leftCamera = new VisionIO() {};
rightCamera = new VisionIO() {};
reefLidar = new LidarIO() {};
reverseLidar = new LidarIO() {};
swerveDrivetrain = new SwerveIO() {};
break;
}
}
// public class RobotMapSim {
// public PhotonCameraSim leftCamera;
// public PhotonCameraSim rightCamera;
// }
public RobotMap() {
configureDriveMotorControllers();
// public RobotMapSim configureSim() {
// RobotMapSim sim = new RobotMapSim();
// FaultPhotonCamera.addDevice(leftCamera, "Left Camera");
// FaultPhotonCamera.addDevice(rightCamera, "Right Camera");
// // The simulated camera properties
// SimCameraProperties cameraProp = new SimCameraProperties();
// // A 640 x 480 camera with a 100 degree diagonal FOV.
// cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100));
// // Approximate detection noise with average and standard deviation error in pixels.
// cameraProp.setCalibError(0.25, 0.08);
// // Set the camera image capture framerate (Note: this is limited by robot loop rate).
// cameraProp.setFPS(20);
// // The average and standard deviation in milliseconds of image data latency.
// cameraProp.setAvgLatencyMs(35);
// cameraProp.setLatencyStdDevMs(5);
FaultPidgeon2.addDevice(swerveDrivetrain.getPigeon2(), "Gyro");
FaultTalonFX.addDevice(elevator, "Elevator");
FaultTalonFX.addDevice(endeffector, "Endeffector");
FaultTalonFX.addDevice(swerveDrivetrain.getModule(0).getDriveMotor(), "Module 0 Drive");
FaultTalonFX.addDevice(swerveDrivetrain.getModule(0).getSteerMotor(), "Module 0 Steer");
FaultCANCoder.addDevice(swerveDrivetrain.getModule(0).getEncoder(), "Module 0 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrain.getModule(1).getDriveMotor(), "Module 1 Drive");
FaultTalonFX.addDevice(swerveDrivetrain.getModule(1).getSteerMotor(), "Module 1 Steer");
FaultCANCoder.addDevice(swerveDrivetrain.getModule(1).getEncoder(), "Module 1 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrain.getModule(2).getDriveMotor(), "Module 2 Drive");
FaultTalonFX.addDevice(swerveDrivetrain.getModule(2).getSteerMotor(), "Module 2 Steer");
FaultCANCoder.addDevice(swerveDrivetrain.getModule(2).getEncoder(), "Module 2 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrain.getModule(3).getDriveMotor(), "Module 3 Drive");
FaultTalonFX.addDevice(swerveDrivetrain.getModule(3).getSteerMotor(), "Module 3 Steer");
FaultCANCoder.addDevice(swerveDrivetrain.getModule(3).getEncoder(), "Module 3 CANCoder");
}
public class RobotMapSim {
public PhotonCameraSim leftCamera;
public PhotonCameraSim rightCamera;
}
public RobotMapSim configureSim() {
RobotMapSim sim = new RobotMapSim();
// The simulated camera properties
SimCameraProperties cameraProp = new SimCameraProperties();
// A 640 x 480 camera with a 100 degree diagonal FOV.
cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100));
// Approximate detection noise with average and standard deviation error in pixels.
cameraProp.setCalibError(0.25, 0.08);
// Set the camera image capture framerate (Note: this is limited by robot loop rate).
cameraProp.setFPS(20);
// The average and standard deviation in milliseconds of image data latency.
cameraProp.setAvgLatencyMs(35);
cameraProp.setLatencyStdDevMs(5);
// sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
// sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
// // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
// // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
sim.leftCamera.enableRawStream(true);
sim.leftCamera.enableProcessedStream(true);
sim.leftCamera.enableDrawWireframe(true);
// sim.leftCamera.enableRawStream(true);
// sim.leftCamera.enableProcessedStream(true);
// sim.leftCamera.enableDrawWireframe(true);
sim.rightCamera.enableRawStream(true);
sim.rightCamera.enableProcessedStream(true);
sim.rightCamera.enableDrawWireframe(true);
// sim.rightCamera.enableRawStream(true);
// sim.rightCamera.enableProcessedStream(true);
// sim.rightCamera.enableDrawWireframe(true);
return sim;
// return sim;
}
// }
}
src/main/java/frc4388/robot/commands/MoveForTimeCommand.java
+1 -1
View File
@@ -4,7 +4,7 @@ import java.time.Instant;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.swerve.SwerveDrive;
// Command to repeat a joystick movement for a specific time.
public class MoveForTimeCommand extends Command {
src/main/java/frc4388/robot/commands/MoveUntilSuply.java
+1 -1
View File
@@ -4,7 +4,7 @@ import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.swerve.SwerveDrive;
// Command to repeat a joystick movement for a specific time.
public class MoveUntilSuply extends Command {
src/main/java/frc4388/robot/commands/alignment/DriveToReef.java
+1 -1
View File
@@ -6,7 +6,7 @@ import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.compute.ReefPositionHelper;
import frc4388.utility.compute.TimesNegativeOne;
src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java
+1 -1
View File
@@ -2,8 +2,8 @@ package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
// Command to repeat a joystick movement for a specific time.
public class DriveUntilLiDAR extends Command {
src/main/java/frc4388/robot/commands/alignment/LidarAlign.java
+1 -1
View File
@@ -8,8 +8,8 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
/* 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 LidarAlign extends Command {
src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java
+1 -1
View File
@@ -6,7 +6,7 @@ package frc4388.robot.commands.swerve;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.commands.PID;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.swerve.SwerveDrive;
public class RotateToAngle extends PID {
src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java
+1 -1
View File
@@ -6,7 +6,7 @@ import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.utility.compute.DataUtils;
import frc4388.utility.controller.VirtualController;
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java
+1 -1
View File
@@ -7,7 +7,7 @@ import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.utility.compute.DataUtils;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
src/main/java/frc4388/robot/constants/BuildConstants.java
+5 -5
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2025RidgeScape";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 171;
public static final String GIT_SHA = "3130f647c83cc82b45a5299e19108f9eec45e6f6";
public static final String GIT_DATE = "2025-07-15 11:07:01 MDT";
public static final int GIT_REVISION = 172;
public static final String GIT_SHA = "8e34bfe35427e1fb86ab2af21dd706d387bf4874";
public static final String GIT_DATE = "2025-07-15 13:42:25 MDT";
public static final String GIT_BRANCH = "advantagekit";
public static final String BUILD_DATE = "2025-07-15 13:40:35 MDT";
public static final long BUILD_UNIX_TIME = 1752608435113L;
public static final String BUILD_DATE = "2025-07-16 16:20:33 MDT";
public static final long BUILD_UNIX_TIME = 1752704433011L;
public static final int DIRTY = 1;
private BuildConstants(){}
src/main/java/frc4388/robot/subsystems/DiffDrive.java
+2 -2
View File
@@ -13,7 +13,7 @@ import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.constants.DriveConstants;
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
// import frc4388.utility.RobotGyro;
import frc4388.utility.compute.RobotTime;
import frc4388.utility.status.Status;
@@ -56,7 +56,7 @@ public class DiffDrive extends SubsystemBase implements Queryable {
@Override
public void periodic() {
if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
if (m_robotTime.m_frameNumber % SwerveDriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
updateSmartDashboard();
}
}
src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java
+3
View File
@@ -1,5 +1,7 @@
package frc4388.robot.subsystems.lidar;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.constants.Constants.LiDARConstants;
import frc4388.utility.status.Status;
@@ -23,6 +25,7 @@ public class LiDAR extends SubsystemBase implements Queryable {
@Override
public void periodic() {
io.updateInputs(state);
Logger.processInputs("LiDAR/"+name, state);
}
// @AutoLogOutput(key = "Lidar/{name}")
src/main/java/frc4388/robot/subsystems/SwerveDrive.java → src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+72 -86
View File
@@ -2,7 +2,7 @@
// 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 frc4388.robot.subsystems;
package frc4388.robot.subsystems.swerve;
import java.util.Optional;
@@ -22,7 +22,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.DriveConstants;
import frc4388.robot.subsystems.swerve.SwerveIO.SwerveState;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.status.Status;
@@ -36,18 +36,22 @@ import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.RobotConfig;
public class SwerveDrive extends SubsystemBase implements Queryable {
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
// private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
private SwerveIO io;
private SwerveStateAutoLogged state;
private Vision vision;
// @AutoLog
// public class SwerveDriveState {
public int gear_index = DriveConstants.STARTING_GEAR;
public int gear_index = SwerveDriveConstants.STARTING_GEAR;
public boolean stopped = false;
public boolean robotKnowsWhereItIs = false;
public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index];
public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED;
public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
// 25%
public double lastOdomSpeed;
@@ -58,17 +62,16 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
// }
// public SwerveDriveState state = new SwerveDriveState();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
// swerveDriveTrain) {
FaultReporter.register(this);
this.swerveDriveTrain = swerveDriveTrain;
this.io = swerveDriveTrain;
this.state = new SwerveStateAutoLogged();
this.vision = vision;
RobotConfig config;
@@ -81,12 +84,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
// DoubleSupplier a = () -> 1.d;
AutoBuilder.configure(
() -> {
return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
return getPose2d();
}, // Robot pose supplier
this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
// pose)
() -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
() -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
// Also optionally outputs individual module feedforwards
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
@@ -139,7 +142,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
public void setOdoPose(Pose2d pose) {
if (pose == null) return;
initalPose2d = pose;
swerveDriveTrain.resetPose(pose);
io.resetPose(pose);
}
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
@@ -172,12 +175,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);
// ! drift correction
if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) {
rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
rotTarget = state.currentPose.getRotation().getDegrees();
io.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
// .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
SmartDashboard.putBoolean("drift correction", false);
} else {
@@ -186,17 +192,17 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
ctrl.HeadingController.setPID(
DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
);
swerveDriveTrain.setControl(ctrl);
io.setControl(ctrl);
SmartDashboard.putBoolean("drift correction", true);
}
} else { // Create robot-relative speeds.
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
io.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(-leftStick.getY() * speedAdjust)
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
@@ -207,9 +213,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
stopped = false;
// Create robot-relative speeds.
if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
io.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
}
@@ -229,7 +235,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(rightStick.getAngle()));
@@ -243,11 +249,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(heading);
ctrl.HeadingController.setPID(
DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
);
swerveDriveTrain.setControl(ctrl);
io.setControl(ctrl);
}
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
@@ -262,31 +268,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
// );
swerveDriveTrain.setControl(ctrl);
}
public void setLimits(double limitInAmps) {
for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {
var talonFXConfigurator = module.getDriveMotor().getConfigurator();
var talonFXConfigs = new TalonFXConfiguration();
talonFXConfigurator.refresh(talonFXConfigs);
talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps;
talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10;
talonFXConfigurator.apply(talonFXConfigs);
}
io.setControl(ctrl);
}
public void activateLuigiMode() {
setLimits(20);
io.setLimits(20);
}
public void deactivateLuigiMode() {
setLimits(DriveConstants.Configurations.SLIP_CURRENT);
io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
}
public boolean rotateToTarget(double angle) {
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(0)
.withVelocityY(0)
.withTargetDirection(Rotation2d.fromDegrees(angle)));
@@ -312,7 +306,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * -speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(rot));
@@ -324,11 +318,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
}
public Pose2d getPose2d() {
return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d);
if(state.currentPose == null)
return initalPose2d;
return state.currentPose;
}
public void resetGyro() {
swerveDriveTrain.tareEverything();
io.tareEverything();
robotKnowsWhereItIs = false;
rotTarget = 0;
// vision.resetRotations();
@@ -337,7 +333,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
public void softStop() {
stopped = true;
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
io.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(0)
.withVelocityY(0)
.withRotationalRate(0)
@@ -356,14 +352,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
SmartDashboard.putNumber("RotTartget", rotTarget);
double time = Vision.getTime();
double freq = swerveDriveTrain.getOdometryFrequency();
io.updateInputs(state);
Optional<Pose2d> curpose = swerveDriveTrain.samplePoseAt(time);
Optional<Pose2d> lastPose = swerveDriveTrain.samplePoseAt(time - freq);
vision.setLastOdomPose(curpose);
setLastOdomSpeed(curpose, lastPose, freq);
vision.setLastOdomPose(state.currentPose);
setLastOdomSpeed(state.currentPose, state.lastPose, state.frequency);
if (vision.isTag()) {
Pose2d pose = vision.getPose2d();
@@ -373,66 +365,66 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees();
}
vision.addVisionMeasurement(swerveDriveTrain);
io.addVisionMeasurement(vision.getPosesToAdd());
}
// if(e.isPresent())
}
private void reset_index() {
gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
// robot start in?)
}
public void shiftDown() {
if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
reset_index(); // If outof bounds: reset index
int i = gear_index - 1;
if (i == -1)
i = 0;
setPercentOutput(DriveConstants.GEARS[i]);
setPercentOutput(SwerveDriveConstants.GEARS[i]);
gear_index = i;
}
public void shiftUp() {
if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
reset_index(); // If outof bounds: reset index
int i = gear_index + 1;
if (i == DriveConstants.GEARS.length)
i = DriveConstants.GEARS.length - 1;
setPercentOutput(DriveConstants.GEARS[i]);
if (i == SwerveDriveConstants.GEARS.length)
i = SwerveDriveConstants.GEARS.length - 1;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
gear_index = i;
}
public void setPercentOutput(double speed) {
speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
gear_index = -1;
}
public void setToSlow() {
setPercentOutput(DriveConstants.SLOW_SPEED);
setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
gear_index = 0;
}
public void setToFast() {
setPercentOutput(DriveConstants.FAST_SPEED);
setPercentOutput(SwerveDriveConstants.FAST_SPEED);
gear_index = 1;
}
public void setToTurbo() {
setPercentOutput(DriveConstants.TURBO_SPEED);
setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
gear_index = 2;
}
public void shiftUpRot() {
rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
}
public void shiftDownRot() {
rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
}
private int tmp_gear_index = DriveConstants.STARTING_GEAR;
private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR;
public void startSlowPeriod() {
tmp_gear_index = gear_index;
@@ -445,15 +437,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
}
public void endSlowPeriod() {
setPercentOutput(DriveConstants.GEARS[tmp_gear_index]);
setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
gear_index = tmp_gear_index;
}
public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
if(curPose.isPresent() && lastPose.isPresent()){
lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){
if(curPose != null && lastPose != null){
lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq;
}
}
@@ -473,11 +465,5 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
return status;
}
// Update CTRE simulation, if used.
public void updateSim(double voltage) {
swerveDriveTrain.updateSimState(0.02, voltage);
}
}
src/main/java/frc4388/robot/constants/DriveConstants.java → src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java
+2 -2
View File
@@ -1,4 +1,4 @@
package frc4388.robot.constants;
package frc4388.robot.subsystems.swerve;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Rotations;
@@ -28,7 +28,7 @@ import frc4388.utility.structs.Gains;
// No mans land
// Beware, there be dragons.
public final class DriveConstants {
public final class SwerveDriveConstants {
public static final double MAX_ROT_SPEED = Math.PI * 2;
public static final double AUTO_MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 1.0;
src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java
+33
View File
@@ -0,0 +1,33 @@
package frc4388.robot.subsystems.swerve;
import java.util.List;
import org.littletonrobotics.junction.AutoLog;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
public interface SwerveIO {
@AutoLog
public class SwerveState {
public Pose2d currentPose = null;
public Pose2d lastPose = null;
public ChassisSpeeds speeds = null;
public double frequency = 1;
}
public default void setControl(SwerveRequest ctrl) {}
public default void setLimits(double limitInAmps) {}
public default void tareEverything() {}
public default void resetPose(Pose2d pose) {}
public default void addVisionMeasurement(List<PoseObservation> poses) {}
public default void updateInputs(SwerveState state) {}
}
src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java
+65
View File
@@ -0,0 +1,65 @@
package frc4388.robot.subsystems.swerve;
import java.util.List;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
import com.ctre.phoenix6.swerve.SwerveModule;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.geometry.Pose2d;
import frc4388.robot.subsystems.lidar.LidarIO.LidarState;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
public class SwervePhoenix implements SwerveIO {
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
public SwervePhoenix(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
this.swerveDriveTrain = swerveDriveTrain;
swerveDriveTrain.getOdometryFrequency();
}
@Override
public void updateInputs(SwerveState state) {
double time = Vision.getTime();
state.frequency = swerveDriveTrain.getOdometryFrequency();
state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null);
state.lastPose = swerveDriveTrain.samplePoseAt(time - state.frequency).orElse(null);
state.speeds = swerveDriveTrain.getState().Speeds;
}
@Override
public void setControl(SwerveRequest ctrl) {
swerveDriveTrain.setControl(ctrl);
}
@Override
public void tareEverything() {
swerveDriveTrain.tareEverything();
}
@Override
public void setLimits(double limitInAmps) {
for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {
var talonFXConfigurator = module.getDriveMotor().getConfigurator();
var talonFXConfigs = new TalonFXConfiguration();
talonFXConfigurator.refresh(talonFXConfigs);
talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps;
talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10;
talonFXConfigurator.apply(talonFXConfigs);
}
}
@Override
public void addVisionMeasurement(List<PoseObservation> poses) {
for(PoseObservation pose : poses) {
swerveDriveTrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp()));
}
}
}
src/main/java/frc4388/robot/subsystems/vision/Vision.java
+10 -9
View File
@@ -1,5 +1,7 @@
package frc4388.robot.subsystems.vision;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.littletonrobotics.junction.AutoLogOutput;
@@ -43,21 +45,20 @@ public class Vision extends SubsystemBase implements Queryable {
}
}
public void addVisionMeasurement(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
// for(EstimatedRobotPose pose : poses){
//
// }
public List<PoseObservation> getPosesToAdd(){
List<PoseObservation> poses = new ArrayList<>();
for(int i = 0; i < state.length; i++) {
if(state[i].lastEstimatedPose != null) {
PoseObservation pose = state[i].lastEstimatedPose;
drivetrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp()));
poses.add(state[i].lastEstimatedPose);
}
}
return poses;
}
public void setLastOdomPose(Optional<Pose2d> pose){
if(pose.isPresent())
lastPhysOdomPose = pose.get();
public void setLastOdomPose(Pose2d pose){
if(pose != null)
lastPhysOdomPose = pose;
}
public boolean isTag(){
src/main/java/frc4388/utility/compute/TimesNegativeOne.java
+9 -9
View File
@@ -7,16 +7,16 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.constants.DriveConstants;
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
// Class that holds weather the drivers sticks should be inverted
public class TimesNegativeOne {
public static boolean XAxis = DriveConstants.INVERT_X;
public static boolean YAxis = DriveConstants.INVERT_Y;
public static boolean RotAxis = DriveConstants.INVERT_ROTATION;
public static boolean XAxis = SwerveDriveConstants.INVERT_X;
public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
public static boolean isRed = false;
public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(DriveConstants.FORWARD_OFFSET);
public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET);
private static boolean isRed() {
Optional<Alliance> alliance = DriverStation.getAlliance();
@@ -26,10 +26,10 @@ public class TimesNegativeOne {
public static void update(){
isRed = isRed();
XAxis = DriveConstants.INVERT_X ^ isRed;
YAxis = DriveConstants.INVERT_Y ^ isRed;
RotAxis = DriveConstants.INVERT_ROTATION;
ForwardOffset = Rotation2d.fromDegrees((DriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
RotAxis = SwerveDriveConstants.INVERT_ROTATION;
ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
SmartDashboard.putBoolean("Is red alliance", isRed);
}