Skip to content

Commit

Permalink
Merge pull request #77 from 2491-NoMythic/subwooferAngle
Browse files Browse the repository at this point in the history 
Subwoofer angle
  • Loading branch information
@EchoNotation
EchoNotation authored Feb 24, 2024
2 parents 2a44473 + f5a919a commit 3ec94cf
Show file tree
Hide file tree
Showing 7 changed files with 94 additions and 54 deletions.
20 changes: 11 additions & 9 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,8 @@

import static frc.robot.settings.Constants.PS4Driver.*;
import static frc.robot.settings.Constants.PS4Operator.*;
import static frc.robot.settings.Constants.ShooterConstants.SHOOTING_RPS;
import static frc.robot.settings.Constants.ShooterConstants.LONG_SHOOTING_RPS;
import static frc.robot.settings.Constants.ShooterConstants.SHORT_SHOOTING_RPS;

import java.nio.file.Path;
import java.time.Instant;
Expand Down Expand Up @@ -204,7 +205,7 @@ private void shooterInst() {
}
private void angleShooterInst(){
angleShooterSubsystem = new AngleShooterSubsystem();
defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, driverController::getPOV, driverController::getR1Button);
defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, driverController::getPOV, driverController::getR1Button, driverController::getR1Button, driverController::getR2Button, driverController::getL2Button);
angleShooterSubsystem.setDefaultCommand(defaultShooterAngleCommand);
}
private void intakeInst() {
Expand Down Expand Up @@ -254,7 +255,8 @@ private void configureBindings() {
() -> modifyAxis(-driverController.getRawAxis(Z_AXIS), DEADBAND_NORMAL),
() -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL),
() -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL),
driverController::getL2Button));
driverController::getL2Button,
driverController::getR1Button));

