Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add SysID Configuration mode #23

Merged
merged 6 commits into from
Jan 19, 2024
Merged

Add SysID Configuration mode #23

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
d381edc
begun to add sysid configuration commands
a1cd Jan 15, 2024
b65e195
started adding turn characterization
a1cd Jan 16, 2024
63cb7eb
modified turn characterization
a1cd Jan 17, 2024
5763b91
modified turn characterization
a1cd Jan 17, 2024
10901cb
Merge branch 'main' into 15-add-sysid-configuration-command
a1cd Jan 19, 2024
a283d2b
Merge branch 'main' into 15-add-sysid-configuration-command
a1cd Jan 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
136 changes: 104 additions & 32 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,17 +13,23 @@

package frc.robot;

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

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.*;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
import frc.robot.commands.DriveCommands;
import frc.robot.commands.FeedForwardCharacterization;
import frc.robot.subsystems.drive.*;
import frc.robot.subsystems.feeder.Feeder;
import frc.robot.subsystems.feeder.FeederIO;
Expand Down Expand Up @@ -114,47 +120,113 @@ public RobotContainer() {
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());

// Set up feedforward characterization
autoChooser.addOption(
"Drive FF Characterization",
new FeedForwardCharacterization(
drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity));
autoChooser.addOption(
"Flywheel FF Characterization",
new FeedForwardCharacterization(
flywheel, flywheel::runVolts, flywheel::getCharacterizationVelocity));
// autoChooser.addOption(
// "Drive FF Characterization",
// new FeedForwardCharacterization(
// drive, drive::runCharacterizationVolts, drive::getCharacterizationDriveVelocity));
// autoChooser.addOption(
// "Flywheel FF Characterization",
// new FeedForwardCharacterization(
// flywheel, flywheel::runVolts, flywheel::getCharacterizationDriveVelocity));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Aren't we just going un comment this out when we make autos? If we know we are going to use this at some point I'm not sure about commenting it out.

Copy link
Contributor Author

@a1cd a1cd Jan 19, 2024 •
edited by GBKP
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes, but this code will cause build to fail if it's uncommented because of changes that I made for the future. We will use this code, but changes need to be made when changes on another branch are merged


// Configure the button bindings
configureButtonBindings();
}

public static SysIDMode sysIDMode = SysIDMode.Disabled;

enum SysIDMode {
Disabled,
DriveMotors,
TurnMotors,
EverythingElse
}

