Skip to content

Commit

Permalink
added SetShooterFromLocationPreset Command in configureShooter (#116)
Browse files Browse the repository at this point in the history 
* added SetShooterFromLocationPreset Command in configureShooter

* Set Presets

* Changed left controller to right

* Merged with main

* Some Changes, Adds Changing on the fly

Co-authored-by: StarDylan <dylan@starink.com>
  • Loading branch information
@viveknadig282 @StarDylan
viveknadig282 and StarDylan authored Mar 24, 2022
1 parent 0f5a6e3 commit 56f80f5
Show file tree
Hide file tree
Showing 6 changed files with 51 additions and 51 deletions.
6 changes: 2 additions & 4 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -360,10 +360,8 @@ public static class ShooterConstants {

// Presets
public static final HashMap<ShooterLocationPreset, ShooterPreset> ALL_SHOOTER_PRESETS = HashMapFiller.populateHashMap(
entry(ShooterLocationPreset.FENDER, new ShooterPreset(100, 1.23, 10, "Fender")),
entry(ShooterLocationPreset.TARMAC_MIDDLE_VERTEX, new ShooterPreset(200, 2.34, 20, "Tarmac Middle Vertex")),
entry(ShooterLocationPreset.TARMAC_SIDE_VERTEX, new ShooterPreset(200, 2.34, 20, "Tarmac Side Vertex")),
entry(ShooterLocationPreset.TRUSS, new ShooterPreset(200, 2.34, 20, "Truss"))
entry(ShooterLocationPreset.LAUNCHPAD, new ShooterPreset(2600, 235000, 0, "Launchpad")),
entry(ShooterLocationPreset.TARMAC_VERTEX, new ShooterPreset(2290, 140000, 0, "Tarmac Vertex"))
); // TODO: Create all shooter presets

// Velocity Training Points
Expand Down
32 changes: 19 additions & 13 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
import frc.robot.helper.ControllerUtil;
import frc.robot.helper.DPadButton;
import frc.robot.helper.JoystickAnalogButton;
import frc.robot.helper.shooter.ShooterState;
import frc.robot.subsystems.FlywheelSubsystem.ShooterLocationPreset;
import frc.robot.subsystems.*;

import java.awt.Robot;
Expand Down Expand Up @@ -197,32 +197,37 @@ private void configureDrivetrain() {
}

private void configureShooter() {

DPadButton dPadUp = new DPadButton(operatorController, DPadButton.Direction.UP);
DPadButton dPadDown = new DPadButton(operatorController, DPadButton.Direction.DOWN);
DPadButton dPadRight = new DPadButton(operatorController, DPadButton.Direction.RIGHT);
DPadButton dPadLeft= new DPadButton(operatorController, DPadButton.Direction.LEFT);
DPadButton dPadLeft = new DPadButton(operatorController, DPadButton.Direction.LEFT);

JoystickAnalogButton operatorRightTrigger = new JoystickAnalogButton(operatorController, XboxController.Axis.kRightTrigger.value);
JoystickAnalogButton operatorLeftTrigger = new JoystickAnalogButton(operatorController, XboxController.Axis.kLeftTrigger.value);
operatorRightTrigger.setThreshold(0.1);

dPadRight.whenPressed(new SetShooterPreset(flywheelSubsystem, ShooterLocationPreset.LAUNCHPAD));
dPadLeft.whenPressed(new SetShooterPreset(flywheelSubsystem, ShooterLocationPreset.TARMAC_VERTEX));

dPadUp.whenHeld(new ZeroHoodMotorCommand(flywheelSubsystem));

// dPadUp.whenPressed(new SetShooterPreset(flywheelSubsystem, ShooterLocationPreset.FENDER));
// dPadDown.whenPressed(new SetShooterPreset(flywheelSubsystem, ShooterLocationPreset.TARMAC_MIDDLE_VERTEX));
// dPadRight.whenPressed(new SetShooterPreset(flywheelSubsystem, ShooterLocationPreset.TARMAC_SIDE_VERTEX));
// dPadLeft.whenPressed(new SetShooterPreset(flywheelSubsystem, ShooterLocationPreset.TRUSS));
operatorRightTrigger.whenHeld(new SetShooterPIDVelocityFromState(
flywheelSubsystem,
flywheelSubsystem::getFlywheelShooterStateFromPreset,
operatorController));

operatorRightTrigger.whenHeld( new SetShooterPIDVelocityFromState(flywheelSubsystem, new ShooterState(2290, 140000), operatorController)); //TODO: Replace me with Presets

// Vibrations
if (TRANSFER) {
operatorLeftTrigger.whenHeld(new TransferShootForward(transferSubsystem), false);
new Button(()-> transferSubsystem.getCurrentBallCount() >= MAX_BALL_COUNT).whenPressed(new WaitAndVibrateCommand(driverController, 0.5, 0.5));
new Button(() -> transferSubsystem.getCurrentBallCount() >= MAX_BALL_COUNT).whenPressed(new WaitAndVibrateCommand(driverController, 0.1, 0.1));
}
dPadUp.whenHeld(new ZeroHoodMotorCommand(flywheelSubsystem)).whenPressed(new InstantCommand(()->System.out.println("Activated Zero")));

new Button(() -> flywheelSubsystem.isAtSetPoint(flywheelSubsystem.getCurrentTargetSpeed())).whenPressed(new WaitAndVibrateCommand(operatorController, 0.5));

}

private void configureTransfer() {
JoystickAnalogButton operatorLeftTrigger = new JoystickAnalogButton(driverController, XboxController.Axis.kLeftTrigger.value);

operatorLeftTrigger.whenHeld(new TransferIndexForward(transferSubsystem), false);
}

private void configureIntake() {
Expand All @@ -249,6 +254,7 @@ private void configureIntake() {
}

private void configureHanger() {
//TODO: IF we are doing traversal, Ensure that Intake is Down with Commands

JoystickButton operatorAButton = new JoystickButton(operatorController, XboxController.Button.kA.value);
JoystickButton operatorXButton = new JoystickButton(operatorController, XboxController.Button.kX.value);
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/auto/Paths.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -252,7 +252,7 @@ private static Command getShootCommand(double timeToShoot) {
new ParallelDeadlineGroup( // TODO dont be bad
new WaitCommand(timeToShoot * 0.7),
new AutoAlignInPlaceCommand(driveSubsystem),
new SetShooterPIDVelocityFromState(flywheelSubsystem, new ShooterState( 2450, 140000)), //TODO: FIX ME (TESTING)
new SetShooterPIDVelocityFromState(flywheelSubsystem, ()->new ShooterState( 2450, 140000)), //TODO: FIX ME (TESTING)
new WaitCommand(timeToShoot * 0.2).andThen(
new InstantCommand(
() -> CommandScheduler.getInstance().schedule(transferForward)
Expand All @@ -264,7 +264,7 @@ private static Command getShootCommand(double timeToShoot) {
}

private static Command getRevUpCommand() {
return new SetShooterPIDVelocityFromState(flywheelSubsystem, new ShooterState( 2450, 140000)); //TODO: FIX ME (TESTING)
return new SetShooterPIDVelocityFromState(flywheelSubsystem, ()->new ShooterState( 2450, 140000)); //TODO: FIX ME (TESTING)
}
}

Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/commands/shooter/SetShooterFromLocationPreset.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,16 +8,14 @@

import static frc.robot.Constants.ShooterConstants.ALL_SHOOTER_PRESETS;

@Deprecated
public class SetShooterFromLocationPreset extends CommandBase {
private FlywheelSubsystem flywheelSubsystem;
private ShooterLocationPreset shooterLocationPreset;
private ShooterState shooterState;

public SetShooterFromLocationPreset(FlywheelSubsystem flywheelSubsystem) {
this.shooterLocationPreset = flywheelSubsystem.getShooterLocationPreset();
this.flywheelSubsystem = flywheelSubsystem;

addRequirements(flywheelSubsystem);
this(flywheelSubsystem, flywheelSubsystem.getShooterLocationPreset());
}

public SetShooterFromLocationPreset(FlywheelSubsystem flywheelSubsystem, ShooterLocationPreset preset) {
Expand All @@ -31,9 +29,11 @@ public SetShooterFromLocationPreset(FlywheelSubsystem flywheelSubsystem, Shooter
public void initialize() {
if (shooterLocationPreset != null) {
flywheelSubsystem.setShooterLocationPreset(shooterLocationPreset);
} else {
shooterLocationPreset = ShooterLocationPreset.TARMAC_VERTEX;
}
shooterState = ALL_SHOOTER_PRESETS.get(flywheelSubsystem.getShooterLocationPreset()).shooterState;

shooterState = ALL_SHOOTER_PRESETS.get(shooterLocationPreset).shooterState;

flywheelSubsystem.setSpeed(shooterState.rpmVelocity);
flywheelSubsystem.setHoodAngle(shooterState.hoodAngle);
Expand Down
33 changes: 13 additions & 20 deletions src/main/java/frc/robot/commands/shooter/SetShooterPIDVelocityFromState.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package frc.robot.commands.shooter;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
Expand All @@ -12,30 +11,27 @@

import java.math.BigDecimal;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;


public class SetShooterPIDVelocityFromState extends CommandBase {
private FlywheelSubsystem.ShooterLocationPreset shooterLocationPreset;

private PIDController flywheelControllerFar;
private PIDController flywheelControllerLow;
private DoubleSupplier flywheelRPMSupplier;

private FlywheelSubsystem flywheelSubsystem;
private ShooterState shooterState;
private Supplier<ShooterState> shooterStateSupplier;

public SetShooterPIDVelocityFromState(FlywheelSubsystem flywheelSubsystem, ShooterState shooterState) {
public SetShooterPIDVelocityFromState(FlywheelSubsystem flywheelSubsystem, Supplier<ShooterState> shooterStateSupplier) {
this.flywheelSubsystem = flywheelSubsystem;
this.shooterState = shooterState;
this.flywheelRPMSupplier = flywheelRPMSupplier;
this.shooterStateSupplier = shooterStateSupplier;

flywheelControllerFar = new PIDController(0.0005,0,0.000008);
flywheelControllerLow = new PIDController(0.00025,0,0.000008);
}

public SetShooterPIDVelocityFromState(FlywheelSubsystem flywheelSubsystem, ShooterState shooterState, XboxController operatorController) {
this(flywheelSubsystem, shooterState);
new Button(() -> flywheelSubsystem.isAtSetPoint(shooterState.rpmVelocity)).whenPressed(new WaitAndVibrateCommand(operatorController, 0.5, 0.1));
public SetShooterPIDVelocityFromState(FlywheelSubsystem flywheelSubsystem, Supplier<ShooterState> shooterStateSupplier, XboxController operatorController) {
this(flywheelSubsystem, shooterStateSupplier);
new Button(() -> flywheelSubsystem.isAtSetPoint(shooterStateSupplier.get().rpmVelocity)).whenPressed(new WaitAndVibrateCommand(operatorController, 0.5, 0.1));
}

@Override
Expand All @@ -47,18 +43,18 @@ public void initialize() {
@Override
public void execute() {

double pidOutput;

double pidOutput = 0;
if (shooterState.rpmVelocity < 3500){
pidOutput = flywheelControllerLow.calculate(flywheelSubsystem.getFlywheelRPM(), shooterState.rpmVelocity);
if (shooterStateSupplier.get().rpmVelocity < 3500){
pidOutput = flywheelControllerLow.calculate(flywheelSubsystem.getFlywheelRPM(), shooterStateSupplier.get().rpmVelocity);
} else {
pidOutput = flywheelControllerFar.calculate(flywheelSubsystem.getFlywheelRPM(), shooterState.rpmVelocity);
pidOutput = flywheelControllerFar.calculate(flywheelSubsystem.getFlywheelRPM(), shooterStateSupplier.get().rpmVelocity);
}

BigDecimal KF_PERCENT_FACTOR_FLYWHEEL = new BigDecimal("0.00018082895");
BigDecimal KF_CONSTANT = new BigDecimal("0.0159208876");

BigDecimal feedforward = (new BigDecimal(shooterState.rpmVelocity).multiply(KF_PERCENT_FACTOR_FLYWHEEL)).add(KF_CONSTANT);
BigDecimal feedforward = (new BigDecimal(shooterStateSupplier.get().rpmVelocity).multiply(KF_PERCENT_FACTOR_FLYWHEEL)).add(KF_CONSTANT);

double feedForwardedPidOutput = pidOutput + feedforward.doubleValue();

Expand All @@ -67,10 +63,7 @@ public void execute() {
double clampedPositiveFinalMotorOutput = (positiveMotorOutput > 1) ? 1 : positiveMotorOutput;

flywheelSubsystem.setPercentSpeed(clampedPositiveFinalMotorOutput);
flywheelSubsystem.setHoodAngle(shooterState.hoodAngle);

SmartDashboard.putNumber("Flywheel Output",clampedPositiveFinalMotorOutput);

flywheelSubsystem.setHoodAngle(shooterStateSupplier.get().hoodAngle);
}

@Override
Expand Down
17 changes: 10 additions & 7 deletions src/main/java/frc/robot/subsystems/FlywheelSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,8 @@

public class FlywheelSubsystem extends SubsystemBase {
public enum ShooterLocationPreset {
FENDER,
TARMAC_SIDE_VERTEX,
TARMAC_MIDDLE_VERTEX,
TRUSS
LAUNCHPAD,
TARMAC_VERTEX,
}

private int currentPresetNumber = 0;
Expand All @@ -43,7 +41,7 @@ public enum ShooterLocationPreset {

private double currentTargetSpeed;

private ShooterLocationPreset shooterLocationPreset = ShooterLocationPreset.FENDER;
private ShooterLocationPreset shooterLocationPreset = ShooterLocationPreset.TARMAC_VERTEX;

private PiecewiseBicubicSplineInterpolatingFunction velocityInterpolatingFunction;
private PiecewiseBicubicSplineInterpolatingFunction hoodAngleInterpolatingFunction;
Expand Down Expand Up @@ -116,7 +114,7 @@ public void autoPresetAutoAim(double distance) {
}

public void setShooterLocationPreset(ShooterLocationPreset preset) {
SmartDashboard.putString("Shooter Preset: ",preset.toString());
SmartDashboard.putString("Shooter Preset: ", preset.toString());
logger.info("Shooter Preset Changed to " + preset);
this.shooterLocationPreset = preset;
}
Expand All @@ -129,6 +127,7 @@ public ShooterLocationPreset getShooterLocationPreset() {
* @param velocity Angular Velocity in (rev/s)
* Flywheel speed is set by integrated PID controller
*/
@Deprecated
public void setSpeed(double velocity) {
// formula for converting m/s to sensor units/100ms
currentTargetSpeed = fromRpmToSu(velocity); // rev/s * 1s/10 (100ms) * 2048su/1rev
Expand All @@ -137,7 +136,7 @@ public void setSpeed(double velocity) {

/**
* @param percent Velocity from min to max as percent from xbox controller (0% - 100%)
* Flywheel speed is set by integrated PID controller
* Flywheel speed is set by integrated get controller
*/
public void setPercentSpeed(double percent) {
masterLeftShooterMotor.set(ControlMode.PercentOutput, percent);
Expand Down Expand Up @@ -193,6 +192,10 @@ public void stopFullShooter() {
/*
* Confirms if velocity is within margin of set point
*/
public double getCurrentTargetSpeed() {
return currentTargetSpeed;
}

public boolean isAtSetPoint(double setpoint) {
double velocity = -getVelocity();

Expand Down

0 comments on commit 56f80f5

Please sign in to comment.