diff --git a/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto new file mode 100644 index 0000000..cd4f4d0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto @@ -0,0 +1,38 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Shoot driving across" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot Driving" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot driving across.path b/src/main/deploy/pathplanner/paths/Shoot driving across.path new file mode 100644 index 0000000..9d737a3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot driving across.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": null, + "nextControl": { + "x": 1.9932621113872893, + "y": 6.3390412980405895 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 1.6628461538461545 + }, + "prevControl": { + "x": 1.936134248900721, + "y": 1.9128215679513527 + }, + "nextControl": { + "x": 1.9291221613556924, + "y": 1.4128707397409563 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": { + "x": 1.9198835421653975, + "y": 6.331901987278464 + }, + "nextControl": { + "x": 1.9453728680910158, + "y": 6.8312518588753814 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 1.6628461538461545 + }, + "prevControl": { + "x": 1.9337461053582308, + "y": 1.9128436544318115 + }, + "nextControl": { + "x": 1.9315103048981825, + "y": 1.4128486532604976 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": { + "x": 1.8878500514131944, + "y": 6.335619778537727 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot driving vertical.path b/src/main/deploy/pathplanner/paths/Shoot driving vertical.path new file mode 100644 index 0000000..59da45c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot driving vertical.path @@ -0,0 +1,118 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.956, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.707930530821583, + "y": 4.031008683647302 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3, + "y": 4.0 + }, + "prevControl": { + "x": 1.5499999999999998, + "y": 4.0 + }, + "nextControl": { + "x": 1.05, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9559102564102573, + "y": 4.0 + }, + "prevControl": { + "x": 2.7118639603594925, + "y": 3.9457652751099213 + }, + "nextControl": { + "x": 3.199956552461022, + "y": 4.054234724890079 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3, + "y": 4.0 + }, + "prevControl": { + "x": 1.5434594129947146, + "y": 4.0568112156556175 + }, + "nextControl": { + "x": 1.0565405870052855, + "y": 3.9431887843443825 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 4.0 + }, + "prevControl": { + "x": 2.7532300973825357, + "y": 3.9599423520140395 + }, + "nextControl": { + "x": 3.2467699026174643, + "y": 4.040057647985961 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3, + "y": 4.0 + }, + "prevControl": { + "x": 1.526511041060335, + "y": 4.105795785727803 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 53e4df3..7f73bd5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -5,9 +5,14 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + package frc4388.robot; import java.io.File; +import java.util.Optional; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -24,6 +29,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -129,6 +135,15 @@ public class RobotContainer { IntakeExtended, new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) ); + + private Command RobotShootDriving = new SequentialCommandGroup( + new InstantCommand(() -> + m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION) + ), + new WaitCommand(99) // stays on for the duration of the auto segment + ).finallyDo((interrupted) -> + m_robotSwerveDrive.disableRotationOverride() + ); private Command IntakeRetracted = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) @@ -170,6 +185,7 @@ public class RobotContainer { NamedCommands.registerCommand("Robot Shoot", RobotShoot); // NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Intake Extended", IntakeExtended); + NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving); DriverStation.silenceJoystickConnectionWarning(true); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 9f111da..b220d26 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 171; - public static final String GIT_SHA = "4ffbe3f5954581f07ac26ee6174113c909cdbaa7"; - public static final String GIT_DATE = "2026-03-19 18:32:02 MDT"; + public static final int GIT_REVISION = 172; + public static final String GIT_SHA = "69677d5ae9e63125a30857f5f7afa0756dfa9416"; + public static final String GIT_DATE = "2026-03-20 15:40:07 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-20 15:31:47 MDT"; - public static final long BUILD_UNIX_TIME = 1774042307703L; + public static final String BUILD_DATE = "2026-03-20 19:59:37 MDT"; + public static final long BUILD_UNIX_TIME = 1774058377112L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 52fbcd3..9522450 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -9,6 +9,7 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; 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.units.measure.AngularVelocity; @@ -114,12 +115,19 @@ public class Shooter extends SubsystemBase { public void periodic() { // FaultReporter.register(this); // TODO Implement fault reporter + Logger.processInputs("Shooter", state); io.updateInputs(state); // Get robot positon and speeds ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds; + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION); + Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle(); Pose2d robotPose2d = m_SwerveDrive.getPose2d(); @@ -171,8 +179,10 @@ public class Shooter extends SubsystemBase { m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; - case 0b001: // No errors and shoot button is pressed + case 0b001: // No errors and shoot button is pressed + if (ang.getDegrees() < ShooterConstants.AIM_ANGLE.get()){ // robot is aimed at hub io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); + } m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 3198f2e..816b953 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -2,8 +2,6 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RotationsPerSecond; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; @@ -37,8 +35,8 @@ public class ShooterConstants { public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); - public static final double NEG_OFFSET = 8.; - public static final double POS_OFFSET = 8.; + public static final ConfigurableDouble NEG_OFFSET = new ConfigurableDouble("Negative offset", 8.); + public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.); public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1); @@ -47,6 +45,7 @@ public class ShooterConstants { public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8); + public static final ConfigurableDouble AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 5); public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360); public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1); @@ -65,12 +64,12 @@ public class ShooterConstants { } else if (chassisXSpeed > 0){ speed = 0.0593402*hubDistMeters*hubDistMeters + 4.90561*hubDistMeters + - 30.35696 + chassisXSpeed * POS_OFFSET + MODEL_TRIM.get(); + 30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get(); } else { // Negative is closer to hub speed = 0.0593402*hubDistMeters*hubDistMeters + 4.90561*hubDistMeters + - 30.35696 + chassisXSpeed * NEG_OFFSET + MODEL_TRIM.get(); + 30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get(); } double max = SHOOTER_MAX_VELOCITY.get(); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 4ebbf14..fbea572 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -7,6 +7,7 @@ package frc4388.robot.subsystems.swerve; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; +import java.util.Optional; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -20,6 +21,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.trajectory.PathPlannerTrajectory; import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -65,6 +67,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + private final PIDController m_rotationOverridePID = new PIDController( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + ); + private boolean m_useRotationOverride = false; + private Translation2d m_rotationOverrideTarget = new Translation2d(); + /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) { // public SwerveDrive(SwerveDrivetrain @@ -83,6 +93,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // Handle exception as needed config = null; } + + PPHolonomicDriveController driveController = new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), // Translation PID + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID (used when override is OFF) + ); + driveController.setRotationTargetOverride(() -> { + if (!m_useRotationOverride) return Optional.empty(); + Rotation2d targetAngle = getPose2d() + .getTranslation() + .minus(m_rotationOverrideTarget) + .getAngle(); + return Optional.of(targetAngle); + }); + // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { @@ -94,11 +118,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds() .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. // Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for - // holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants - ), + driveController, // <-- use the variable, not inline new PPHolonomicDriveController(...) config, // The robot configuration () -> { // Boolean supplier that controls when the path will be mirrored for the red @@ -235,6 +255,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } + + + public void aimAtPosition(Translation2d fieldPos, double aimLeadTime) { + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); + Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(chassisSpeeds.vxMetersPerSecond) + .withVelocityY(chassisSpeeds.vyMetersPerSecond) + .withTargetDirection(ang); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + ); + io.setControl(ctrl); + } public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical // reason to have a robot @@ -408,33 +449,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { driveFieldAngle(leftStick, ang); } - - - public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, double ballVelocity, double distanceToHub) { - - Translation2d robotSpeed = new Translation2d( - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond - ); - - if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ - double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity)); - fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); - Logger.recordOutput("Offset Value", aimOffset); - - } - - - - Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); - - Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); - - Logger.recordOutput("Aim at Offset", fieldPos); - - driveFieldAngle(leftStick, ang); - } - public Pose2d getCurrentPose(){ return state.currentPose; } @@ -526,6 +540,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable { softStop(); } + public void enableRotationOverride(Translation2d fieldTarget) { + m_rotationOverrideTarget = fieldTarget; + m_useRotationOverride = true; + } + + public void disableRotationOverride() { + m_useRotationOverride = false; + } + @Override public void periodic() { // This method will be called once per scheduler run\ @@ -663,5 +686,4 @@ public class SwerveDrive extends SubsystemBase implements Queryable { return status; } -} - +} \ No newline at end of file