From 85c02fb4eeed6aba273bd466e55b4519669a0df6 Mon Sep 17 00:00:00 2001 From: penguin212 Date: Fri, 5 Apr 2024 14:46:28 -0700 Subject: [PATCH 1/2] same speed for both flywheels --- src/main/java/frc/robot/RobotContainer.java | 3 +-- .../flywheel/ShooterFlywheelReadyCommand.java | 4 +-- .../shooter/ShooterFlywheelSubsystem.java | 27 +++++++------------ 3 files changed, 12 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 407fbc1e..ceeedc2b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -550,8 +550,7 @@ private void configureBindings() { ) { System.out.println("Dist: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getShootingDistance()) + " Angle: " + GRTUtil.twoDecimals(shooterPivotSubsystem.getPosition()) - + " Top: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getTopMotorSplineSpeed()) - + " Bot: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getBottomMotorSplineSpeed())); + + " Speed: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getSplineSpeed())); } noteInBack = intakeRollerSubsystem.getRockwellSensorValue(); diff --git a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java index be0d60b1..483d6ed7 100644 --- a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java +++ b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java @@ -22,8 +22,8 @@ public ShooterFlywheelReadyCommand(ShooterFlywheelSubsystem shooterSubsystem, Li this.lightBarSubsystem = lightBarSubsystem; addRequirements(shooterSubsystem); - topSpeed = shooterSubsystem.getTopMotorSplineSpeed(); - bottomSpeed = shooterSubsystem.getBottomMotorSplineSpeed(); + topSpeed = shooterSubsystem.getSplineSpeed(); + bottomSpeed = shooterSubsystem.getSplineSpeed(); shooterSubsystem.setAutoAimShooter(true); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java index a8b3cee0..fb23ddd2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java @@ -50,7 +50,7 @@ public class ShooterFlywheelSubsystem extends SubsystemBase { private BooleanSupplier redSupplier; private AkimaSplineInterpolator akima; - private PolynomialSplineFunction topFlywheelSpline; + private PolynomialSplineFunction flywheelSpline; private PolynomialSplineFunction bottomFlywheelSpline; private boolean atSpeed = false; private boolean autoAim = false; @@ -89,15 +89,13 @@ public ShooterFlywheelSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier 5.6, ShooterConstants.MAX_SHOOTER_DISTANCE}; - double[] topSpeeds = {.4, .5, .7, .75, .75, .75, .75}; - double[] bottomSpeeds = {.5, .5, .35, .4, .4, .75, .75}; + double[] speeds = {.4, .5, .7, .75, .75, .75, .75}; targetTopRPS = 0.0; targetBottomRPS = 0.0; akima = new AkimaSplineInterpolator(); - topFlywheelSpline = akima.interpolate(distances, topSpeeds); - bottomFlywheelSpline = akima.interpolate(distances, bottomSpeeds); + flywheelSpline = akima.interpolate(distances, speeds); configs.kP = .5; configs.kI = 0.05; @@ -146,7 +144,7 @@ public void setShooterMotorSpeed(double speed) { /** Sets the shooter to the splined speeds. */ public void setShooterMotorSpeed() { - setShooterMotorSpeed(getTopMotorSplineSpeed(), getBottomMotorSplineSpeed()); + setShooterMotorSpeed(getSplineSpeed(), getSplineSpeed()); } /** Changes auto aim angle. */ @@ -176,17 +174,10 @@ public boolean atSpeed() { * * @return The splined top motor speed. */ - public double getTopMotorSplineSpeed() { - return topFlywheelSpline.value(MathUtil.clamp(getShootingDistance(), ShooterConstants.MIN_SHOOTER_DISTANCE, ShooterConstants.MAX_SHOOTER_DISTANCE)); - } - - /** - * Returns the current spline bottom motor speed. - * - * @return The spline bottom motor speed. - */ - public double getBottomMotorSplineSpeed() { - return bottomFlywheelSpline.value(MathUtil.clamp(getShootingDistance(), ShooterConstants.MIN_SHOOTER_DISTANCE, ShooterConstants.MAX_SHOOTER_DISTANCE)); + public double getSplineSpeed() { + return flywheelSpline.value(MathUtil.clamp( + getShootingDistance(), ShooterConstants.MIN_SHOOTER_DISTANCE, ShooterConstants.MAX_SHOOTER_DISTANCE + )); } /** @@ -233,7 +224,7 @@ public double getShootingDistance() { public void periodic() { //only grabs spline speeds if shooter motor is running (ie not stopped) if (autoAim) { - setShooterMotorSpeed(getTopMotorSplineSpeed(), getBottomMotorSplineSpeed()); + setShooterMotorSpeed(getSplineSpeed(), getSplineSpeed()); } shooter13CurrentEntry.setDouble(shooterMotorTop.getSupplyCurrent().getValueAsDouble()); From 23b6fbb0250b38101f68fcc6fb12e741ce2f2957 Mon Sep 17 00:00:00 2001 From: penguin212 Date: Fri, 5 Apr 2024 17:50:29 -0700 Subject: [PATCH 2/2] tuned new shooter --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 53 +++++++------------ .../shooter/ShooterFlywheelSubsystem.java | 10 ++-- .../shooter/ShooterPivotSubsystem.java | 25 ++++----- .../subsystems/swerve/SwerveSubsystem.java | 2 +- 5 files changed, 38 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b92957ed..18833459 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -128,8 +128,8 @@ public static class ShooterConstants { public static final double BOTTOM_SHOOTER_MOTOR_SPEED = .64; public static final double MAX_FLYWHEEL_RPS = 6380.0 / 60; - public static final double MIN_SHOOTER_DISTANCE = 1.08; - public static final double MAX_SHOOTER_DISTANCE = 8; + public static final double MIN_SHOOTER_DISTANCE = 1.2; + public static final double MAX_SHOOTER_DISTANCE = 7; public static final double FLYWHEEL_SHUTTLE_SPEED = 0.3; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ceeedc2b..dba71ce6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -136,7 +136,7 @@ public class RobotContainer { private final ShuffleboardTab swerveCrauton; private double shooterPivotSetPosition = Units.degreesToRadians(18); - private double shooterTopSpeed = .75; + private double shooterSpeed = .8; private double shooterBotSpeed = .4; private double intakePosition = 0; @@ -325,28 +325,12 @@ private void configureBindings() { shooterPivotSetPosition -= .003; break; - case 45: - shooterTopSpeed += .001; - break; - - case 315: - shooterTopSpeed -= .001; - break; - - case 135: - shooterBotSpeed += .001; - break; - - case 225: - shooterBotSpeed -= .001; - break; - case 90: - intakePosition += .01; + shooterSpeed += .001; break; - + case 270: - intakePosition -= .01; + shooterSpeed -= .001; break; default: @@ -357,8 +341,7 @@ private void configureBindings() { - // System.out.print(" Top: " + GRTUtil.twoDecimals(shooterTopSpeed) - // + " Bot: " + GRTUtil.twoDecimals(shooterBotSpeed) + // System.out.print(" Speed: " + GRTUtil.twoDecimals(shooterSpeed) // ); // shooterPivotSubsystem.getAutoAimAngle(); @@ -372,18 +355,18 @@ private void configureBindings() { // elevatorSubsystem.setManual(); - elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> { - if (mechController.getPOV() == 0) { - elevatorSubsystem.setTargetState(ElevatorState.TRAP); - // elevatorSubsystem.setMotorPower(0.1); - } else if (mechController.getPOV() == 180) { - elevatorSubsystem.setTargetState(ElevatorState.ZERO); - // elevatorSubsystem.setMotorPower(-0.1); - } - else{ - // elevatorSubsystem.setMotorPower(0); - } - }, elevatorSubsystem)); + // elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> { + // if (mechController.getPOV() == 0) { + // elevatorSubsystem.setTargetState(ElevatorState.TRAP); + // // elevatorSubsystem.setMotorPower(0.1); + // } else if (mechController.getPOV() == 180) { + // elevatorSubsystem.setTargetState(ElevatorState.ZERO); + // // elevatorSubsystem.setMotorPower(-0.1); + // } + // else{ + // // elevatorSubsystem.setMotorPower(0); + // } + // }, elevatorSubsystem)); elevatorToZero.onTrue(new ElevatorToZeroCommand(elevatorSubsystem)); /* INTAKE TEST */ @@ -511,7 +494,7 @@ private void configureBindings() { shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> { if (yButton.getAsBoolean()) { lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2); - // shooterFlywheelSubsystem.setShooterMotorSpeed(shooterTopSpeed, shooterBotSpeed); // for tuning + // shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning shooterFlywheelSubsystem.setShooterMotorSpeed(); shooterPivotSubsystem.setAutoAimBoolean(true); if (shooterFlywheelSubsystem.atSpeed()) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java index fb23ddd2..b6698565 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java @@ -72,6 +72,8 @@ public ShooterFlywheelSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier shooterMotorTop = new TalonFX(ShooterConstants.SHOOTER_MOTOR_TOP_ID); shooterMotorBottom = new TalonFX(ShooterConstants.SHOOTER_MOTOR_BOTTOM_ID); + request.EnableFOC = true; + shooterMotorTop.setInverted(true); shooterMotorBottom.setInverted(true); @@ -84,12 +86,12 @@ public ShooterFlywheelSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier double[] distances = {ShooterConstants.MIN_SHOOTER_DISTANCE, 2, 3, - 3.71, - 4.8, - 5.6, + 4, + 5, + 6, ShooterConstants.MAX_SHOOTER_DISTANCE}; - double[] speeds = {.4, .5, .7, .75, .75, .75, .75}; + double[] speeds = {.5, .5, .65, .75, .75, .8, .8}; targetTopRPS = 0.0; targetBottomRPS = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index d2e5de3c..31566ee9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.SwerveConstants; +import frc.robot.util.GRTUtil; import frc.robot.util.Pose2dSupplier; import java.util.function.BooleanSupplier; @@ -104,21 +105,17 @@ public ShooterPivotSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier re double[] distances = {ShooterConstants.MIN_SHOOTER_DISTANCE, 2, 3, - 3.71, - 4.2, + 4, 5, - 5.6, - 7, + 6, ShooterConstants.MAX_SHOOTER_DISTANCE}; - double[] angles = {Units.degreesToRadians(65.5), - Units.degreesToRadians(53.5), - Units.degreesToRadians(46), - Units.degreesToRadians(40), - Units.degreesToRadians(37.5), - Units.degreesToRadians(33.5), - Units.degreesToRadians(32), - Units.degreesToRadians(28), - Units.degreesToRadians(28)}; + double[] angles = {Units.degreesToRadians(60), + Units.degreesToRadians(51), + Units.degreesToRadians(35), + Units.degreesToRadians(29), //4 + Units.degreesToRadians(27.26), + Units.degreesToRadians(26), + Units.degreesToRadians(27.5)}; // X = distances, Y = angles in rads akima = new AkimaSplineInterpolator(); @@ -157,7 +154,7 @@ public double getAutoAimAngle() { currentDistance = getShootingDistance(); // System.out.println("Distance to speaker: " + GRTUtil.twoDecimals(currentDistance) - // + " Set angle: " + GRTUtil.twoDecimals(Units.radiansToDegrees(angleSpline.value(currentDistance))) + // + " Set angle: " + GRTUtil.twoDecimals(Units.radiansToDegrees(angleSpline.value(MathUtil.clamp(currentDistance, ShooterConstants.MIN_SHOOTER_DISTANCE, ShooterConstants.MAX_SHOOTER_DISTANCE)))) // + " Current angle: " + GRTUtil.twoDecimals(Units.radiansToDegrees(rotationEncoder.getPosition()))); return angleSpline.value(MathUtil.clamp(getShootingDistance(), ShooterConstants.MIN_SHOOTER_DISTANCE, ShooterConstants.MAX_SHOOTER_DISTANCE)); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 03bd1f52..4cbfec8d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -285,7 +285,7 @@ public void periodic() { velocity = new Pose2d(new Translation2d(vX, vY), new Rotation2d(vTheta)); lastPose = getRobotPosition(); - System.out.println("shooting pos: " + getShootingPosition()); + // System.out.println("shooting pos: " + getShootingPosition()); } xEntry.setDouble(getShootingPosition().getX()); yEntry.setDouble(getShootingPosition().getY());