diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c4eca1b..1b8278b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -231,18 +231,27 @@ public class RobotContainer { m_robotSwerveDrive.shiftDownRot(); })); - //TEST - > Swerve drive pid on position + //TEST - > Defense: X position on wheels and swerve drive pid on position new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> { - currentPose = m_robotSwerveDrive.getCurrentPose(); - })) - .whileTrue(new RunCommand(() -> { - m_stayInPosition.goToTargetPose(currentPose); + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.defenseXPosition(); }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.softStop(); + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.stopDefenseXPosition(); + + // .onTrue(new InstantCommand(() -> { + // currentPose = m_robotSwerveDrive.getCurrentPose(); + // })) + // .whileTrue(new RunCommand(() -> { + // m_stayInPosition.goToTargetPose(currentPose); + // }, m_robotSwerveDrive)) + // .onFalse(new InstantCommand(() -> { + // m_robotSwerveDrive.softStop(); + // })); })); + + // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) .whileTrue(new RunCommand(() -> m_robotSwerveDrive.driveFacingPosition( diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 586e645..d8311c9 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 = 157; - public static final String GIT_SHA = "2de8c605970a55114d3a66ebfb26160df3e0aa50"; - public static final String GIT_DATE = "2026-03-12 18:35:44 MDT"; + public static final int GIT_REVISION = 158; + public static final String GIT_SHA = "07ec609b019fd8db439e14beb74c50fdbf75a091"; + public static final String GIT_DATE = "2026-03-14 15:41:27 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-14 15:24:12 MDT"; - public static final long BUILD_UNIX_TIME = 1773523452704L; + public static final String BUILD_DATE = "2026-03-14 17:27:47 MDT"; + public static final long BUILD_UNIX_TIME = 1773530867415L; 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 ed625f9..0319706 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -89,8 +89,9 @@ public class Shooter extends SubsystemBase { shooterButtonReady = false; } - public AngularVelocity getBallVelocity() { - return RotationsPerSecond.of((state.motor1Velocity.in(RotationsPerSecond) + state.indexerVelocity.in(RotationsPerSecond))); + public double getBallVelocity() { + return Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) * ShooterConstants.SHOOTER_RADIUS * 2 * Math.PI; + //Math.abs(state.indexerForwardVelocity.in(RotationsPerSecond))*ShooterConstants.INDEXER_RADIUS) } @AutoLogOutput diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index d55e93b..f074a74 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -18,7 +18,8 @@ public class ShooterConstants { public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; public static final double INDEXER_GEAR_RATIO = 1.; public static final double T_CONSTANT = 2; - + public static final double SHOOTER_RADIUS = 2/39.37; + public static final double INDEXER_RADIUS = 0.625/39.37; public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -42); // public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 9b32782..6be6431 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -26,7 +26,7 @@ public interface ShooterIO { AngularVelocity motor1Velocity = RotationsPerSecond.of(0); AngularVelocity motor2Velocity = RotationsPerSecond.of(0); - AngularVelocity indexerVelocity = RotationsPerSecond.of(0); + AngularVelocity indexerForwardVelocity = RotationsPerSecond.of(42.53); //guestimate double indexerOutput = 0; Current motor1Current = Amps.of(0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 55a75d9..f173553 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -87,6 +87,9 @@ public class ShooterReal implements ShooterIO { public void setIndexerOutput(ShooterState state, double percentOutput) { state.indexerTargetOutput = percentOutput; m_indexerMotor.set(percentOutput); + if (state.indexerTargetOutput - state.indexerOutput > 0.05){ + state.indexerForwardVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); + } } @@ -95,7 +98,6 @@ public class ShooterReal implements ShooterIO { state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); - state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); state.indexerOutput = m_indexerMotor.get(); // state.indexerOutput = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 071a21d..69d865f 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -150,14 +150,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void setInitalPose(Pose2d startingAutoPose){ initalPose2d = startingAutoPose; } - // MIRA public void setOdoPose(Pose2d pose) { - // if (pose == null) return; - // io.tareEverything(); - // initalPose2d = pose; - // io.resetPose(pose); - // robotKnowsWhereItIs = true; - // rotTarget = pose.getRotation().getDegrees(); - // } // public void oneModuleTest(SwerveModule module, Translation2d leftStick, @@ -378,20 +370,41 @@ public class SwerveDrive extends SubsystemBase implements Queryable { driveFieldAngle(leftStick, ang); } + public void defenseXPosition(){ + io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_FRONT_STEER, Rotation2d.fromDegrees(45.0)); + io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER, Rotation2d.fromDegrees(-45.0)); + io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_BACK_STEER, Rotation2d.fromDegrees(-45.0)); + io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_BACK_STEER, Rotation2d.fromDegrees(45.0)); + } - public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, AngularVelocity ballVelocity, double distanceToHub) { + public void stopDefenseXPosition(){ + io.restoreSteerOffsets(); + } + + public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) { + // Calculate the angle between the current position and the lead position + //Rotation2d ang = getPose2d().getTranslation().minus(fieldPos).getAngle(); + Rotation2d ang = new Rotation2d(0,1); + System.out.println(ang); + + 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.in(RotationsPerSecond) > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){ - double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity.in(RotationsPerSecond))); + if (ballVelocity > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){ + double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); Logger.recordOutput("Aim Offset", aimOffset); - // double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity.in(RotationsPerSecond)) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub))); + // double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub))); // Logger.recordOutput("Aim Offset", aimOffset); } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index e295b7f..a0c47d0 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -75,9 +75,10 @@ public final class SwerveDriveConstants { // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); - private static final class ModuleSpecificConstants { //2025 + public static final class ModuleSpecificConstants { //2026 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); + public static final Angle FRONT_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); + public static Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -85,7 +86,8 @@ public final class SwerveDriveConstants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103); + public static final Angle FRONT_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.465332+0.3+0.003174-0.0103); + public static Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; @@ -93,7 +95,8 @@ public final class SwerveDriveConstants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562); + public static final Angle BACK_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.023438+0.5+0.0168-0.00562); + public static Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -101,7 +104,8 @@ public final class SwerveDriveConstants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366); + public static final Angle BACK_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.029541+0.05-0.002197-0.00366); + public static Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java index fa7de1a..8d81397 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -7,8 +7,10 @@ import org.littletonrobotics.junction.AutoLog; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.CanDevice; public interface SwerveIO { @AutoLog @@ -25,6 +27,10 @@ public interface SwerveIO { public default void tareEverything() {} + public default void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {} + + public default void restoreSteerOffsets(){} + public default void resetPose(Pose2d pose) {} public default void addVisionMeasurement(List poses) {} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java index c4ab4d6..ae1c594 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -11,8 +11,10 @@ import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import frc4388.robot.subsystems.vision.Vision; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.CanDevice; public class SwerveReal implements SwerveIO { SwerveDrivetrain swerveDriveTrain; @@ -55,6 +57,15 @@ public class SwerveReal implements SwerveIO { } } + @Override + public void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) { + } + + @Override + public void restoreSteerOffsets() { + + } + @Override public void setLimits(double limitInAmps) { for (SwerveModule module : swerveDriveTrain.getModules()) { diff --git a/src/main/java/frc4388/utility/status/CanDevice.java b/src/main/java/frc4388/utility/status/CanDevice.java index ef72647..ed49c4f 100644 --- a/src/main/java/frc4388/utility/status/CanDevice.java +++ b/src/main/java/frc4388/utility/status/CanDevice.java @@ -49,5 +49,10 @@ public class CanDevice { return s; } + + public Object getId() { + return id; + } + }