From fe551581a99ef0dcc574d4f7aff6c406a0be87a4 Mon Sep 17 00:00:00 2001 From: Alexander Hamilton Date: Thu, 18 Jan 2024 00:36:09 -0800 Subject: [PATCH] Make the first working auto path! Woohoo! I am so happy with this information! --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/subsystems/Swerve.java | 52 ++++++++++++++++--- src/main/java/frc/robot/util/Constants.java | 29 +++++++++-- 3 files changed, 70 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 693aae19..7af40b11 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,6 +2,7 @@ import java.util.Optional; +import com.pathplanner.lib.commands.PathPlannerAuto; import com.revrobotics.CANSparkBase; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -87,8 +88,7 @@ private void configureDriverBindings() { } public Command getAutonomousCommand() { - // TODO: Add auto commands here - return null; + return new PathPlannerAuto("BetterThanBread"); } public void onDisabled() {} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 4b8832b6..349113f0 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -4,27 +4,33 @@ package frc.robot.subsystems; +import java.util.Optional; import java.util.function.Supplier; import com.ctre.phoenix6.hardware.Pigeon2; +import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +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.SubsystemBase; import frc.robot.DriverUI; import frc.robot.commands.Drive; +import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.DriveConstants; import frc.robot.util.Constants.FieldConstants; import monologue.Logged; @@ -57,14 +63,17 @@ public class Swerve extends SubsystemBase implements Logged { DriveConstants.BACK_RIGHT_CHASSIS_ANGULAR_OFFSET); @Log.NT - SwerveModuleState[] measuredStates; + SwerveModuleState[] swerveMeasuredStates; @Log.NT - SwerveModuleState[] desiredStates; + SwerveModuleState[] swerveDesiredStates; @Log.NT Pose3d robotPose3d = new Pose3d(); + @Log.NT + Pose2d robotPose2d = new Pose2d(); + // The gyro sensor private final Pigeon2 gyro = new Pigeon2(DriveConstants.GYRO_CAN_ID); @@ -103,6 +112,19 @@ public class Swerve extends SubsystemBase implements Logged { */ public Swerve() { + AutoBuilder.configureHolonomic( + this::getPose, + this::resetOdometry, + this::getRobotRelativeSpeeds, + this::drive, + AutoConstants.HPFC, + () -> { + Optional alliance = DriverStation.getAlliance(); + return alliance.isPresent() && alliance.get() == Alliance.Red; + }, + this + ); + resetEncoders(); gyro.setYaw(0); setBrakeMode(); @@ -114,30 +136,35 @@ public Swerve() { public void periodic() { poseEstimator.updateWithTime(DriverUI.currentTimestamp, gyro.getRotation2d(), getModulePositions()); + logPositions(); + } + + public void logPositions() { + if (FieldConstants.IS_SIMULATION) { for (MAXSwerveModule mod : swerveModules) { mod.tick(); } } - measuredStates = new SwerveModuleState[] { + swerveMeasuredStates = new SwerveModuleState[] { frontLeft.getState(), frontRight.getState(), rearLeft.getState(), rearRight.getState() }; - ChassisSpeeds speeds = DriveConstants.DRIVE_KINEMATICS.toChassisSpeeds(measuredStates); + ChassisSpeeds speeds = DriveConstants.DRIVE_KINEMATICS.toChassisSpeeds(swerveMeasuredStates); resetOdometry( getPose().exp( new Twist2d( 0, 0, speeds.omegaRadiansPerSecond * .02))); - } - public void logPositions() { DriverUI.field.setRobotPose(getPose()); SmartDashboard.putNumber("Swerve/RobotRotation", gyro.getRotation2d().getRadians()); + + robotPose2d = getPose(); robotPose3d = new Pose3d( new Translation3d( @@ -148,7 +175,8 @@ public void logPositions() { * DriveConstants.ROBOT_LENGTH_METERS / 2.0, Rotation2d.fromDegrees(gyro.getPitch().refresh().getValue()).getSin() * DriveConstants.ROBOT_LENGTH_METERS / 2.0)), - gyro.getRotation3d()); + FieldConstants.IS_SIMULATION ? new Rotation3d(0, 0, getPose().getRotation().getRadians()) : gyro.getRotation3d()); + } /** @@ -164,6 +192,10 @@ public SwerveDrivePoseEstimator getPoseEstimator() { return poseEstimator; } + public void drive(ChassisSpeeds speeds) { + drive(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond, false); + } + public void drive(double xSpeed, double ySpeed, double rotSpeed, boolean fieldRelative) { xSpeed *= (DriveConstants.MAX_SPEED_METERS_PER_SECOND * speedMultiplier); @@ -179,6 +211,10 @@ public void drive(double xSpeed, double ySpeed, double rotSpeed, boolean fieldRe setModuleStates(swerveModuleStates); } + public ChassisSpeeds getRobotRelativeSpeeds() { + return DriveConstants.DRIVE_KINEMATICS.toChassisSpeeds(getModuleStates()); + } + /** * Sets the wheels into an X formation to prevent movement. */ @@ -206,7 +242,7 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { rearLeft.setDesiredState(desiredStates[2]); rearRight.setDesiredState(desiredStates[3]); - this.desiredStates = desiredStates; + this.swerveDesiredStates = desiredStates; } public void resetOdometry(Pose2d pose) { diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 08fdec7b..1b12a37e 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -6,6 +6,9 @@ import java.util.ArrayList; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; import com.revrobotics.CANSparkBase; import java.util.Optional; import edu.wpi.first.math.controller.HolonomicDriveController; @@ -30,7 +33,7 @@ public final class Constants { public static final class DriveConstants { // Driving Parameters - Note that these are not the maximum capable speeds of // the robot, rather the allowed maximum speeds - public static double MAX_SPEED_METERS_PER_SECOND = 3; + public static double MAX_SPEED_METERS_PER_SECOND = 4.277; public static final double MAX_ANGULAR_SPEED_RADS_PER_SECOND = 4 * Math.PI; // radians per second @@ -79,10 +82,11 @@ public static final class DriveConstants { public static final class AutoConstants { // The below values need to be tuned for each new robot. - public static final double MAX_SPEED_METERS_PER_SECOND = 3; - public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 3.5; - public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 2 * Math.PI; - public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED = 3 * Math.PI; + // They are curently set to the values suggested by Choreo + public static final double MAX_SPEED_METERS_PER_SECOND = 4.277; + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 7.344; + public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 10.468; + public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED = 37.053; public static final double PX_CONTROLLER = 1; public static final double PY_CONTROLLER = 1; @@ -120,6 +124,21 @@ public static final class AutoConstants { new TrapezoidProfile.Constraints( AutoConstants.MAX_ANGULAR_SPEED_RADIANS_PER_SECOND, AutoConstants.MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED))); + + public static final HolonomicPathFollowerConfig HPFC = new HolonomicPathFollowerConfig( + new PIDConstants( + AutoConstants.X_CORRECTION_P, + AutoConstants.X_CORRECTION_I, + AutoConstants.X_CORRECTION_D), + new PIDConstants( + AutoConstants.ROTATION_CORRECTION_P, + AutoConstants.ROTATION_CORRECTION_I, + AutoConstants.ROTATION_CORRECTION_D), + MAX_SPEED_METERS_PER_SECOND, + Math.hypot(DriveConstants.WHEEL_BASE, DriveConstants.TRACK_WIDTH), + new ReplanningConfig() + ); + } public static final class ModuleConstants {