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..56b25cea 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,31 @@ 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.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 +138,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 9ce2de4f..9139bcb6 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; @@ -351,6 +359,50 @@ public Command sysIdAngleMotorCommand() 3.0, 5.0, 3.0); } + /** + * 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",