From fc424bb3347dd198d18ca27523f886673054aff2 Mon Sep 17 00:00:00 2001 From: chris Date: Fri, 20 Dec 2024 16:17:17 -0600 Subject: [PATCH] documentation, cleanup code --- src/main/java/frc/robot/Robot.java | 16 +--- .../frc/robot/commands/AbsoluteDriveAdv.java | 78 ------------------ .../java/frc/robot/subsystems/Swerve.java | 81 ++++++++++++------- 3 files changed, 51 insertions(+), 124 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/AbsoluteDriveAdv.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 492fb0f..4b88a75 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,8 +7,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -26,19 +24,7 @@ public Robot() { addPeriodic(() -> CommandScheduler.getInstance().run(), kDefaultPeriod); configureBindings(); //! TODO: Buttons are for turning to directions, can cause conflict with definitions in configureBindings, change? - // AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv( //? TODO: Rename? - // swerve, - // () -> MathUtil.applyDeadband(xboxController.getLeftX(), DriveK.leftJoystickDeadband), - // () -> MathUtil.applyDeadband(xboxController.getLeftY(), DriveK.leftJoystickDeadband) , - // () -> MathUtil.applyDeadband(xboxController.getRightX(), DriveK.rightJoystickDeadband), - // xboxController.getHID()::getAButton, - // xboxController.getHID()::getXButton, - // xboxController.getHID()::getYButton, - // xboxController.getHID()::getBButton - // ); - - var slider = Shuffleboard.getTab("testing").add("Left/Right", 0).withWidget(BuiltInWidgets.kNumberSlider).getEntry(); - var sliderTwo = Shuffleboard.getTab("testing").add("Forward/Backward", 0).withWidget(BuiltInWidgets.kNumberSlider).getEntry(); + // Fill in parameter info Command driveFieldOrientedAngularVelocity = swerve.driveCommand( () -> DriveK.translationalYLimiter.calculate(MathUtil.applyDeadband(-xboxController.getLeftY(), ControllerK.leftJoystickDeadband)), diff --git a/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java deleted file mode 100644 index 95c5fe7..0000000 --- a/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java +++ /dev/null @@ -1,78 +0,0 @@ -package frc.robot.commands; - -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import frc.robot.subsystems.Swerve; - -public class AbsoluteDriveAdv { - - private final Swerve swerve; - private final DoubleSupplier vXInput, vYInput, headingAdjust; - private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; - private boolean resetHeading; - - /** - * @param swerve Swerve Subsystem - * @param vXInput Double supplier X component of joystick input after deadband is applied, should be -1 to 1 - * @param vYInput Double supplier Y component of joystick input after deadband is applied, should be -1 to 1 - * @param headingAdjust Double supplier current robot heading to be adjusted after deadband is applied - * @param lookAway Faces the robot towards the opposind alliance wall from the driver - * @param lookTowards Faces the robot towards the driver - * @param lookLeft Faces the robot left - * @param lookRight Faces the robot right - * - * @return - */ - public AbsoluteDriveAdv(Swerve swerve, DoubleSupplier vXInput, DoubleSupplier vYInput, DoubleSupplier headingAdjust, - BooleanSupplier lookAway, BooleanSupplier lookTowards, BooleanSupplier lookLeft, - BooleanSupplier lookRight) { - - this.swerve = swerve; - this.vXInput = vXInput; - this.vYInput = vYInput; - this.headingAdjust = headingAdjust; - this.lookAway = lookAway; - this.lookTowards = lookTowards; - this.lookLeft = lookLeft; - this.lookRight = lookRight; - } - - public void initialize() { - resetHeading = true; - } - - public void execute() { - double headingX = 0; // Resets values - double headingY = 0; - - if (lookAway.getAsBoolean()) { // Y - headingY = -1; - } - if (lookTowards.getAsBoolean()) { // Y - headingY = 1; - } - if (lookLeft.getAsBoolean()) { // X - headingX = -1; - } - if (lookRight.getAsBoolean()) { // X - headingX = 1; - } - - ChassisSpeeds desiredSpeed = swerve.getTargetChassisSpeeds(vXInput.getAsDouble(), vYInput.getAsDouble(), new Rotation2d(headingAdjust.getAsDouble())); - - swerve.drive(new Translation2d(headingX, headingY), headingAdjust.getAsDouble(), true, true); //verify if correct - } - - public void end() { - - } - - public boolean isFinished() { - return false; //! TODO: - } - -} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ab52fea..945982b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -8,7 +8,6 @@ import java.io.IOException; import java.util.function.DoubleSupplier; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; @@ -19,7 +18,6 @@ import edu.wpi.first.units.Angle; import edu.wpi.first.units.Measure; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.SwerveK; @@ -39,8 +37,6 @@ public class Swerve extends SubsystemBase { public Swerve() { double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(SwerveK.steerGearRatio, 4096); double driveConversionFactor = SwerveMath.calculateMetersPerRotation(SwerveK.driveGearRatio, SwerveK.wheelDiameter.in(Inches)); - // System.out.println("Angle conversion factor: " + angleConversionFactor); - // if (1 == 1)throw new RuntimeException(""); SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; SwerveParser parser = null; @@ -54,19 +50,8 @@ public Swerve() { } @Override - public void periodic() { // Calls constantly while robot is running - for (var mod : swerveDrive.getModules()) { - var motor = (WPI_TalonSRX) mod.getAngleMotor().getMotor(); - if (motor.getDeviceID() == 4) { - // motor.set(0.05); - - // System.out.println(motor.getSelectedSensorPosition()); - // System.out.println("Motor: " + motor.getDeviceID() + " | Closed loop error: " + motor.getClosedLoopError() + " | Closed loop target: " + motor.getClosedLoopTarget() + " | Current Position: " + motor.getSelectedSensorPosition()); - SmartDashboard.putNumber("Error", motor.getClosedLoopError()); - SmartDashboard.putNumber("Current", motor.getSelectedSensorPosition()); - SmartDashboard.putNumber("Reference", motor.getClosedLoopTarget()); - } - } + public void periodic() { + // Do nothing } public void setupPathPlanner() { @@ -104,40 +89,87 @@ public Command driveCommand(DoubleSupplier TranslationX, DoubleSupplier Translat fieldRelative, true)); } + /** + * + * @param targetAngle Desired angle + * @param currentAngle Current angle + * @param fieldRelative Wether or not swerve is controlled using field relative speeds + * @return + */ public Command turnCommand(Measure targetAngle, Measure currentAngle, boolean fieldRelative) { return runOnce(() -> turn(targetAngle, currentAngle, fieldRelative)); } + /** + * + * @param targetAngle Desired angle + * @param currentAngle Current angle + * @param fieldRelative Wether or not swerve is controlled using field relative speeds + */ public void turn(Measure targetAngle, Measure currentAngle, boolean fieldRelative) { drive(getPose().getTranslation(), getTurningAngle(targetAngle, currentAngle).in(Degrees), fieldRelative, false); } + /** + * Gets the closest angle to turn to depending on the current heading of the robot + * + * @param desiredAngle Angle to turn to + * @param currentHeading Current heading + * @return + */ public Measure getTurningAngle(Measure desiredAngle, Measure currentHeading) { double angle = (desiredAngle.minus(currentHeading).plus(Degrees.of(540))).in(Degrees); angle = (angle % 360) - 180; return Degrees.of(angle); } + /** + * + * @param translation Linear velocity of the robot in meters per second + * @param rotation Rotation rate of the robot in Radians per second + * @param fieldRelative Wether the robot is field relative (true) or robot relative (false) + * @param isOpenLoop Wether it uses a closed loop velocity control or an open loop + */ public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { swerveDrive.drive(translation, rotation, fieldRelative, isOpenLoop); } + /** + * Resets the odometry to the given pose + * @param initialHolonomicPose Pose to reset the odemetry to + */ public void resetOdometry(Pose2d initialHolonomicPose) { swerveDrive.resetOdometry(initialHolonomicPose); } + /** + * + * @return Current pose of the robot as a Pose2d + */ public Pose2d getPose() { return swerveDrive.getPose(); } + /** + * + * @return Current speed of the robot + */ public ChassisSpeeds getRobotVelocity() { return swerveDrive.getRobotVelocity(); } + /** + * Set the speed of the robot with closed loop velocity control + * @param chassisSpeeds + */ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { swerveDrive.setChassisSpeeds(chassisSpeeds); } + /** + * + * @return Rotational component of the robots pose + */ public Rotation2d getHeading() { return getPose().getRotation(); } @@ -149,17 +181,4 @@ public Rotation2d getHeading() { public void zeroGyro() { swerveDrive.zeroGyro(); } - - //! TODO: Fill out this info - /** - * - * @param xInput - * @param yInput - * @param angle - * @return - */ - public ChassisSpeeds getTargetChassisSpeeds(double xInput, double yInput, Rotation2d angle) { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(), angle.getRadians(), getPose().getRotation().getRadians(), SwerveK.maxRobotSpeed.in(MetersPerSecond)); - } -} +} \ No newline at end of file