Skip to content

Commit

Permalink
TARSReboot-add sysid code
Browse files Browse the repository at this point in the history
  • Loading branch information
@rishabhreng
rishabhreng committed Nov 19, 2024
1 parent c9e16e1 commit 74674de
Show file tree
Hide file tree
Showing 3 changed files with 183 additions and 49 deletions.
2 changes: 1 addition & 1 deletion TARSReboot/src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -237,6 +237,6 @@ public static class DrivetrainConstants {

public static final CommandSwerveDrivetrain DriveTrain =
new CommandSwerveDrivetrain(
DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
DrivetrainConstants, 250, FrontLeft, FrontRight, BackLeft, BackRight);
}
}
45 changes: 24 additions & 21 deletions TARSReboot/src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

package frc.robot;

import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
Expand All @@ -13,6 +14,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.robot.Constants.DrivetrainConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;

Expand All @@ -39,30 +41,31 @@ public class RobotContainer {
private final Telemetry logger = new Telemetry(MaxSpeed);

private void configureBindings() {
drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
drivetrain.applyRequest(
() ->
drive
.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with
// negative Y (forward)
.withVelocityY(
-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left)
.withRotationalRate(
joystick.getRightX()
* MaxAngularRate) // Drive counterclockwise with negative X (left)
));

joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
drivetrain.setDefaultCommand(
drivetrain.runSwerveFC(
() -> -joystick.getLeftY() * MaxSpeed,
() -> -joystick.getLeftX() * MaxSpeed,
() -> joystick.getRightX() * MaxAngularRate));
joystick
.b()
.whileTrue(
drivetrain.applyRequest(
() ->
point.withModuleDirection(
new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
.onTrue(
drivetrain
.runOnce(() -> drivetrain.seedFieldRelative())
.alongWith(Commands.print("Gyro reset"))
.withName("Reset Gyro"));

joystick
.back()
.onTrue(Commands.runOnce(() -> SignalLogger.stop()).andThen(Commands.print("end")));
joystick
.start()
.onTrue(Commands.runOnce(() -> SignalLogger.start()).andThen(Commands.print("start")));

// reset the field-centric heading on left bumper press
joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative()));
// SPECIFIC MODE IS DEFINED IN DRIVETRAIN SUBSYSTEM
joystick.povUp().onTrue(drivetrain.runSysId(false, Direction.kForward));
joystick.povDown().onTrue(drivetrain.runSysId(false, Direction.kReverse));
joystick.povRight().onTrue(drivetrain.runSysId(true, Direction.kForward));
joystick.povLeft().onTrue(drivetrain.runSysId(true, Direction.kReverse));

if (Utils.isSimulation()) {
drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
Expand Down
185 changes: 158 additions & 27 deletions TARSReboot/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
@@ -1,30 +1,107 @@
package frc.robot.subsystems;

import static edu.wpi.first.units.Units.*;

import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.SteerRequestType;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.PointWheelsAt;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SwerveDriveBrake;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SysIdSwerveRotation;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SysIdSwerveSteerGains;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SysIdSwerveTranslation;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;

/**
* 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 {
/**
* Enum to specify the mode of the SysId routine. Translation: System identification for
* translation. SteerGains: System identification for steer gains Rotation: System identification
* for rotation (angular motion)
*/
public static enum SysIdMode {
Translation,
SteerGains,
Rotation
}

private final SysIdMode sysIdMode = SysIdMode.Translation; // MODIFY THIS TO CHANGE SYSID MODE

private static final double kSimLoopPeriod = 0.005; // 5 ms
private Notifier m_simNotifier = null;
private double m_lastSimTime;

// Field centric request
private FieldCentric fieldCentricRequest =
new FieldCentric()
.withSteerRequestType(SteerRequestType.MotionMagic)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage)
.withDeadband(6 * 0.1)
.withRotationalDeadband(Math.PI * 0.1);

// Brake & point requests
private SwerveDriveBrake brakeRequest = new SwerveDriveBrake();
private PointWheelsAt pointWheelsAtRequest = new PointWheelsAt();

// Sysid requests
private SysIdSwerveTranslation sysIdSwerveTranslation = new SysIdSwerveTranslation();
private SysIdSwerveSteerGains sysIdSwerveSteerGains = new SysIdSwerveSteerGains();
private SysIdSwerveRotation sysIdSwerveRotation = new SysIdSwerveRotation();

private SysIdRoutine sysIdRoutineTranslation =
new SysIdRoutine(
new SysIdRoutine.Config(
Volts.of(0.5).per(Seconds.of(1)),
Volts.of(2),
Seconds.of(5),
state -> SignalLogger.writeString("stateTranslation", state.toString())),
new SysIdRoutine.Mechanism(
volts -> setControl(sysIdSwerveTranslation.withVolts(volts)), null, this));

private SysIdRoutine sysIdRoutineSteerGains =
new SysIdRoutine(
new SysIdRoutine.Config(
null,
Volts.of(4),
null,
state -> SignalLogger.writeString("stateSteerGains", state.toString())),
new SysIdRoutine.Mechanism(
volts -> setControl(sysIdSwerveSteerGains.withVolts(volts)), null, this));

private SysIdRoutine sysIdRoutineRotation =
new SysIdRoutine(
new SysIdRoutine.Config(
null,
Volts.of(4),
null,
state -> SignalLogger.writeString("stateRotationGains", state.toString())),
new SysIdRoutine.Mechanism(
volts -> setControl(sysIdSwerveRotation.withVolts(volts)), null, this));

/** Alliance logic * */
/* 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 */
Expand All @@ -40,16 +117,90 @@ public CommandSwerveDrivetrain(
}
}

public CommandSwerveDrivetrain(
SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) {
super(driveTrainConstants, modules);
if (Utils.isSimulation()) {
startSimThread();
// Handles actual requests. Abstracted through other methods.
public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
return run(() -> this.setControl(requestSupplier.get()));
}

@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;
});
}
}

