diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0bee08af..a07e95d8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DriveConstants; import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; @@ -145,12 +146,18 @@ public RobotContainer() { autoChooser = new SendableChooser<>(); // Set up feedforward characterization -// autoChooser.addOption( -// "Drive FF Characterization", -// new FeedForwardCharacterization( -// m_driveSubsystem, -// m_driveSubsystem::runCharacterizationVolts, -// m_driveSubsystem::getCharacterizationVelocity)); + autoChooser.addOption( + "Drive Quasistatic Forward", + m_driveSubsystem.runSysidQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Quasistatic Backwards", + m_driveSubsystem.runSysidQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive Dynamic Forward", + m_driveSubsystem.runSysidDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Dynamic Backwards", + m_driveSubsystem.runSysidDynamic(SysIdRoutine.Direction.kReverse)); // Configure the button bindings configureButtonBindings(); @@ -170,131 +177,49 @@ private void configureButtonBindings() { Trigger spinUpTrigger = controller.leftTrigger().and(controller.rightTrigger().negate()); Trigger shootTrigger = controller.leftTrigger().and(controller.rightTrigger()); -// intakeTrigger.whileTrue(new IntakeControlCommand(m_armSubsystem, m_shooter)); intakeTrigger.whileTrue(m_shooter.intakeCommand(0.95, 0.5, 0.1) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); -// spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false) -// .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM))); -// shootTrigger.whileTrue(m_shooter.runShooterVelocity(true) -// .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM))); - -// controller.x().whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); -// controller.y().whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), -// () -> m_shooter.setKickerPower(0.0), -// m_shooter)); - - double centerDistance = 1.34 - Units.inchesToMeters(3.0); - -// m_driveSubsystem.setDefaultCommand( -// DriveCommands.joystickDrive( -// m_driveSubsystem, -// () -> -controller.getLeftY(), -// () -> -controller.getLeftX(), -// () -> -controller.getRightX())); -// controller -// .start() -// .onTrue( -// Commands.runOnce( -// () -> -// m_driveSubsystem.setPose(AllianceFlipUtil.apply( -// new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(240.25), 5.55), -// Rotation2d.fromDegrees(180.0)))), -// m_driveSubsystem) -// .ignoringDisable(true)); + spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM))); + shootTrigger.whileTrue(m_shooter.runShooterVelocity(true) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM))); + + controller.x().whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); + controller.y().whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), + () -> m_shooter.setKickerPower(0.0), + m_shooter)); + + m_driveSubsystem.setDefaultCommand( + DriveCommands.joystickDrive( + m_driveSubsystem, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + controller + .start() + .onTrue( + Commands.runOnce( + () -> + m_driveSubsystem.setPose(AllianceFlipUtil.apply( + new Pose2d(m_driveSubsystem.getVisionPose().getTranslation(), + Rotation2d.fromDegrees(180.0)))), + m_driveSubsystem) + .ignoringDisable(true)); } /** * Use this method to configure any named commands needed for PathPlanner autos */ private void configureNamedCommands() { -// NamedCommands.registerCommand("Run Intake", Commands.run(() -> m_shooter.setIntakePower(0.5))); -// NamedCommands.registerCommand("Run Shooter", Commands.run(m_shooter::runShooterVelocity)); + NamedCommands.registerCommand("Run Intake", Commands.run(() -> m_shooter.setIntakePower(0.5))); } private void configureDashboard() { ShuffleboardTab commandTab = Shuffleboard.getTab("Commads"); -// commandTab.add("Disable Arm Brake", m_armSubsystem.enableBrakeMode(false)); -// commandTab.add("Enable Arm Brake", m_armSubsystem.enableBrakeMode(true)); -/* - commandTab.add("Center Robot Pose", Commands.runOnce( - () -> - m_driveSubsystem.setPose( - new Pose2d( - FieldConstants.FIELD_LENGTH / 2.0, - FieldConstants.FIELD_WIDTH / 2.0, - new Rotation2d())), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("CenterToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(1.34, 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - double centerDistance = 1.34 - Units.inchesToMeters(3.0); - - commandTab.add("2InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(2.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("6InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(6.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("12InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(12.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("24InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(24.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("48InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(48.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("96InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(96.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("192InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(192.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - */ + commandTab.add("Disable Arm Brake", m_armSubsystem.enableBrakeMode(false)); + commandTab.add("Enable Arm Brake", m_armSubsystem.enableBrakeMode(true)); } /** diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 09a2035a..7d87abc7 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -93,10 +93,10 @@ public void periodic() { if (m_desiredState != ArmState.DISABLED) { // check to see if the wrist is currently too close to the rest of the arm - m_io.setWristAngle(m_desiredWristPoseDegs, m_wristVelocityMult); + double predictedUnderGap = MathUtil.clamp(ArmConstants.WRIST_ARM_GAP.getValue() + - (m_desiredArmPoseDegs + m_desiredWristPoseDegs), 0, 180); - double underGap = MathUtil.clamp(ArmConstants.WRIST_ARM_GAP.getValue() - - (m_inputs.armPositionDegs + m_inputs.wristPositionDegs), 0, 180); + m_io.setWristAngle(m_desiredWristPoseDegs + predictedUnderGap, m_wristVelocityMult); // set the arms angle m_io.setArmAngle(m_desiredArmPoseDegs, m_armVelocityMult); @@ -110,8 +110,8 @@ public void periodic() { public void handleState() { switch(m_desiredState) { case STOW -> { - if (m_inputs.armPositionDegs > 60 && m_currentState == ArmState.AMP) { - m_wristVelocityMult = 0.0; + if (m_inputs.armPositionDegs > 65 && m_currentState == ArmState.AMP) { + m_wristVelocityMult = 0.25; m_armVelocityMult = 1.0; } else { m_currentState = ArmState.STOW; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 2df734be..0723f8b8 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.drive; +import com.ctre.phoenix6.SignalLogger; import com.gos.lib.properties.pid.PidProperty; import com.gos.lib.properties.pid.WpiPidPropertyBuilder; import com.pathplanner.lib.auto.AutoBuilder; @@ -27,12 +28,16 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.*; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.drive.module.Module; import frc.robot.subsystems.drive.module.ModuleIO; @@ -46,6 +51,8 @@ import lib.logger.DataLogUtil.DataLogTable; import lib.utils.PoseEstimator; +import static edu.wpi.first.units.Units.Volts; + public class DriveSubsystem extends SubsystemBase { public static final Lock odometryLock = new ReentrantLock(); @@ -76,6 +83,8 @@ public class DriveSubsystem extends SubsystemBase { private final DataLogTable m_logTable = DataLogUtil.getTable("Swerve"); + private final SysIdRoutine m_sysId; + private final SwerveModuleState[] m_measureStates = new SwerveModuleState[] { new SwerveModuleState(), new SwerveModuleState(), @@ -165,6 +174,20 @@ public DriveSubsystem( // turn on logging setupLogging(); SmartDashboard.putData("Field",m_field); + + m_sysId = new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (SysIdRoutineLog.State state) -> SignalLogger.writeString("Drive/SysidState", state.toString()) + ), + new SysIdRoutine.Mechanism( + (Measure volts) -> runCharacterizationVolts(volts.in(Volts)), + null, + this + ) + ); } @Override @@ -365,6 +388,14 @@ public static Translation2d[] getModuleTranslations() { }; } + public Command runSysidQuasistatic(SysIdRoutine.Direction direction) { + return m_sysId.quasistatic(direction); + } + + public Command runSysidDynamic(SysIdRoutine.Direction direction) { + return m_sysId.dynamic(direction); + } + public void setupLogging() { for (int i = 0; i < modules.length; i++) { Module module = modules[i]; diff --git a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java index eefebf21..d9c6aa8f 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java @@ -60,7 +60,7 @@ private PhoenixOdometryThread() { @Override public void start() { - if (timestampQueues.size() > 0) { + if (!timestampQueues.isEmpty()) { super.start(); } } @@ -138,4 +138,4 @@ public void run() { } } } -} \ No newline at end of file +}