if(Preferences.getBoolean("Detector Limelight", false)) {
new Trigger(driverController::getR1Button).onTrue(new SequentialCommandGroup(
Expand All @@ -276,7 +278,7 @@ private void configureBindings() {
// new Trigger(driverController::getCrossButton).whileTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop()));
new Trigger(driverController::getCrossButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_DOWN))).onFalse(new InstantCommand(()-> climber.climberGo(0)));
new Trigger(driverController::getTriangleButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberGo(0)));
new Trigger(driverController::getSquareButton).whileTrue(new ClimberPullDown(climber));
// new Trigger(driverController::getSquareButton).whileTrue(new ClimberPullDown(climber));
}
if(shooterExists) {
new Trigger(()->driverController.getPOV() == 90).whileTrue(new InstantCommand(()->shooter.shootRPS(ShooterConstants.AMP_RPS), shooter));
Expand All @@ -302,15 +304,15 @@ public boolean runsWhenDisabled() {
* L2: aim at speaker and rev up shooter to max (hold)
* L1: manually feed shooter (hold)
* R2: shoot if everything is lined up (hold)
* R1: automatically pick up note (press)
* Circle: lineup with the amp +shoot at amp speed (hold)
* D-Pad down: move shooter up manually (hold)
* R1: aim shooter at amp (hold)
* Options button: collect note from human player
* Triangle: move climber up (hold)
* Cross: auto-climb down (hold)
* Square: manually pull down with climber (hold)
* Square: auto-pick up note
* Cross: manually pull down with climber (hold)
* Touchpad: manually turn on Intake (hold) [only works if intake code doesn't exist in IndexCommand]
* L1,L2,R1,R2 held: aim shooter at speaker and set shooter to shooter speed
*
*/
//FOR TESTING PURPOSES:
Expand All @@ -319,7 +321,7 @@ public boolean runsWhenDisabled() {
SmartDashboard.putData("intake off", new InstantCommand(intake::intakeOff, intake));
}
if(shooterExists) {
SmartDashboard.putData("shooter on speaker", new InstantCommand(()->shooter.shootRPS(ShooterConstants.SHOOTING_RPS), shooter));
SmartDashboard.putData("shooter on speaker", new InstantCommand(()->shooter.shootRPS(ShooterConstants.LONG_SHOOTING_RPS), shooter));
SmartDashboard.putData("shooter on amp", new InstantCommand(()->shooter.shootRPS(ShooterConstants.AMP_RPS), shooter));
SmartDashboard.putData("shooter off", new InstantCommand(shooter::turnOff, shooter));
}
Expand Down Expand Up @@ -401,7 +403,7 @@ private void registerNamedCommands() {
NamedCommands.registerCommand("stopDrivetrain", new InstantCommand(driveTrain::stop, driveTrain));
NamedCommands.registerCommand("autoPickup", new CollectNote(driveTrain, limelight));

if(shooterExists) {NamedCommands.registerCommand("shooterOn", new InstantCommand(()->shooter.shootRPS(SHOOTING_RPS), shooter));}
if(shooterExists) {NamedCommands.registerCommand("shooterOn", new InstantCommand(()->shooter.shootRPS(LONG_SHOOTING_RPS), shooter));}
if(indexerExists) {NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED), indexer));
NamedCommands.registerCommand("stopFeedingShooter", new InstantCommand(indexer::off, indexer));}
if(intakeExists) {
Expand Down
18 changes: 11 additions & 7 deletions src/main/java/frc/robot/commands/AimRobotMoving.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,11 +32,12 @@ public class AimRobotMoving extends Command {
DoubleSupplier translationXSupplier;
DoubleSupplier translationYSupplier;
BooleanSupplier run;
BooleanSupplier cancel;
DoubleSupplier rotationSupplier;
double rotationSpeed;
double allianceOffset;

public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run){
public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier cancel){
m_drivetrain = drivetrain;
speedController = new PIDController(
AUTO_AIM_ROBOT_kP,
Expand All @@ -47,6 +48,7 @@ public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSup
this.translationXSupplier = translationXSupplier;
this.translationYSupplier = translationYSupplier;
this.rotationSupplier = rotationSupplier;
this.cancel = cancel;
this.run = run;
addRequirements(drivetrain);
}
Expand Down Expand Up @@ -75,11 +77,13 @@ public void execute() {
} else {
allianceOffset = 0;
}
m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds(
translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
rotationSpeed,
new Rotation2d(m_drivetrain.getGyroscopeRotation().getRadians()+allianceOffset)));
if(!cancel.getAsBoolean()) {
m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds(
translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
rotationSpeed,
new Rotation2d(m_drivetrain.getGyroscopeRotation().getRadians()+allianceOffset)));
}
// m_drivetrain.drive(new ChassisSpeeds(
// translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
// translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
Expand All @@ -101,6 +105,6 @@ public void end(boolean interrupted) {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return !run.getAsBoolean();
return (!run.getAsBoolean() || cancel.getAsBoolean());
}
}
25 changes: 18 additions & 7 deletions src/main/java/frc/robot/commands/AimShooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,12 +13,19 @@ public class AimShooter extends Command {
AngleShooterSubsystem angleShooterSubsystem;
DoubleSupplier POVSupplier;
BooleanSupplier humanPlayerSupplier;
BooleanSupplier SubwooferSupplier1;
BooleanSupplier SubwooferSupplier2;
BooleanSupplier SubwooferSupplier3;

public AimShooter(AngleShooterSubsystem angleShooterSubsystem, DoubleSupplier POVSupplier, BooleanSupplier humanPlayerSupplier) {
public AimShooter(AngleShooterSubsystem angleShooterSubsystem, DoubleSupplier POVSupplier, BooleanSupplier humanPlayerSupplier,
BooleanSupplier SubwooferSupplier1, BooleanSupplier SubwooferSupplier2, BooleanSupplier SubwooferSupplier3) {
addRequirements(angleShooterSubsystem);
this.angleShooterSubsystem = angleShooterSubsystem;
this.POVSupplier = POVSupplier;
this.humanPlayerSupplier = humanPlayerSupplier;
this.SubwooferSupplier1 = SubwooferSupplier1;
this.SubwooferSupplier2 = SubwooferSupplier2;
this.SubwooferSupplier3 = SubwooferSupplier3;
}

@Override
Expand All @@ -28,16 +35,20 @@ public void initialize() {

@Override
public void execute() {
if (POVSupplier.getAsDouble() == 90 || POVSupplier.getAsDouble() == 45 || POVSupplier.getAsDouble() == 135) {
angleShooterSubsystem.setDesiredShooterAngle(SmartDashboard.getNumber("amp angle", Field.AMPLIFIER_ANGLE)/*Field.AMPLIFIER_ANGLE*/);
if(SubwooferSupplier1.getAsBoolean()&&SubwooferSupplier2.getAsBoolean()&&SubwooferSupplier3.getAsBoolean()) {
angleShooterSubsystem.setDesiredShooterAngle(Field.SUBWOOFER_ANGLE);
} else {
if(humanPlayerSupplier.getAsBoolean()) {
angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE);
if (POVSupplier.getAsDouble() == 90 || POVSupplier.getAsDouble() == 45 || POVSupplier.getAsDouble() == 135) {
angleShooterSubsystem.setDesiredShooterAngle(SmartDashboard.getNumber("amp angle", Field.AMPLIFIER_ANGLE)/*Field.AMPLIFIER_ANGLE*/);
} else {
angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle());
if(humanPlayerSupplier.getAsBoolean()) {
angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE);
} else {
angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle());
}
}
}
}
}

