Skip to content

Commit

Permalink
Merge pull request #71 from grt192/shooter-tuning
Browse files Browse the repository at this point in the history 
Shooter tuning
  • Loading branch information
@CrolineCrois
CrolineCrois authored Apr 6, 2024
2 parents 58ebaea + 23b6fbb commit 6a14881
Show file tree
Hide file tree
Showing 6 changed files with 49 additions and 77 deletions.
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
56 changes: 19 additions & 37 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,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;

Expand Down Expand Up @@ -341,28 +341,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:
Expand All @@ -373,8 +357,7 @@ private void configureBindings() {



// System.out.print(" Top: " + GRTUtil.twoDecimals(shooterTopSpeed)
// + " Bot: " + GRTUtil.twoDecimals(shooterBotSpeed)
// System.out.print(" Speed: " + GRTUtil.twoDecimals(shooterSpeed)
// );

// shooterPivotSubsystem.getAutoAimAngle();
Expand All @@ -388,18 +371,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 */
Expand Down Expand Up @@ -519,7 +502,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()) {
Expand Down Expand Up @@ -558,8 +541,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();

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down
35 changes: 14 additions & 21 deletions src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);

Expand All @@ -84,20 +86,18 @@ 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[] topSpeeds = {.4, .5, .7, .75, .75, .75, .75};
double[] bottomSpeeds = {.5, .5, .35, .4, .4, .75, .75};
double[] speeds = {.5, .5, .65, .75, .75, .8, .8};

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;
Expand Down Expand Up @@ -146,7 +146,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. */
Expand Down Expand Up @@ -176,17 +176,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
));
}

/**
Expand Down Expand Up @@ -233,7 +226,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());
Expand Down
25 changes: 11 additions & 14 deletions src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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));
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down

0 comments on commit 6a14881

Please sign in to comment.