diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index e376b3a6..e2e2e3fd 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -46,7 +46,7 @@ public SwerveDriveDrive(Gamepad driver) { x -> x * Turn.MAX_TELEOP_TURNING.get(), new RateLimit(Turn.RC.get()) ); - + addRequirements(swerve); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java new file mode 100644 index 00000000..12692af3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java @@ -0,0 +1,70 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Alignment.Rotation; +import com.stuypulse.robot.constants.Settings.Alignment.Translation; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveToPose extends Command { + + private final SwerveDrive swerve; + + private final HolonomicController controller; + private final Pose2d targetPose; + private final BStream isAligned; + + private final FieldObject2d targetPose2d; + + public SwerveDriveToPose(Pose2d targetPose) { + swerve = SwerveDrive.getInstance(); + this.targetPose = targetPose; + + targetPose2d = Odometry.getInstance().getField().getObject("Target Pose"); + + controller = new HolonomicController( + new PIDController(Translation.P, Translation.I, Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)); + + isAligned = BStream.create(this::isAligned) + .filtered(new BDebounceRC.Both(Alignment.DEBOUNCE_TIME.get())); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(Alignment.X_TOLERANCE.get(), Alignment.Y_TOLERANCE.get(), Alignment.ANGLE_TOLERANCE.get()); + } + + @Override + public void execute() { + Pose2d currentPose = Odometry.getInstance().getPose(); + targetPose2d.setPose(targetPose); + + controller.update(targetPose, currentPose); + swerve.setChassisSpeeds(controller.getOutput()); + } + + @Override + public boolean isFinished() { + return isAligned.get(); + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java new file mode 100644 index 00000000..560f7ff0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java @@ -0,0 +1,90 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Alignment.Rotation; +import com.stuypulse.robot.constants.Settings.Alignment.Translation; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.Vector2D; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveToShoot extends Command { + + private final SwerveDrive swerve; + + private final HolonomicController controller; + private Pose2d targetPose; + + private final FieldObject2d targetPose2d; + private final FieldObject2d speakerPose2d; + + public SwerveDriveToShoot() { + swerve = SwerveDrive.getInstance(); + + targetPose2d = Odometry.getInstance().getField().getObject("Target Pose"); + speakerPose2d = Odometry.getInstance().getField().getObject("Speaker Pose"); + + controller = new HolonomicController( + new PIDController(Translation.P, Translation.I, Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)); + + addRequirements(swerve); + } + + private Pose2d getSpeakerTargetPose() { + Vector2D speakerCenterVec = new Vector2D(Field.getAllianceSpeakerPose().getTranslation()); + Vector2D robotVec = new Vector2D(Odometry.getInstance().getPose().getTranslation()); + + // the distances between the robot and the target + double Dx = speakerCenterVec.x - robotVec.x; + double Dy = speakerCenterVec.y - robotVec.y; + + // the offset of the speakers opening width from center using similar triangles + double dy = (Dy / Dx) * Field.SPEAKER_OPENING_X; + + // gets the new speaker target vector to aim at + Vector2D speakerTargetVec = new Vector2D(Field.getAllianceSpeakerPose().getX(), Field.getAllianceSpeakerPose().getY() + dy); + + speakerPose2d.setPose(speakerTargetVec.x, speakerTargetVec.y, Rotation2d.fromDegrees(0)); + + // gets the target vector away from the speaker to the target distance to shoot from + Vector2D targetVec = speakerTargetVec.add(robotVec.sub(speakerTargetVec).normalize().mul(Units.inchesToMeters(Alignment.TARGET_DISTANCE_IN.get()))); + Rotation2d targetAngle = targetVec.getTranslation2d().minus(robotVec.getTranslation2d()).getAngle().plus(Rotation2d.fromDegrees(180)); + return new Pose2d(targetVec.x, targetVec.y, targetAngle); + } + + @Override + public void initialize() { + targetPose = getSpeakerTargetPose(); + } + + @Override + public void execute() { + Pose2d currentPose = Odometry.getInstance().getPose(); + + controller.update(targetPose, currentPose); + swerve.setChassisSpeeds(controller.getOutput()); + targetPose2d.setPose(targetPose); + } + + @Override + public boolean isFinished() { + return controller.isDone(Alignment.X_TOLERANCE.get(), Alignment.Y_TOLERANCE.get(), Alignment.ANGLE_TOLERANCE.get()); + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java new file mode 100644 index 00000000..7d8efcd6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java @@ -0,0 +1,30 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveXMode extends Command { + + // { front right, front left, back right, back left } + private static final SwerveModuleState[] states = new SwerveModuleState[] { + new SwerveModuleState(0, Rotation2d.fromDegrees(225)), + new SwerveModuleState(0, Rotation2d.fromDegrees(315)), + new SwerveModuleState(0, Rotation2d.fromDegrees(45)), + new SwerveModuleState(0, Rotation2d.fromDegrees(135)) + }; + + private final SwerveDrive swerve; + + public SwerveDriveXMode() { + swerve = SwerveDrive.getInstance(); + addRequirements(swerve); + } + + @Override + public void execute() { + swerve.setModuleStates(states); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index a59f1ef1..5a9ca153 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -4,20 +4,25 @@ import com.stuypulse.robot.util.Fiducial; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; /** * This interface stores information about the field elements. */ public interface Field { - public final double FIDUCIAL_SIZE = Units.inchesToMeters(6.125); double NOTE_LENGTH = Units.inchesToMeters(14.0); + /*** APRILTAGS ***/ + + double FIDUCIAL_SIZE = Units.inchesToMeters(6.125); + Fiducial FIDUCIALS[] = { // 2024 Field Fiducial Layout new Fiducial(1, new Pose3d(new Translation3d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), @@ -84,4 +89,18 @@ public static Fiducial getFiducial(int fid) { } return null; } + + /*** SPEAKER ***/ + + Pose2d SPEAKER_POSES[] = { + getFiducial(7).getLocation().toPose2d(), // BLUE CENTER + getFiducial(4).getLocation().toPose2d(), // RED CENTER + }; + + public static Pose2d getAllianceSpeakerPose() { + boolean isBlue = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue; + return SPEAKER_POSES[isBlue ? 0 : 1]; + } + + double SPEAKER_OPENING_X = Units.inchesToMeters(13.6); } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2d63cf09..27c507aa 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,8 +5,7 @@ package com.stuypulse.robot.constants; -import edu.wpi.first.math.util.Units; - +import com.pathplanner.lib.util.PIDConstants; import com.stuypulse.stuylib.math.Vector2D; import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; @@ -93,11 +92,19 @@ public interface Swerve { double WIDTH = Units.inchesToMeters(20.75); double LENGTH = Units.inchesToMeters(20.75); + double XY = 0.1; + double THETA = 0.1; + SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Max Module Speed (meter per s)", 5.0); SmartNumber MAX_TURNING = new SmartNumber("Swerve/Max Turn Velocity (rad per s)", 6.28); SmartNumber MODULE_VELOCITY_DEADBAND = new SmartNumber("Swerve/Module Velocity Deadband (m per s)", 0.02); + public interface Motion { + PIDConstants XY = new PIDConstants(0.7, 0, 0.02); + PIDConstants THETA = new PIDConstants(10, 0, 0.1); + } + public interface Encoder { public interface Drive { double WHEEL_DIAMETER = Units.inchesToMeters(3); @@ -248,4 +255,27 @@ public interface Conveyor { public static Vector2D vpow(Vector2D vec, double power) { return vec.mul(Math.pow(vec.magnitude(), power - 1)); } + + public interface Alignment { + + SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15); + SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); + SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); + SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); + + SmartNumber TARGET_DISTANCE_IN = new SmartNumber("Alignment/Target Distance (in)", 110); + SmartNumber TAKEOVER_DISTANCE_IN = new SmartNumber("Alignment/Takeover Distance (in)", 50); + + public interface Translation { + SmartNumber P = new SmartNumber("Alignment/Translation/kP", 2.5); + SmartNumber I = new SmartNumber("Alignment/Translation/kI", 0); + SmartNumber D = new SmartNumber("Alignment/Translation/kD", 0.0); + } + + public interface Rotation { + SmartNumber P = new SmartNumber("Alignment/Rotation/kP", 1); + SmartNumber I = new SmartNumber("Alignment/Rotation/kI", 0); + SmartNumber D = new SmartNumber("Alignment/Rotation/kD", 0); + } + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 27ea95d0..238b9b0b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -1,6 +1,10 @@ package com.stuypulse.robot.subsystems.swerve; import com.kauailabs.navx.frc.AHRS; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.util.ReplanningConfig; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Swerve; @@ -22,6 +26,7 @@ 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.RobotBase; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -108,7 +113,34 @@ public SwerveDrive(SwerveModule... modules){ this.kinematics = new SwerveDriveKinematics(getModuleOffsets()); this.gyro = new AHRS(SPI.Port.kMXP); this.modules2D = new FieldObject2d[modules.length]; - } + } + + public void configureAutoBuilder() { + Odometry odometry = Odometry.getInstance(); + + AutoBuilder.configureHolonomic( + odometry::getPose, + odometry::reset, + this::getChassisSpeeds, + this::setChassisSpeeds, + new HolonomicPathFollowerConfig( + Swerve.Motion.XY, + Swerve.Motion.THETA, + Swerve.MAX_MODULE_SPEED.get(), + Swerve.WIDTH, + new ReplanningConfig(true, true)), + () -> { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + instance + ); + + PathPlannerLogging.setLogActivePathCallback((poses) -> odometry.getField().getObject("path").setPoses(poses)); + } public void initFieldObject(Field2d field) { for (int i = 0; i < modules.length; i++){ @@ -228,6 +260,10 @@ public void periodic() { SmartDashboard.putNumber("Swerve/X Acceleration (Gs)", gyro.getWorldLinearAccelX()); SmartDashboard.putNumber("Swerve/Y Acceleration (Gs)", gyro.getWorldLinearAccelY()); SmartDashboard.putNumber("Swerve/Z Acceleration (Gs)", gyro.getWorldLinearAccelZ()); + + SmartDashboard.putNumber("Swerve/Chassis X Speed", getChassisSpeeds().vxMetersPerSecond); + SmartDashboard.putNumber("Swerve/Chassis Y Speed", getChassisSpeeds().vyMetersPerSecond); + SmartDashboard.putNumber("Swerve/Chassis Rotation", getChassisSpeeds().omegaRadiansPerSecond); } public void simulationPeriodic() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java index 21921bfc..db4cca7f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java @@ -12,6 +12,7 @@ import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.streams.angles.filters.ARateLimit; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -44,6 +45,10 @@ public SimModule(String id, Translation2d offset) { angleController = new AnglePIDController(Turn.kP, Turn.kI, Turn.kD) .setSetpointFilter(new ARateLimit(Swerve.MAX_TURNING)); } + + public double getPosition() { + return driveSim.getOutput(0); + } @Override public double getVelocity() { @@ -76,8 +81,8 @@ public void simulationPeriodic() { driveSim.setInput(0); turnSim.setInput(0); } else { - driveSim.setInput(driveController.getOutput()); - turnSim.setInput(angleController.getOutput()); + driveSim.setInput(MathUtil.clamp(driveController.getOutput(), -12, 12)); + turnSim.setInput(MathUtil.clamp(angleController.getOutput(), -12, 12)); } RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage( @@ -89,6 +94,7 @@ public void simulationPeriodic() { SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Drive Voltage", driveController.getOutput()); SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Turn Voltage", angleController.getOutput()); + SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Position", getPosition()); SmartDashboard.putNumber("Swerve/Modules/" + this.getId() + "/Angle Error", angleController.getError().toDegrees()); } } diff --git a/src/main/java/com/stuypulse/robot/util/HolonomicController.java b/src/main/java/com/stuypulse/robot/util/HolonomicController.java new file mode 100644 index 00000000..9fa82f86 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/HolonomicController.java @@ -0,0 +1,59 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.math.Angle; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; + +public class HolonomicController implements Sendable { + private final Controller xController; + private final Controller yController; + private final AngleController angleController; + + public HolonomicController(Controller xController, Controller yController, AngleController angleController) { + this.xController = xController; + this.yController = yController; + this.angleController = angleController; + } + + public ChassisSpeeds update(Pose2d setpoint, Pose2d measurement) { + xController.update(setpoint.getX(), measurement.getX()); + yController.update(setpoint.getY(), measurement.getY()); + angleController.update( + Angle.fromRotation2d(setpoint.getRotation()), + Angle.fromRotation2d(measurement.getRotation())); + + + return getOutput(); + } + + public ChassisSpeeds getOutput() { + return ChassisSpeeds.fromFieldRelativeSpeeds( + xController.getOutput(), + yController.getOutput(), + angleController.getOutput(), + angleController.getMeasurement().getRotation2d()); + } + + public boolean isDone(double xToleranceMeters, double yToleranceMeters, double angleToleranceDegrees) { + return xController.isDone(xToleranceMeters) && yController.isDone(yToleranceMeters) && angleController.isDoneDegrees(angleToleranceDegrees); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Holonomic Controller"); + builder.addDoubleProperty("Angle Setpoint (degrees)", () -> angleController.getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Angle Measurement (degrees)", () -> angleController.getMeasurement().toDegrees(), null); + builder.addDoubleProperty("X Setpoint (meters)", () -> xController.getSetpoint(), null); + builder.addDoubleProperty("X Measurement (meters)", () -> xController.getMeasurement(), null); + builder.addDoubleProperty("Y Setpoint (meters)", () -> yController.getSetpoint(), null); + builder.addDoubleProperty("Y Measurement (meters)", () -> yController.getMeasurement(), null); + builder.addDoubleProperty("X Error (meters)", () -> xController.getError(), null); + builder.addDoubleProperty("Y Error (meters)", () -> yController.getError(), null); + builder.addDoubleProperty("Angle Error (degrees)", () -> angleController.getError().getRotation2d().getDegrees(), null); + } +} \ No newline at end of file