diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java index 331d76a3..b99afb00 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java @@ -16,66 +16,74 @@ import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.generated.TunerConstants; /** - * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem - * so it can be used in command-based projects easily. + * Class that extends the Phoenix SwerveDrivetrain class and implements + * subsystem so it can be used in command-based projects easily. */ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { private static final double kSimLoopPeriod = 0.005; // 5 ms private Notifier m_simNotifier = null; private double m_lastSimTime; - private final SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds(); - private final SwerveRequest.SysIdSwerveTranslation translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveRotation rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - private final SwerveRequest.SysIdSwerveSteerGains steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean hasAppliedOperatorPerspective = false; + + private final SwerveRequest.ApplyChassisSpeeds AutoRequest = new SwerveRequest.ApplyChassisSpeeds(); + + private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveRotation RotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); /* Use one of these sysidroutines for your particular test */ - private SysIdRoutine m_SysIdRoutineTranslation = - new SysIdRoutine( + private SysIdRoutine SysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( - null, - Volts.of(7), - null, - (state)->SignalLogger.writeString("state", state.toString()) - ), + null, + Volts.of(7), + null, + (state) -> SignalLogger.writeString("state", state.toString())), new SysIdRoutine.Mechanism( - (volts)->setControl(translationCharacterization.withVolts(volts)), - null, - this)); + (volts) -> setControl(TranslationCharacterization.withVolts(volts)), + null, + this)); - private SysIdRoutine m_SysIdRoutineRotation = - new SysIdRoutine( + private final SysIdRoutine SysIdRoutineRotation = new SysIdRoutine( new SysIdRoutine.Config( - null, - Volts.of(7), - null, - (state)->SignalLogger.writeString("state", state.toString()) - ), + null, + Volts.of(7), + null, + (state) -> SignalLogger.writeString("state", state.toString())), new SysIdRoutine.Mechanism( - (volts)->setControl(rotationCharacterization.withVolts(volts)), - null, - this)); - private SysIdRoutine m_SysIdRoutineSteer = - new SysIdRoutine( + (volts) -> setControl(RotationCharacterization.withVolts(volts)), + null, + this)); + private final SysIdRoutine SysIdRoutineSteer = new SysIdRoutine( new SysIdRoutine.Config( - null, - Volts.of(7), - null, - (state)->SignalLogger.writeString("state", state.toString()) - ), + null, + Volts.of(7), + null, + (state) -> SignalLogger.writeString("state", state.toString())), new SysIdRoutine.Mechanism( - (volts)->setControl(steerCharacterization.withVolts(volts)), - null, - this)); + (volts) -> setControl(SteerCharacterization.withVolts(volts)), + null, + this)); + + /* Change this to the sysid routine you want to test */ + private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation; public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { super(driveTrainConstants, OdometryUpdateFrequency, modules); @@ -84,6 +92,7 @@ public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, do startSimThread(); } } + public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { super(driveTrainConstants, modules); configurePathPlanner(); @@ -102,7 +111,7 @@ private void configurePathPlanner() { ()->this.getState().Pose, // Supplier of current robot pose this::seedFieldRelative, // Consumer for seeding pose against auto this::getCurrentRobotChassisSpeeds, - (speeds)->this.setControl(autoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot + (speeds)->this.setControl(AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot new HolonomicPathFollowerConfig(new PIDConstants(10, 0, 0), new PIDConstants(10, 0, 0), TunerConstants.kSpeedAt12VoltsMps, @@ -120,12 +129,16 @@ public Command getAutoPath(String pathName) { return new PathPlannerAuto(pathName); } - /* Both the sysid commands are specific to one particular sysid routine, change which one you're trying to characterize */ + /* + * Both the sysid commands are specific to one particular sysid routine, change + * which one you're trying to characterize + */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_SysIdRoutineSteer.quasistatic(direction); + return RoutineToApply.quasistatic(direction); } + public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_SysIdRoutineSteer.dynamic(direction); + return RoutineToApply.dynamic(direction); } public ChassisSpeeds getCurrentRobotChassisSpeeds() { @@ -146,4 +159,21 @@ private void startSimThread() { }); m_simNotifier.startPeriodic(kSimLoopPeriod); } + + @Override + public void periodic() { + /* Periodically try to apply the operator perspective */ + /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ + /* This allows us to correct the perspective in case the robot code restarts mid-match */ + /* Otherwise, only check and apply the operator perspective if the DS is disabled */ + /* This ensures driving behavior doesn't change until an explicit disable event occurs during testing*/ + if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent((allianceColor) -> { + this.setOperatorPerspectiveForward( + allianceColor == Alliance.Red ? RedAlliancePerspectiveRotation + : BlueAlliancePerspectiveRotation); + hasAppliedOperatorPerspective = true; + }); + } + } } diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java index 4087ecc0..bd6a2018 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java @@ -8,12 +8,8 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.generated.TunerConstants; @@ -33,11 +29,6 @@ public class RobotContainer { private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private final Rotation2d blueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private final Rotation2d redAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); - /* Path follower */ private Command runAuto = drivetrain.getAutoPath("Tests"); @@ -58,9 +49,6 @@ private void configureBindings() { // reset the field-centric heading on left bumper press joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); - // if (Utils.isSimulation()) { - // drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); - // } drivetrain.registerTelemetry(logger::telemeterize); joystick.pov(0).whileTrue(drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0))); @@ -69,16 +57,11 @@ private void configureBindings() { /* Bindings for drivetrain characterization */ /* These bindings require multiple buttons pushed to swap between quastatic and dynamic */ + /* Back/Start select dynamic/quasistatic, Y/X select forward/reverse direction */ joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); - - /* When we get alliance data from Driver Station, forward it to the drivetrain so it knows what perspective is forward for operator control */ - new Trigger(() -> DriverStation.getAlliance().isPresent()) - .whileTrue(new RunCommand(() -> drivetrain.setOperatorPerspectiveForward( - DriverStation.getAlliance().get() == Alliance.Red ? redAlliancePerspectiveRotation - : blueAlliancePerspectiveRotation))); } public RobotContainer() {