@Override
public boolean isFinished() {
Expand Down
68 changes: 41 additions & 27 deletions src/main/java/frc/robot/commands/IndexCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,18 +41,24 @@ public class IndexCommand extends Command {
AngleShooterSubsystem angleShooterSubsytem;
boolean auto;
double runsEmpty = 0;
BooleanSupplier SubwooferSupplier1;
BooleanSupplier SubwooferSupplier2;
BooleanSupplier SubwooferSupplier3;

/** Creates a new IndexCommand. */
public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier, DoubleSupplier ampSupplier) {
this.m_Indexer = m_IndexerSubsystem;
this.shootIfReadySupplier = shootIfReadySupplier;
this.revUpSupplier = revUpSupplier;
this.shootIfReadySupplier = shootIfReadySupplier;//R2
this.revUpSupplier = revUpSupplier;//L2
this.shooter = shooter;
this.intake = intake;
this.drivetrain = drivetrain;
this.ampSupplier = ampSupplier;
this.angleShooterSubsytem = angleShooterSubsystem;
this.humanPlayerSupplier = humanPlaySupplier;
this.humanPlayerSupplier = humanPlaySupplier;//R1
this.SubwooferSupplier1 = SubwooferSupplier1;
this.SubwooferSupplier2 = SubwooferSupplier2;
this.SubwooferSupplier3 = SubwooferSupplier3;
SmartDashboard.putNumber("amp RPS", AMP_RPS);
SmartDashboard.putNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED);
SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_ANGLE);
Expand Down Expand Up @@ -91,31 +97,39 @@ public void execute() {
} else {
runsEmpty = 0;
intake.intakeOff();
if(revUpSupplier.getAsBoolean()) {
shooter.shootRPS(ShooterConstants.SHOOTING_RPS);
if(shootIfReadySupplier.getAsBoolean()&&revUpSupplier.getAsBoolean()&&humanPlayerSupplier.getAsBoolean()) {
shooter.shootRPS(ShooterConstants.SUBWOOFER_RPS);
} else {
shooter.shootRPS(SmartDashboard.getNumber("amp RPS", ShooterConstants.AMP_RPS)/*ShooterConstants.AMP_RPS*/);
}
boolean indexer = false;
if(angleShooterSubsytem.validShot() && drivetrain.validShot() && shooter.validShot()) {
RobotState.getInstance().ShooterReady = true;
if (shootIfReadySupplier.getAsBoolean()) {
indexer = true;
}
} else {
RobotState.getInstance().ShooterReady = false;
}
if(SmartDashboard.getBoolean("feedMotor", false)) {
indexer = true;
}
if (indexer) {
if(ampSupplier.getAsDouble() == 90) {
m_Indexer.set(SmartDashboard.getNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED));
} else {
m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
}
} else {
m_Indexer.off();
if(revUpSupplier.getAsBoolean()) {
if(angleShooterSubsytem.shortSpeakerDist()) {
shooter.shootRPS(ShooterConstants.SHORT_SHOOTING_RPS);
} else {
shooter.shootRPS(ShooterConstants.LONG_SHOOTING_RPS);
}
} else {
shooter.shootRPS(SmartDashboard.getNumber("amp RPS", ShooterConstants.AMP_RPS)/*ShooterConstants.AMP_RPS*/);
}
boolean indexer = false;
if(angleShooterSubsytem.validShot() && drivetrain.validShot() && shooter.validShot()) {
RobotState.getInstance().ShooterReady = true;
if (shootIfReadySupplier.getAsBoolean()) {
indexer = true;
}
} else {
RobotState.getInstance().ShooterReady = false;
}
if(SmartDashboard.getBoolean("feedMotor", false)) {
indexer = true;
}
if (indexer) {
if(ampSupplier.getAsDouble() == 90) {
m_Indexer.set(SmartDashboard.getNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED));
} else {
m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
}
} else {
m_Indexer.off();
}
}
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/NamedCommands/initialShot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ public initialShot(ShooterSubsystem shooter, IndexerSubsystem indexer, double re
@Override
public void initialize() {
timer.start();
shooter.shootRPS(ShooterConstants.SHOOTING_RPS);
shooter.shootRPS(ShooterConstants.SHORT_SHOOTING_RPS);
}

// Called every time the scheduler runs while the command is scheduled.
Expand Down
9 changes: 6 additions & 3 deletions src/main/java/frc/robot/settings/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -274,10 +274,11 @@ public static final class ShooterConstants{
public static final double AUTO_AIM_ROBOT_kP = 0.125;
public static final double AUTO_AIM_ROBOT_kI = 0.00;
public static final double AUTO_AIM_ROBOT_kD = 0.00;

public static final double SHOOTING_RPS = 90;

public static final double LONG_SHOOTING_RPS = 90;
public static final double SHORT_SHOOTING_RPS = 80;
public static final double AMP_RPS = 7.5;

public static final double SUBWOOFER_RPS = SHORT_SHOOTING_RPS;

//the PID values used on the PID loop on the motor controller that control the position of the shooter angle
public static final double ANGLE_SHOOTER_POWER_KP = 0.026;
Expand Down Expand Up @@ -426,8 +427,10 @@ public final class Field{
public static final double SPEAKER_Y = 5.3;//16.412;
public static final double SPEAKER_Z = 2.08; //height of opening
public static final double MAX_SHOOTING_DISTANCE = 2491;
public static final double SHORT_RANGE_SHOOTING_DIST = 3;

public static final double AMPLIFIER_ANGLE = 101;
public static final double SUBWOOFER_ANGLE = 80;
//angle at 60 for bounce techinque, didn't work
}

Expand Down
6 changes: 6 additions & 0 deletions src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ public class AngleShooterSubsystem extends SubsystemBase {
public static ChassisSpeeds DTChassisSpeeds;
public double desiredZeroOffset;
int runsValid;
double speakerDistGlobal;

public AngleShooterSubsystem() {
runsValid = 0;
Expand Down Expand Up @@ -139,11 +140,16 @@ public double calculateSpeakerAngle() {
SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle);

double differenceAngle = (desiredShooterAngle - this.getShooterAngle());
speakerDistGlobal = offsetSpeakerdist;
// SmartDashboard.putNumber("differenceAngleShooter", differenceAngle);

return desiredShooterAngle;
}

public boolean shortSpeakerDist() {
return speakerDistGlobal<=Field.SHORT_RANGE_SHOOTING_DIST;
}

private double calculateSpeakerAngleDifference() {
return Math.abs(calculateSpeakerAngle() - this.getShooterAngle());
}
Expand Down

0 comments on commit 3ec94cf

Please sign in to comment.