From a6d38e56576a756b7cf10024f4d61282053d8b57 Mon Sep 17 00:00:00 2001 From: Curt Date: Sat, 7 Sep 2024 16:58:40 -0400 Subject: [PATCH 1/3] Swerve configuration test improvements Robot.java - Test mode calls RobotContainer.setDriveMode to configure different controls RobotContainer.java - Move drive options to class-level so they are accessible in configureBindings, added a new binding map for Test mode that is useful for configuring swerve SwerveSubsystem.java - Added various helper functions that can be called from RobotContainer to set up buttons for testing swerve SwerveDriveTest.java - Fix angle motor SysId test issue with reading the wrong encoder --- src/main/java/frc/robot/Robot.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 133 ++++++++++-------- .../swervedrive/SwerveSubsystem.java | 77 +++++++++- src/main/java/swervelib/SwerveDriveTest.java | 4 +- 4 files changed, 152 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 94e6ddb6..52a18c40 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -125,6 +125,8 @@ public void teleopInit() if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); + } else { + CommandScheduler.getInstance().cancelAll(); } m_robotContainer.setDriveMode(); m_robotContainer.setMotorBrake(true); @@ -143,13 +145,7 @@ public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); - try - { - new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve")); - } catch (IOException e) - { - throw new RuntimeException(e); - } + m_robotContainer.setDriveMode(); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d44e8b4a..92017828 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,8 +8,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -32,6 +35,50 @@ public class RobotContainer // The robot's subsystems and commands are defined here... private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); + // Applies deadbands and inverts controls because joysticks + // are back-right positive while robot + // controls are front-left positive + // left stick controls translation + // right stick controls the rotational velocity + // buttons are quick rotation positions to different ways to face + // WARNING: default buttons are on the same buttons as the ones defined in configureBindings + AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase, + () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), + OperatorConstants.LEFT_Y_DEADBAND), + () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), + OperatorConstants.LEFT_X_DEADBAND), + () -> -MathUtil.applyDeadband(driverXbox.getRightX(), + OperatorConstants.RIGHT_X_DEADBAND), + driverXbox.getHID()::getYButtonPressed, + driverXbox.getHID()::getAButtonPressed, + driverXbox.getHID()::getXButtonPressed, + driverXbox.getHID()::getBButtonPressed); + + // Applies deadbands and inverts controls because joysticks + // are back-right positive while robot + // controls are front-left positive + // left stick controls translation + // right stick controls the desired angle NOT angular rotation + Command driveFieldOrientedDirectAngle = drivebase.driveCommand( + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> driverXbox.getRightX(), + () -> driverXbox.getRightY()); + + // Applies deadbands and inverts controls because joysticks + // are back-right positive while robot + // controls are front-left positive + // left stick controls translation + // right stick controls the angular velocity of the robot + Command driveFieldOrientedAnglularVelocity = drivebase.driveCommand( + () -> MathUtil.applyDeadband(driverXbox.getLeftY() * -1, OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX() * -1, OperatorConstants.LEFT_X_DEADBAND), + () -> driverXbox.getRightX() * -1); + + Command driveFieldOrientedDirectAngleSim = drivebase.simDriveCommand( + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> driverXbox.getRawAxis(2)); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -40,54 +87,6 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); - - // Applies deadbands and inverts controls because joysticks - // are back-right positive while robot - // controls are front-left positive - // left stick controls translation - // right stick controls the rotational velocity - // buttons are quick rotation positions to different ways to face - // WARNING: default buttons are on the same buttons as the ones defined in configureBindings - AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase, - () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), - OperatorConstants.LEFT_Y_DEADBAND), - () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), - OperatorConstants.LEFT_X_DEADBAND), - () -> -MathUtil.applyDeadband(driverXbox.getRightX(), - OperatorConstants.RIGHT_X_DEADBAND), - driverXbox.getHID()::getYButtonPressed, - driverXbox.getHID()::getAButtonPressed, - driverXbox.getHID()::getXButtonPressed, - driverXbox.getHID()::getBButtonPressed); - - // Applies deadbands and inverts controls because joysticks - // are back-right positive while robot - // controls are front-left positive - // left stick controls translation - // right stick controls the desired angle NOT angular rotation - Command driveFieldOrientedDirectAngle = drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverXbox.getRightX(), - () -> driverXbox.getRightY()); - - // Applies deadbands and inverts controls because joysticks - // are back-right positive while robot - // controls are front-left positive - // left stick controls translation - // right stick controls the angular velocity of the robot - Command driveFieldOrientedAnglularVelocity = drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverXbox.getRightX() * 0.5); - - Command driveFieldOrientedDirectAngleSim = drivebase.simDriveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverXbox.getRawAxis(2)); - - drivebase.setDefaultCommand( - !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim); } /** @@ -99,16 +98,32 @@ public RobotContainer() */ private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - - driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); - driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); - driverXbox.b().whileTrue( - Commands.deferredProxy(() -> drivebase.driveToPose( - new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) - )); - driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2)); - // driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); + if (DriverStation.isTest()) { + driverXbox.a().onTrue(drivebase.angleMotorPValueCommand()); + driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand()); + driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); + driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2)); + driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); + driverXbox.back().whileTrue(drivebase.centerModulesCommand()); + driverXbox.leftBumper().onTrue(Commands.none()); + driverXbox.rightBumper().onTrue(Commands.none()); + drivebase.setDefaultCommand( + !RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : driveFieldOrientedDirectAngleSim); + } else { + driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); + driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); + driverXbox.b().whileTrue( + Commands.deferredProxy(() -> drivebase.driveToPose( + new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) + )); + driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2)); + driverXbox.start().whileTrue(Commands.none()); + driverXbox.back().whileTrue(Commands.none()); + driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); + driverXbox.rightBumper().onTrue(Commands.none()); + drivebase.setDefaultCommand( + !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim); + } } /** @@ -124,7 +139,7 @@ public Command getAutonomousCommand() public void setDriveMode() { - //drivebase.setDefaultCommand(); + configureBindings(); } public void setMotorBrake(boolean brake) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 9c376c7c..9ae8bf3f 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -4,14 +4,23 @@ package frc.robot.subsystems.swervedrive; +import java.io.File; +import java.util.Arrays; +import java.util.function.DoubleSupplier; + +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -23,19 +32,18 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; import frc.robot.Constants.AutonConstants; -import java.io.File; -import java.util.function.DoubleSupplier; -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; import swervelib.math.SwerveMath; +import swervelib.parser.PIDFConfig; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveParser; @@ -357,6 +365,67 @@ public Command sysIdAngleMotorCommand() 3.0, 5.0, 3.0); } + /** + * Returns a Command that sets the P value of the angle motors to the value on the SmartDashboard and updates the PIDF + * configuration of each module. The command is run only once and is intended to be used for characterizing the angle + * motors' P value. The P value is read from the SmartDashboard and if it is not present, a default value of 0 is used. + * + * @return a Command that reads the P value of the angle motors from the SmartDashboard and updates the PIDF configuration of each module + */ + public Command angleMotorPValueCommand() { + return Commands.runOnce(() -> { + SmartDashboard.putNumber("Angle Motor P", swerveDrive.getModuleMap().values().iterator().next().getAnglePIDF().p); + swerveDrive.getModuleMap().values().forEach(module -> { + module.getAngleMotor().configurePIDF(new PIDFConfig(SmartDashboard.getNumber("Angle Motor P", 0), 0)); + module.getAngleMotor().burnFlash(); + }); + }, this); + } + + /** + * Returns a Command that centers the modules of the SwerveDrive subsystem. + * + * @return a Command that centers the modules of the SwerveDrive subsystem + */ + public Command centerModulesCommand() { + return run(() -> Arrays.asList(swerveDrive.getModules()) + .forEach(it -> it.setAngle(0.0))); + } + + /** + * Returns a Command that drives the swerve drive to a specific distance at a given speed. + * + * @param distanceInMeters the distance to drive in meters + * @param speedInMetersPerSecond the speed at which to drive in meters per second + * @return a Command that drives the swerve drive to a specific distance at a given speed + */ + public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) { + return Commands.deferredProxy( + () -> Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this) + .until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) > distanceInMeters) + ); + } + + /** + * Sets the maximum speed of the swerve drive. + * + * @param maximumSpeed the maximum speed to set for the swerve drive in meters per second + */ + public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) { + swerveDrive.setMaximumSpeed(maximumSpeedInMetersPerSecond, false, swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); + } + + /** + * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object. + * + * @param kS the static gain of the feedforward + * @param kV the velocity gain of the feedforward + * @param kA the acceleration gain of the feedforward + */ + public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) { + swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); + } + /** * Command to drive the robot using translative values and heading as angular velocity. * diff --git a/src/main/java/swervelib/SwerveDriveTest.java b/src/main/java/swervelib/SwerveDriveTest.java index 848852c6..a3d71dfe 100644 --- a/src/main/java/swervelib/SwerveDriveTest.java +++ b/src/main/java/swervelib/SwerveDriveTest.java @@ -356,8 +356,8 @@ public static void logAngularMotorVoltage(SwerveModule module, SysIdRoutineLog l public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog log, Supplier powerSupplied) { double power = powerSupplied.get(); - double angle = module.getAbsolutePosition(); - double velocity = module.getAbsoluteEncoder().getVelocity(); + double angle = module.getAngleMotor().getPosition(); + double velocity = module.getAngleMotor().getVelocity(); SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Power", power); SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Position", angle); SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity", From b21e603e038f30623f89ab9e4421410978ca4948 Mon Sep 17 00:00:00 2001 From: Curt Date: Sat, 7 Sep 2024 17:26:54 -0400 Subject: [PATCH 2/3] Change place where Angle Motor P is added to NT --- .../java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 9ae8bf3f..3509a46a 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -373,8 +373,8 @@ public Command sysIdAngleMotorCommand() * @return a Command that reads the P value of the angle motors from the SmartDashboard and updates the PIDF configuration of each module */ public Command angleMotorPValueCommand() { + SmartDashboard.putNumber("Angle Motor P", swerveDrive.getModuleMap().values().iterator().next().getAnglePIDF().p); return Commands.runOnce(() -> { - SmartDashboard.putNumber("Angle Motor P", swerveDrive.getModuleMap().values().iterator().next().getAnglePIDF().p); swerveDrive.getModuleMap().values().forEach(module -> { module.getAngleMotor().configurePIDF(new PIDFConfig(SmartDashboard.getNumber("Angle Motor P", 0), 0)); module.getAngleMotor().burnFlash(); From 00b16826b748df648599ff893228b127396d14cc Mon Sep 17 00:00:00 2001 From: Curt Date: Sat, 14 Sep 2024 15:20:50 -0400 Subject: [PATCH 3/3] Remove Angle Motor configuring --- src/main/java/frc/robot/RobotContainer.java | 1 - .../subsystems/swervedrive/SwerveSubsystem.java | 17 ----------------- 2 files changed, 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 92017828..56b25cea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -99,7 +99,6 @@ public RobotContainer() private void configureBindings() { if (DriverStation.isTest()) { - driverXbox.a().onTrue(drivebase.angleMotorPValueCommand()); driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand()); driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2)); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 3509a46a..14f645a8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -365,23 +365,6 @@ public Command sysIdAngleMotorCommand() 3.0, 5.0, 3.0); } - /** - * Returns a Command that sets the P value of the angle motors to the value on the SmartDashboard and updates the PIDF - * configuration of each module. The command is run only once and is intended to be used for characterizing the angle - * motors' P value. The P value is read from the SmartDashboard and if it is not present, a default value of 0 is used. - * - * @return a Command that reads the P value of the angle motors from the SmartDashboard and updates the PIDF configuration of each module - */ - public Command angleMotorPValueCommand() { - SmartDashboard.putNumber("Angle Motor P", swerveDrive.getModuleMap().values().iterator().next().getAnglePIDF().p); - return Commands.runOnce(() -> { - swerveDrive.getModuleMap().values().forEach(module -> { - module.getAngleMotor().configurePIDF(new PIDFConfig(SmartDashboard.getNumber("Angle Motor P", 0), 0)); - module.getAngleMotor().burnFlash(); - }); - }, this); - } - /** * Returns a Command that centers the modules of the SwerveDrive subsystem. *