From 52ce7f01baa4b01ee1f000d7613f3f1fa0d940e4 Mon Sep 17 00:00:00 2001 From: penguin212 Date: Thu, 4 Apr 2024 00:39:57 -0700 Subject: [PATCH] added shooting while moving? refactor shooter distance getters --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../shooter/ShooterFlywheelSubsystem.java | 27 +-- .../shooter/ShooterPivotSubsystem.java | 25 +-- .../robot/subsystems/swerve/SwerveField.java | 10 ++ .../subsystems/swerve/SwerveSubsystem.java | 158 +++++++++++------- src/main/java/frc/robot/util/GRTUtil.java | 1 + 6 files changed, 120 insertions(+), 105 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/swerve/SwerveField.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 13e454ce..407fbc1e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -158,11 +158,11 @@ public RobotContainer() { intakeRollerSubsystem = new IntakeRollerSubsystem(lightBarSubsystem); shooterPivotSubsystem = new ShooterPivotSubsystem( - swerveSubsystem::getRobotPosition, + swerveSubsystem::getShootingDistance, fmsSubsystem::isRedAlliance ); shooterFlywheelSubsystem = new ShooterFlywheelSubsystem( - swerveSubsystem::getRobotPosition, + swerveSubsystem::getShootingDistance, fmsSubsystem::isRedAlliance ); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java index a85f49be..2b3c0990 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java @@ -23,6 +23,7 @@ import frc.robot.util.Pose2dSupplier; import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; import org.apache.commons.math3.analysis.interpolation.AkimaSplineInterpolator; import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction; @@ -45,7 +46,7 @@ public class ShooterFlywheelSubsystem extends SubsystemBase { private double targetTopRPS; private double targetBottomRPS; - private Pose2dSupplier poseSupplier; + private DoubleSupplier distanceSupplier; private BooleanSupplier redSupplier; private AkimaSplineInterpolator akima; @@ -53,7 +54,6 @@ public class ShooterFlywheelSubsystem extends SubsystemBase { private PolynomialSplineFunction bottomFlywheelSpline; private boolean atSpeed = false; private boolean autoAim = false; - private Pose2d targetPose; private NetworkTable motorsNTTable; private NetworkTableEntry shooter13CurrentEntry; @@ -67,7 +67,7 @@ public class ShooterFlywheelSubsystem extends SubsystemBase { * * @param poseSupplier The poseSupplier for shooter speed calculations. */ - public ShooterFlywheelSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier redSupplier) { + public ShooterFlywheelSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier redSupplier) { //motors shooterMotorTop = new TalonFX(ShooterConstants.SHOOTER_MOTOR_TOP_ID); shooterMotorBottom = new TalonFX(ShooterConstants.SHOOTER_MOTOR_BOTTOM_ID); @@ -107,7 +107,7 @@ public ShooterFlywheelSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier red shooterMotorBottom.getConfigurator().apply(configs); shooterMotorTop.getConfigurator().apply(configs); - this.poseSupplier = poseSupplier; + this.distanceSupplier = distanceSupplier; this.redSupplier = redSupplier; //nts ntInstance = NetworkTableInstance.getDefault(); @@ -226,24 +226,7 @@ public double getTargetBottomRPS() { } public double getShootingDistance() { - double currentDistance; - Pose2d currentField = poseSupplier.getPose2d(); - - if (redSupplier.getAsBoolean()) { //true = red - double xLength = Math.pow(currentField.getX() - SwerveConstants.RED_SPEAKER_POS.getX(), 2); - double yLength = Math.pow(currentField.getY() - SwerveConstants.RED_SPEAKER_POS.getY(), 2); - - currentDistance = Math.sqrt(xLength + yLength); - } else { - double xLength = Math.pow(currentField.getX() - SwerveConstants.BLUE_SPEAKER_POS.getX(), 2); - double yLength = Math.pow(currentField.getY() - SwerveConstants.BLUE_SPEAKER_POS.getY(), 2); - - currentDistance = Math.sqrt(xLength + yLength); - } - - return MathUtil.clamp(currentDistance, - ShooterConstants.MIN_SHOOTER_DISTANCE, ShooterConstants.MAX_SHOOTER_DISTANCE - ); + return distanceSupplier.getAsDouble(); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index 43d10185..9477fd0b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -22,6 +22,7 @@ import frc.robot.util.Pose2dSupplier; import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; import org.apache.commons.math3.analysis.interpolation.AkimaSplineInterpolator; import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction; @@ -47,7 +48,7 @@ public class ShooterPivotSubsystem extends SubsystemBase { private double currentEncoderAngle; private double currentDistance; - private Pose2dSupplier poseSupplier; //new Pose2d(); + private DoubleSupplier distanceSupplier; //new Pose2d(); private BooleanSupplier redSupplier; private AkimaSplineInterpolator akima; @@ -64,10 +65,10 @@ public class ShooterPivotSubsystem extends SubsystemBase { private NetworkTableEntry shooter12TemperatureEntry; /** Inits motors and pose field. Also inits PID stuff. */ - public ShooterPivotSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier redSupplier) { + public ShooterPivotSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier redSupplier) { timer.start(); - this.poseSupplier = poseSupplier; + this.distanceSupplier = distanceSupplier; this.redSupplier = redSupplier; //motors @@ -147,23 +148,7 @@ public void setAngle(double angle) { } private double getShootingDistance() { - Pose2d currentField = poseSupplier.getPose2d(); - - if (redSupplier.getAsBoolean()) { //true = red - double xLength = Math.pow(currentField.getX() - SwerveConstants.RED_SPEAKER_POS.getX(), 2); - double yLength = Math.pow(currentField.getY() - SwerveConstants.RED_SPEAKER_POS.getY(), 2); - - currentDistance = Math.sqrt(xLength + yLength); - } else { - double xLength = Math.pow(currentField.getX() - SwerveConstants.BLUE_SPEAKER_POS.getX(), 2); - double yLength = Math.pow(currentField.getY() - SwerveConstants.BLUE_SPEAKER_POS.getY(), 2); - - currentDistance = Math.sqrt(xLength + yLength); - } - - return MathUtil.clamp(currentDistance, - ShooterConstants.MIN_SHOOTER_DISTANCE, - ShooterConstants.MAX_SHOOTER_DISTANCE); + return distanceSupplier.getAsDouble(); } /** Gets correct Angle for pivot to turn to. */ diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveField.java b/src/main/java/frc/robot/subsystems/swerve/SwerveField.java new file mode 100644 index 00000000..24349ba1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveField.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems.swerve; + +public class SwerveField { + + private SwerveSubsystem swerveSubsystem; + + public SwerveField(SwerveSubsystem swerveSubsystem) { + this.swerveSubsystem = swerveSubsystem; + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 188b431e..122c209e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -59,6 +59,8 @@ import frc.robot.Constants.SwerveConstants; import frc.robot.util.GRTUtil; import frc.robot.vision.ApriltagWrapper; + +import java.math.MathContext; import java.util.Optional; import java.util.function.BooleanSupplier; @@ -68,10 +70,7 @@ public class SwerveSubsystem extends SubsystemBase { private final AHRS ahrs; - private final Timer crimer; - private final Timer ahrsTimer; - - private final Timer lockTimer; + private final Timer lockTimer = new Timer(); private static final double LOCK_TIMEOUT_SECONDS = 1.0; // The elapsed idle time to wait before locking public static final double MAX_VEL = 4.172; //calculated @@ -80,7 +79,8 @@ public class SwerveSubsystem extends SubsystemBase { public static final double MAX_ALPHA = 8; public static final double ANGLE_OFFSET_FOR_AUTO_AIM = Units.degreesToRadians(0); - + public static final double SHOT_SPEED = 40; // meters per sec + private final SwerveModule frontLeftModule; private final SwerveModule frontRightModule; private final SwerveModule backLeftModule; @@ -136,6 +136,9 @@ public class SwerveSubsystem extends SubsystemBase { private Translation2d targetPoint = new Translation2d(); private boolean aiming = false; + private Pose2d lastPose = new Pose2d(); + private Pose2d velocity = new Pose2d(); + private Timer velocityTimer = new Timer(); /** Constructs a {@link SwerveSubsystem}. */ public SwerveSubsystem(BooleanSupplier redSupplier) { @@ -155,12 +158,6 @@ public SwerveSubsystem(BooleanSupplier redSupplier) { inst.startServer(); - crimer = new Timer(); - crimer.start(); - - ahrsTimer = new Timer(); - ahrsTimer.start(); - choreoTab = Shuffleboard.getTab("Auton"); fieldVisualization = new Field2d(); choreoTab.add("Field", fieldVisualization) @@ -184,12 +181,7 @@ public SwerveSubsystem(BooleanSupplier redSupplier) { // Vision measurement standard deviations: [X, Y, theta] MatBuilder.fill(Nat.N3(), Nat.N1(), 0.1, 0.1, 0.01) ); - - lockTimer = new Timer(); - - - // Configure AutoBuilder AutoBuilder.configureHolonomic( this::getRobotPosition, @@ -208,6 +200,8 @@ public SwerveSubsystem(BooleanSupplier redSupplier) { }, this ); + + velocityTimer.start(); } @Override @@ -276,15 +270,16 @@ public void periodic() { printModuleAngles(); } - } + //update velocity + double timePassed = velocityTimer.get(); + velocityTimer.restart(); + double vX = (getRobotPosition().getX() - lastPose.getX()) / timePassed; + double vY = (getRobotPosition().getY() - lastPose.getY()) / timePassed; + double vTheta = (getRobotPosition().getRotation().getRadians() - lastPose.getRotation().getRadians()) + / timePassed; + velocity = new Pose2d(new Translation2d(vX, vY), new Rotation2d(vTheta)); + lastPose = getRobotPosition(); - /** Executes swerve X locking, putting swerve's wheels into an X configuration to prevent motion. - */ - public void applyLock() { - frontLeftModule.setDesiredState(new SwerveModuleState(0.0, new Rotation2d(Math.PI / 4.0))); - frontRightModule.setDesiredState(new SwerveModuleState(0.0, new Rotation2d(-Math.PI / 4.0))); - backLeftModule.setDesiredState(new SwerveModuleState(0.0, new Rotation2d(-Math.PI / 4.0))); - backRightModule.setDesiredState(new SwerveModuleState(0.0, new Rotation2d(Math.PI / 4.0))); } /** @@ -353,20 +348,6 @@ public void setRobotRelativeDrivePowers(ChassisSpeeds robotRelativeSpeeds) { MAX_VEL, MAX_VEL, MAX_OMEGA); } - - /** - * Gets the current chassis speeds relative to the robot. - * - * @return The robot relative chassis speeds. - */ - public ChassisSpeeds getRobotRelativeChassisSpeeds() { - ChassisSpeeds robotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - kinematics.toChassisSpeeds(states), - getRobotPosition().getRotation() // getGyroHeading() - ); - return robotRelativeSpeeds; - } - /** * Sets the chassis speeds. * @@ -389,6 +370,7 @@ public void setChassisSpeeds(double xSpeed, double ySpeed, double angleSpeed) { this.states, speeds, MAX_VEL, MAX_VEL, MAX_OMEGA); } + /** * Sets the drive powers with a heading lock. Field relative. @@ -405,6 +387,51 @@ public void setDrivePowersWithHeadingLock(double xPower, double yPower, Rotation setDrivePowers(xPower, yPower, turnPower); } + /** + * Sets the states of the swerve modules. + * + * @param states The array of swerve modules states + */ + public void setSwerveModuleStates(SwerveModuleState[] states) { + this.states = states; + } + + /** Executes swerve X locking, putting swerve's wheels into an X configuration to prevent motion. + */ + public void applyLock() { + frontLeftModule.setDesiredState(new SwerveModuleState(0.0, new Rotation2d(Math.PI / 4.0))); + frontRightModule.setDesiredState(new SwerveModuleState(0.0, new Rotation2d(-Math.PI / 4.0))); + backLeftModule.setDesiredState(new SwerveModuleState(0.0, new Rotation2d(-Math.PI / 4.0))); + backRightModule.setDesiredState(new SwerveModuleState(0.0, new Rotation2d(Math.PI / 4.0))); + } + + /** + * Gets the current chassis speeds relative to the robot. + * + * @return The robot relative chassis speeds. + */ + public ChassisSpeeds getRobotRelativeChassisSpeeds() { + ChassisSpeeds robotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + kinematics.toChassisSpeeds(states), + getRobotPosition().getRotation() // getGyroHeading() + ); + return robotRelativeSpeeds; + } + + /** + * Gets the module positions. + * + * @return The array of module positions. + */ + public SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + frontLeftModule.getState(), + frontRightModule.getState(), + backLeftModule.getState(), + backRightModule.getState() + }; + } + public void setAim(boolean aiming){ this.aiming = aiming; } @@ -437,6 +464,10 @@ private double getYFromTarget() { return targetPoint.getY() - getRobotPosition().getY(); } + private double getDistanceFromTarget() { + return Math.sqrt(getXFromTarget() * getXFromTarget() + getYFromTarget() * getYFromTarget()); + } + /** Gets pivot angle to any point on the field. */ public double getAngleToTarget() { double xDist = getXFromTarget(); @@ -445,32 +476,17 @@ public double getAngleToTarget() { return Math.atan2(yDist, xDist) + Math.PI; } - /** Returns the PID error for the rotation controller. */ - public double getAngleError() { - return thetaController.getPositionError(); - } + public double getShootingAngle() { + Translation2d shootingPos = getShootingPosition(); + double xDist = shootingPos.getX(); + double yDist = shootingPos.getY(); - /** - * Sets the states of the swerve modules. - * - * @param states The array of swerve modules states - */ - public void setSwerveModuleStates(SwerveModuleState[] states) { - this.states = states; + return Math.atan2(yDist, xDist) + Math.PI; } - /** - * Gets the module positions. - * - * @return The array of module positions. - */ - public SwerveModulePosition[] getModulePositions() { - return new SwerveModulePosition[] { - frontLeftModule.getState(), - frontRightModule.getState(), - backLeftModule.getState(), - backRightModule.getState() - }; + /** Returns the PID error for the rotation controller. */ + public double getAngleError() { + return thetaController.getPositionError(); } /** @@ -489,7 +505,27 @@ public SwerveDriveKinematics getKinematics() { */ public Pose2d getRobotPosition() { return poseEstimator.getEstimatedPosition(); + } + + public Pose2d getRobotVelocity() { + return velocity; + } + + public Translation2d getShootingPosition() { + double shotTime = getDistanceFromTarget() / SHOT_SPEED; + Pose2d robotPos = getRobotPosition(); + Translation2d estimatedPose = new Translation2d(); + for (int i = 0; i < 3; i++) { + double estimatedX = getRobotVelocity().getX() * shotTime + robotPos.getX(); + double estimatedY = getRobotVelocity().getY() * shotTime + robotPos.getY(); + estimatedPose = new Translation2d(estimatedX, estimatedY); + shotTime = estimatedPose.getDistance(getTargetPoint()) / SHOT_SPEED; + } + return estimatedPose; + } + public double getShootingDistance() { + return getShootingPosition().getDistance(targetPoint); } /** diff --git a/src/main/java/frc/robot/util/GRTUtil.java b/src/main/java/frc/robot/util/GRTUtil.java index 4a1a0bde..df5152b5 100644 --- a/src/main/java/frc/robot/util/GRTUtil.java +++ b/src/main/java/frc/robot/util/GRTUtil.java @@ -5,6 +5,7 @@ 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.wpilibj.util.Color; /** Utility functions specific to our repo. */