/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
drive.setDefaultCommand(
DriveCommands.joystickDrive(
drive,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
controller
.b()
.onTrue(
Commands.runOnce(
() ->
drive.setPose(
new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
drive)
.ignoringDisable(true));
controller
.a()
.whileTrue(
Commands.startEnd(
() -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
switch (sysIDMode) {
case Disabled:
drive.setDefaultCommand(
DriveCommands.joystickDrive(
drive,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
controller
.b()
.onTrue(
Commands.runOnce(
() ->
drive.setPose(
new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
drive)
.ignoringDisable(true));
controller
.a()
.whileTrue(
Commands.startEnd(
() -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
break;
case DriveMotors:
drive.setDefaultCommand(
DriveCommands.joystickDrive(
drive,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
var drivetrainDriveSysID =
new SysIdRoutine(
new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)),
new Mechanism(
drive::runCharacterizationVolts,
drive::populateDriveCharacterizationData,
drive,
"DrivetrainDriveMotors"));
controller
.x()
.whileTrue(drivetrainDriveSysID.dynamic(Direction.kForward))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
controller
.y()
.whileTrue(drivetrainDriveSysID.dynamic(Direction.kReverse))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
controller
.a()
.whileTrue(drivetrainDriveSysID.quasistatic(Direction.kForward).withTimeout(2.0))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
controller
.b()
.whileTrue(drivetrainDriveSysID.quasistatic(Direction.kReverse).withTimeout(2.0))
.onFalse(Commands.runOnce(drive::stopWithX, drive));
break;
case TurnMotors:
var drivetrainTurnSysID =
new SysIdRoutine(
new Config(Voltage.per(Units.Second).of(.5), Voltage.of(8.0), Seconds.of(12.0)),
new Mechanism(
drive::runCharacterizationVolts,
drive::populateTurnCharacterizationData,
drive,
"DrivetrainDriveMotors"));
controller
.x()
.whileTrue(drivetrainTurnSysID.dynamic(Direction.kForward)
.andThen(drivetrainTurnSysID.dynamic(Direction.kReverse))
.andThen(drivetrainTurnSysID.quasistatic(Direction.kForward))
.andThen(drivetrainTurnSysID.quasistatic(Direction.kReverse))
.andThen(Commands.runOnce(drive::stopWithX, drive))
)
.onFalse(Commands.runOnce(drive::stopWithX, drive));
break;
case EverythingElse:
break;
}
}

/**
Expand Down
49 changes: 46 additions & 3 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@

package frc.robot.subsystems.drive;

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

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.pathfinding.Pathfinding;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
Expand All @@ -27,8 +29,10 @@
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.*;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.util.LocalADStarAK;
import org.littletonrobotics.junction.AutoLogOutput;
Expand Down Expand Up @@ -168,21 +172,60 @@ public void stopWithX() {
}

/** Runs forwards at the commanded voltage. */
public void runCharacterizationVolts(double volts) {
public void runCharacterizationVolts(Measure<Voltage> voltageMeasure) {
for (int i = 0; i < 4; i++) {
modules[i].runCharacterization(volts);
modules[i].runCharacterization(voltageMeasure.in(Volts));
}
}

/** Returns the average drive velocity in radians/sec. */
public double getCharacterizationVelocity() {
double driveVelocityAverage = 0.0;
for (var module : modules) {
driveVelocityAverage += module.getCharacterizationVelocity();
driveVelocityAverage += module.getCharacterizationVelocityRadPerSec();
}
return driveVelocityAverage / 4.0;
}

/** Returns the average drive velocity in radians/sec. */
public void populateDriveCharacterizationData(SysIdRoutineLog routineLog) {
Measure<Velocity<Angle>> driveVelocityAverage = RadiansPerSecond.zero();
Measure<Angle> drivePositionAverage = Radians.zero();

for (var module : modules) {
var motor = routineLog.motor("DriveMotor #" + module.getIndex());
var angularPosition = module.getCharacterizationDrivePosition();
var angularVelocity = module.getCharacterizationDriveVelocity();
motor.angularPosition(angularPosition);
motor.angularVelocity(angularVelocity);

drivePositionAverage = drivePositionAverage.plus(angularPosition);
driveVelocityAverage = driveVelocityAverage.plus(angularVelocity);
}
var averageDriveMotor = routineLog.motor("Average DriveMotor");
averageDriveMotor.angularVelocity(driveVelocityAverage.divide(4.0));
averageDriveMotor.angularPosition(drivePositionAverage.divide(4.0));
}

public void populateTurnCharacterizationData(SysIdRoutineLog routineLog) {
Measure<Velocity<Angle>> driveVelocityAverage = RadiansPerSecond.zero();
Measure<Angle> drivePositionAverage = Radians.zero();

for (var module : modules) {
var motor = routineLog.motor("TurnMotor #" + module.getIndex());
var angularPosition = module.getCharacterizationTurnPosition();
var angularVelocity = module.getCharacterizationTurnVelocity();
motor.angularPosition(angularPosition);
motor.angularVelocity(angularVelocity);

driveVelocityAverage = driveVelocityAverage.plus(angularVelocity);
drivePositionAverage = drivePositionAverage.plus(angularPosition);
}
var averageDriveMotor = routineLog.motor("Average TurnMotor");
averageDriveMotor.angularVelocity(driveVelocityAverage.divide(4.0));
averageDriveMotor.angularPosition(drivePositionAverage.divide(4.0));
}

/** Returns the module states (turn angles and drive velocities) for all of the modules. */
@AutoLogOutput(key = "SwerveStates/Measured")
private SwerveModuleState[] getModuleStates() {
Expand Down
26 changes: 25 additions & 1 deletion src/main/java/frc/robot/subsystems/drive/Module.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,9 @@
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Velocity;
import frc.robot.Constants;
import org.littletonrobotics.junction.Logger;

Expand Down Expand Up @@ -177,7 +180,28 @@ public SwerveModuleState getState() {
}

/** Returns the drive velocity in radians/sec. */
public double getCharacterizationVelocity() {
public double getCharacterizationVelocityRadPerSec() {
return inputs.driveVelocityRadPerSec;
}

/** Returns the drive velocity unitless. */
public Measure<Angle> getCharacterizationDrivePosition() {
return edu.wpi.first.units.Units.Radians.of(inputs.drivePositionRad);
}
/** Returns the turn velocity unitless. */
public Measure<Angle> getCharacterizationTurnPosition() {
return edu.wpi.first.units.Units.Radians.of(inputs.turnPosition.getRadians());
}
/** Returns the drive velocity unitless. */
public Measure<Velocity<Angle>> getCharacterizationDriveVelocity() {
return edu.wpi.first.units.Units.RadiansPerSecond.of(inputs.driveVelocityRadPerSec);
}
/** Returns the turn velocity unitless. */
public Measure<Velocity<Angle>> getCharacterizationTurnVelocity() {
return edu.wpi.first.units.Units.RadiansPerSecond.of(inputs.turnVelocityRadPerSec);
}

public int getIndex() {
return index;
}
}
Loading