Skip to content

Commit

Permalink
SysId stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
@ColinH0
ColinH0 committed Sep 19, 2024
1 parent 6a55ffb commit 3e00609
Show file tree
Hide file tree
Showing 8 changed files with 169 additions and 50 deletions.
41 changes: 41 additions & 0 deletions .DataLogTool/datalogtool-window.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
{
"MainWindow": {
"GLOBAL": {
"fps": "120",
"height": "510",
"maximized": "0",
"style": "0",
"userScale": "2",
"width": "925",
"xpos": "-1",
"ypos": "-1"
}
},
"Window": {
"Debug##Default": {
"Collapsed": "0",
"Pos": "60,60",
"Size": "400,400"
},
"Download": {
"Collapsed": "0",
"Pos": "0,250",
"Size": "375,260"
},
"Entries": {
"Collapsed": "0",
"Pos": "380,20",
"Size": "540,365"
},
"Input Files": {
"Collapsed": "0",
"Pos": "0,20",
"Size": "375,230"
},
"Output": {
"Collapsed": "0",
"Pos": "380,390",
"Size": "540,120"
}
}
}
1 change: 1 addition & 0 deletions .DataLogTool/datalogtool.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
{}
56 changes: 56 additions & 0 deletions .SysId/sysid-window.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,56 @@
{
"MainWindow": {
"GLOBAL": {
"fps": "120",
"height": "720",
"maximized": "0",
"style": "0",
"userScale": "2",
"width": "1280",
"xpos": "115",
"ypos": "138"
}
},
"Window": {
"###Analyzer": {
"Collapsed": "0",
"Pos": "320,25",
"Size": "360,550"
},
"###Data Selector": {
"Collapsed": "0",
"Pos": "5,480",
"Size": "310,235"
},
"###Log Loader": {
"Collapsed": "0",
"Pos": "5,25",
"Size": "310,450"
},
"###Program Log": {
"Collapsed": "0",
"Pos": "323,581",
"Size": "360,135"
},
"Debug##Default": {
"Collapsed": "0",
"Pos": "60,60",
"Size": "400,400"
},
"Diagnostic Plots": {
"Collapsed": "0",
"Pos": "685,25",
"Size": "590,690"
},
"Exception Caught!": {
"Collapsed": "0",
"Pos": "285,310",
"Size": "480,97"
},
"Override Units": {
"Collapsed": "0",
"Pos": "480,288",
"Size": "320,144"
}
}
}
1 change: 1 addition & 0 deletions .SysId/sysid.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
{}
Binary file removed logs/Log_f14a7d8ba8a7048a.wpilog
View file
Binary file not shown.
77 changes: 44 additions & 33 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@

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.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
Expand Down Expand Up @@ -61,19 +59,20 @@ public RobotContainer() {
switch (Constants.currentMode) {
case REAL:
// Real robot, instantiate hardware IO implementations
drive = Drive.initialize(
new GyroIOPigeon2(),
drive =
Drive.initialize(
new GyroIOPigeon2(),
new ModuleIOSparkMax(0),
new ModuleIOSparkMax(1),
new ModuleIOSparkMax(2),
new ModuleIOSparkMax(3));
// new Drive(
// new GyroIOPigeon2(),
// new ModuleIOSparkMax(0),
// new ModuleIOSparkMax(1),
// new ModuleIOSparkMax(2),
// new ModuleIOSparkMax(3));

// new Drive(
// new GyroIOPigeon2(),
// new ModuleIOSparkMax(0),
// new ModuleIOSparkMax(1),
// new ModuleIOSparkMax(2),
// new ModuleIOSparkMax(3));

flywheel = new Flywheel(new FlywheelIOSparkMax());
// drive = new Drive(
Expand Down Expand Up @@ -129,16 +128,19 @@ public RobotContainer() {
"Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
autoChooser.addOption(
"Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
autoChooser.addOption(
"Flywheel SysId (Quasistatic Forward)",
flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
autoChooser.addOption(
"Flywheel SysId (Quasistatic Reverse)",
flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
autoChooser.addOption(
"Flywheel SysId (Dynamic Forward)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward));
autoChooser.addOption(
"Flywheel SysId (Dynamic Reverse)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse));

// autoChooser.addOption(
// "Flywheel SysId (Quasistatic Forward)",
// flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
// autoChooser.addOption(
// "Flywheel SysId (Quasistatic Reverse)",
// flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
// autoChooser.addOption(
// "Flywheel SysId (Dynamic Forward)",
// flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward));
// autoChooser.addOption(
// "Flywheel SysId (Dynamic Reverse)",
// flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse));

// Configure the button bindings
configureButtonBindings();
Expand All @@ -157,24 +159,33 @@ private void configureButtonBindings() {
() -> -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.x().onTrue(Commands.runOnce(drive::stopWithX, drive));

controller.leftBumper().whileTrue(drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));

controller.leftTrigger().whileTrue(drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));

controller.rightBumper().whileTrue(drive.sysIdDynamic(SysIdRoutine.Direction.kForward));

controller.rightTrigger().whileTrue(drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));

// 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));

controller.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive));
// controller.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive));
}

/**
Expand Down
22 changes: 16 additions & 6 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,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.Angle;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -52,8 +56,13 @@ public class Drive extends SubsystemBase {

private final GyroIO gyroIO;
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
private final Module[] modules = new Module[4]; // FL, FR, BL, BR
private final SysIdRoutine sysId;
private final MutableMeasure<Voltage> m_appliedVoltage = MutableMeasure.ofBaseUnits(0, Volts);
private final MutableMeasure<Angle> m_position = MutableMeasure.ofBaseUnits(0, Radians);
private final MutableMeasure<Velocity<Angle>> m_velocity =
MutableMeasure.ofBaseUnits(0, RadiansPerSecond);

private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
private Rotation2d rawGyroRotation = new Rotation2d();
Expand Down Expand Up @@ -117,18 +126,19 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
// Configure SysId
sysId =
new SysIdRoutine(
new SysIdRoutine.Config(
null,
null,
null,
(state) -> Logger.recordOutput("Drive/SysIdState", state.toString())),
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
(voltage) -> {
for (int i = 0; i < 4; i++) {
modules[i].runCharacterization(voltage.in(Volts));
}
},
null,
log -> {
log.motor("driveSparkMax")
.voltage(m_appliedVoltage.mut_replace(inputs.driveAppliedVolts, Volts))
.angularPosition(m_position.mut_replace(inputs.drivePositionRad, Radians))
.angularVelocity(m_velocity.mut_replace(inputs.driveVelocityRadPerSec, RadiansPerSecond));
},
this));
}

Expand Down
21 changes: 10 additions & 11 deletions src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,15 +13,14 @@

package frc.robot.subsystems.drive;

import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.RobotController;

/**
* Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO
Expand All @@ -45,7 +44,7 @@ public class ModuleIOSparkMax implements ModuleIO {

private final RelativeEncoder driveEncoder;
private final RelativeEncoder turnRelativeEncoder;
private final AnalogInput turnAbsoluteEncoder;
private final AbsoluteEncoder turnAbsoluteEncoder;

private final boolean isTurnMotorInverted = true;
private final Rotation2d absoluteEncoderOffset;
Expand All @@ -55,25 +54,25 @@ public ModuleIOSparkMax(int index) {
case 0:
driveSparkMax = new CANSparkMax(1, MotorType.kBrushless);
turnSparkMax = new CANSparkFlex(2, MotorType.kBrushless);
turnAbsoluteEncoder = new AnalogInput(0);
turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder();
absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
break;
case 1:
driveSparkMax = new CANSparkMax(3, MotorType.kBrushless);
turnSparkMax = new CANSparkFlex(4, MotorType.kBrushless);
turnAbsoluteEncoder = new AnalogInput(1);
turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder();
absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
break;
case 2:
driveSparkMax = new CANSparkMax(5, MotorType.kBrushless);
turnSparkMax = new CANSparkFlex(6, MotorType.kBrushless);
turnAbsoluteEncoder = new AnalogInput(2);
turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder();
absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
break;
case 3:
driveSparkMax = new CANSparkMax(7, MotorType.kBrushless);
turnSparkMax = new CANSparkFlex(8, MotorType.kBrushless);
turnAbsoluteEncoder = new AnalogInput(3);
turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder();
absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
break;
default:
Expand All @@ -83,6 +82,9 @@ public ModuleIOSparkMax(int index) {
driveSparkMax.restoreFactoryDefaults();
turnSparkMax.restoreFactoryDefaults();

turnAbsoluteEncoder.setPositionConversionFactor(2 * Math.PI); // Radians
turnAbsoluteEncoder.setVelocityConversionFactor((2 * Math.PI) / 60.0); // Radians per second

driveSparkMax.setCANTimeout(250);
turnSparkMax.setCANTimeout(250);

Expand Down Expand Up @@ -119,10 +121,7 @@ public void updateInputs(ModuleIOInputs inputs) {
inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage();
inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()};

inputs.turnAbsolutePosition =
new Rotation2d(
turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI)
.minus(absoluteEncoderOffset);
inputs.turnAbsolutePosition = Rotation2d.fromRadians(turnAbsoluteEncoder.getPosition());
inputs.turnPosition =
Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO);
inputs.turnVelocityRadPerSec =
Expand Down

0 comments on commit 3e00609

Please sign in to comment.