Skip to content

Commit

Permalink
Move operator perspective selection to Drivetrain Periodic
Browse files Browse the repository at this point in the history 
Some formatting cleanup for sysid
  • Loading branch information
@CoryNessCTR
CoryNessCTR committed Feb 12, 2024
1 parent eb94e7c commit 9856cc0
Show file tree
Hide file tree
Showing 2 changed files with 71 additions and 58 deletions.
110 changes: 70 additions & 40 deletions java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -84,6 +92,7 @@ public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, do
startSimThread();
}
}

public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) {
super(driveTrainConstants, modules);
configurePathPlanner();
Expand All @@ -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,
Expand All @@ -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() {
Expand All @@ -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;
});
}
}
}
19 changes: 1 addition & 18 deletions java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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");

Expand All @@ -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)));
Expand All @@ -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() {
Expand Down

0 comments on commit 9856cc0

Please sign in to comment.