diff --git a/.DataLogTool/datalogtool-window.json b/.DataLogTool/datalogtool-window.json new file mode 100644 index 0000000..890055b --- /dev/null +++ b/.DataLogTool/datalogtool-window.json @@ -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" + } + } +} diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.DataLogTool/datalogtool.json @@ -0,0 +1 @@ +{} diff --git a/.SysId/sysid-window.json b/.SysId/sysid-window.json new file mode 100644 index 0000000..59a50dd --- /dev/null +++ b/.SysId/sysid-window.json @@ -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" + } + } +} diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/logs/Log_f14a7d8ba8a7048a.wpilog b/logs/Log_f14a7d8ba8a7048a.wpilog deleted file mode 100644 index 95b798a..0000000 Binary files a/logs/Log_f14a7d8ba8a7048a.wpilog and /dev/null differ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bb30546..becf44d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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; @@ -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( @@ -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(); @@ -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)); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 11cbffe..9b5deb2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -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; @@ -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 m_appliedVoltage = MutableMeasure.ofBaseUnits(0, Volts); + private final MutableMeasure m_position = MutableMeasure.ofBaseUnits(0, Radians); + private final MutableMeasure> m_velocity = + MutableMeasure.ofBaseUnits(0, RadiansPerSecond); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); @@ -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)); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 8589858..22030d8 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.drive; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -20,8 +21,6 @@ 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 @@ -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; @@ -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: @@ -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); @@ -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 =