Skip to content

Commit

Permalink
[Robot] Fix poop command
Browse files Browse the repository at this point in the history
  • Loading branch information
@mvog2501
mvog2501 committed Jul 18, 2024
1 parent 5d5e724 commit 0c1c2fd
Show file tree
Hide file tree
Showing 5 changed files with 16 additions and 59 deletions.
2 changes: 0 additions & 2 deletions Robot/src/main/java/com/swrobotics/robot/commands/IndexerFeedCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@ public void initialize() {
@Override
public void execute() {
indexer.setFeedToShooter(true);
SimView.setShooting(true);
if (!indexer.hasPiece() && !hasStartedTimer) {
timer.start();
hasStartedTimer = true;
Expand All @@ -35,7 +34,6 @@ public void execute() {
@Override
public void end(boolean interrupted) {
indexer.setFeedToShooter(false);
SimView.setShooting(false);
}

@Override
Expand Down
13 changes: 5 additions & 8 deletions Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,11 @@
import com.swrobotics.robot.subsystems.climber.ClimberArm;
import com.swrobotics.robot.subsystems.speaker.IntakeSubsystem;
import com.swrobotics.robot.subsystems.speaker.PivotSubsystem;
import com.swrobotics.robot.subsystems.speaker.ShooterSubsystem;
import com.swrobotics.robot.subsystems.speaker.aim.TableAimCalculator;
import com.swrobotics.robot.subsystems.swerve.SwerveDrive;
import com.swrobotics.robot.subsystems.swerve.SwerveDrive.TurnRequest;

import edu.wpi.first.math.filter.Debouncer.DebounceType;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
Expand Down Expand Up @@ -101,7 +101,7 @@ public ControlBoard(RobotContainer robot) {
.onTrue(Commands.runOnce(() -> robot.intake.set(IntakeSubsystem.State.OFF)));


forceSubwooferTrigger = new Trigger(() -> driver.dpad.down.isPressed()).debounce(0.1).whileTrue(robot.shooter.getSpeakerCommand());
forceSubwooferTrigger = new Trigger(() -> driver.dpad.down.isPressed()).whileTrue(robot.shooter.getSpeakerCommand());

new Trigger(operator.start::isPressed)
.onTrue(Commands.runOnce(robot.indexer::beginReverse))
Expand All @@ -110,11 +110,12 @@ public ControlBoard(RobotContainer robot) {

new Trigger(() -> driver.leftBumper.isPressed()).whileTrue(robot.shooter.getLobCommand()).whileTrue(new AimTowardsLobCommand(robot.drive, robot.shooter));

new Trigger(() -> operator.y.isPressed()).whileTrue(robot.shooter.getAmpCommand());
new Trigger(() -> operator.y.isPressed()).debounce(0.2, DebounceType.kFalling).whileTrue(robot.shooter.getAmpCommand());

new Trigger(() -> operator.b.isPressed()).debounce(0.075).whileTrue(Commands.runOnce(() -> robot.indexer.setFeedToShooter(true))).onFalse(Commands.runOnce(() -> robot.indexer.setFeedToShooter(false)));

new Trigger(() -> operator.b.isPressed() && !driverWantsAim()).debounce(0.075).whileTrue(robot.shooter.getPoopCommand());
// Shooter will check that it would otherwise be idle
new Trigger(() -> operator.b.isPressed()).debounce(0.075).whileTrue(robot.shooter.getPoopCommand());

new Trigger(() -> CHARACTERISE_WHEEL_RADIUS.get()).whileTrue(new CharactarizeWheelCommand(robot.drive));
}
Expand Down Expand Up @@ -184,10 +185,6 @@ public void periodic() {
climbState = ClimbState.IDLE;
robot.indexer.endReverse();
robot.intake.setReverse(false);
robot.shooter.setFlywheelControl(ShooterSubsystem.FlywheelControl.SHOOT); // Always aim with note when not teleop

// robot.drive.setEstimatorIgnoreVision(false && DriverStation.isAutonomous());

return;
}

Expand Down
3 changes: 3 additions & 0 deletions Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
import com.swrobotics.lib.net.NTString;
import com.swrobotics.robot.config.IOAllocation;
import com.swrobotics.robot.config.NTData;
import com.swrobotics.robot.logging.SimView;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
Expand Down Expand Up @@ -32,6 +34,7 @@ public IndexerSubsystem(IntakeSubsystem intake) {

public void setFeedToShooter(boolean feedToShooter) {
this.feedToShooter = feedToShooter;
SimView.setShooting(feedToShooter);
}

public boolean hasPiece() {
Expand Down
41 changes: 8 additions & 33 deletions Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,7 @@
import com.swrobotics.mathlib.MathUtil;
import com.swrobotics.robot.config.NTData;
import com.swrobotics.robot.logging.FieldView;
import com.swrobotics.robot.logging.SimView;
import com.swrobotics.robot.subsystems.speaker.aim.AimCalculator;
import com.swrobotics.robot.subsystems.speaker.aim.AmpAimCalculator;
import com.swrobotics.robot.subsystems.speaker.aim.LobCalculator;
import com.swrobotics.robot.subsystems.speaker.aim.TableAimCalculator;
import com.swrobotics.robot.subsystems.swerve.SwerveDrive;
import edu.wpi.first.math.filter.Debouncer;
Expand All @@ -20,20 +17,12 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public final class ShooterSubsystem extends SubsystemBase {
public enum FlywheelControl {
SHOOT,
POOP,
IDLE,
REVERSE
}

private static final Pose2d blueSpeakerPose = new Pose2d(Units.inchesToMeters(6), 5.5475, new Rotation2d(0)); // Opening
// extends
// 18"
Expand All @@ -49,10 +38,6 @@ public enum FlywheelControl {
private boolean isPreparing;

private AimCalculator.Aim targetAim; // Target aim is null if not currently aiming
private AimCalculator aimCalculator;
private final TableAimCalculator tableAimCalculator;

private FlywheelControl flywheelControl;

public ShooterSubsystem(SwerveDrive drive, IndexerSubsystem indexer) {
this.pivot = new PivotSubsystem();
Expand All @@ -61,15 +46,9 @@ public ShooterSubsystem(SwerveDrive drive, IndexerSubsystem indexer) {
this.drive = drive;
this.indexer = indexer;

// aimCalculator = new ManualAimCalculator();
tableAimCalculator = new TableAimCalculator();
aimCalculator = tableAimCalculator;

NTData.SHOOTER_AFTER_DELAY
.nowAndOnChange((delay) -> afterShootDelay = new Debouncer(delay, Debouncer.DebounceType.kFalling));

flywheelControl = FlywheelControl.SHOOT;

setDefaultCommand(getIdleCommand().ignoringDisable(true));
handleIdle();
}
Expand Down Expand Up @@ -226,7 +205,8 @@ public Command getIdleCommand() {
}

private void handleAmp() {

pivot.setTargetAngle(Math.toRadians(NTData.SHOOTER_AMP_ANGLE.get()) / MathUtil.TAU);
flywheel.setTargetVelocity(NTData.SHOOTER_AMP_VELOCITY.get());
}

public Command getAmpCommand() {
Expand All @@ -251,8 +231,11 @@ public Command getFeedCommand() {
}

public Command getPoopCommand() {
return Commands.run(() -> flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get()), this)
.withName("Shooter Poop");
Command poop = Commands.run(() -> flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get()), this)
.withName("Shooter Poop");

// Every other command takes priority other than idle
return Commands.either(poop.asProxy(), Commands.none(), () -> getCurrentCommand() == getDefaultCommand());
}

NTDouble pctErr = new NTDouble("Shooter/Debug/Percent Error", 0);
Expand All @@ -268,18 +251,10 @@ public boolean isReadyToShoot() {
return isPreparing && flywheel.isReadyToShoot() && pivot.isAtSetpoint();
}

public void setFlywheelControl(FlywheelControl flywheelControl) {
this.flywheelControl = flywheelControl;
}

public double getFlywheelPercentOfTarget() {
return flywheel.getPercentOfTarget();
}

public void setTempAimCalculator(AimCalculator calculator) {
aimCalculator = calculator;
}

public AimCalculator.Aim getTargetAim() {
return targetAim;
}
Expand Down
16 changes: 0 additions & 16 deletions Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/AmpAimCalculator.java
View file

This file was deleted.

0 comments on commit 0c1c2fd

Please sign in to comment.