public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
return run(() -> this.setControl(requestSupplier.get()));
/**
* Run the swerve drivetrain in field-centric mode.
*
* @param forward Supplier for forward movement.
* @param strafe Supplier for strafing movement.
* @param rotate Supplier for rotational movement.
* @return Command
*/
public Command runSwerveFC(DoubleSupplier forward, DoubleSupplier strafe, DoubleSupplier rotate) {
return applyRequest(
() ->
fieldCentricRequest
.withVelocityX(forward.getAsDouble())
.withVelocityY(strafe.getAsDouble())
.withRotationalRate(rotate.getAsDouble()));
}

// Brake command
public Command brake() {
return applyRequest(() -> brakeRequest);
}

// Point command
public Command pointWheelsAt(DoubleSupplier x, DoubleSupplier y) {
return applyRequest(
() ->
pointWheelsAtRequest.withModuleDirection(
new Rotation2d(x.getAsDouble(), y.getAsDouble())));
}

/**
* Command to run SysId, based on enum defined above.
*
* @param isDynamic true for dynamic, false for quasistatic.
* @param direction Direction to run the SysId in, forward or reverse.
*/
public Command runSysId(boolean isDynamic, Direction direction) {
switch (sysIdMode) {
case Translation:
return isDynamic
? sysIdRoutineTranslation.dynamic(direction)
: sysIdRoutineTranslation.quasistatic(direction);
case SteerGains:
return isDynamic
? sysIdRoutineSteerGains.dynamic(direction)
: sysIdRoutineSteerGains.quasistatic(direction);
case Rotation:
return isDynamic
? sysIdRoutineRotation.dynamic(direction)
: sysIdRoutineRotation.quasistatic(direction);
default:
return Commands.none();
}
}

private void startSimThread() {
Expand All @@ -68,24 +219,4 @@ 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;
});
}
}
}

0 comments on commit 74674de

Please sign in to comment.