Skip to content

Commit

Permalink
added flags to clean up logic
Browse files Browse the repository at this point in the history 
flags centralize everything that can be measured about the robot
  • Loading branch information
@teja-yaramada
teja-yaramada committed Oct 9, 2024
1 parent d653087 commit bf05f00
Show file tree
Hide file tree
Showing 3 changed files with 147 additions and 48 deletions.
36 changes: 36 additions & 0 deletions src/main/java/frc/team3128/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package frc.team3128;

import java.util.HashMap;
import java.util.function.BooleanSupplier;

import com.ctre.phoenix6.hardware.CANcoder;
import com.pathplanner.lib.path.PathConstraints;
Expand All @@ -24,6 +25,10 @@
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.team3128.subsystems.Amper;
import frc.team3128.subsystems.Hopper;
import frc.team3128.subsystems.Intake;
import frc.team3128.subsystems.Shooter;
import frc.team3128.subsystems.Swerve;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
Expand Down Expand Up @@ -443,6 +448,37 @@ public static class AmperConstants {

public static final double AMPER_ANGLE = 31.96;
}

public static class Flags {
public static final BooleanSupplier hopperHasNote = ()-> Hopper.getInstance().hasObjectPresent();
public static final BooleanSupplier shooterHasNote = ()-> Shooter.getInstance().noteInRollers();
public static final BooleanSupplier hasNoNotes = both(not(hopperHasNote), not(shooterHasNote));
public static final BooleanSupplier hasOneNote = xor(hopperHasNote, shooterHasNote);
public static final BooleanSupplier hasTwoNotes = both(hopperHasNote, shooterHasNote);

public static final BooleanSupplier readyForAdvance = both(hopperHasNote, not(shooterHasNote));

public static final BooleanSupplier intakeOut = ()-> Intake.getInstance().getMeasurement() > 20;
public static final BooleanSupplier amperOut = ()-> Amper.getInstance().getMeasurement() > 3;

public static final BooleanSupplier shooterRunning = ()-> ShooterConstants.SHOOTER_MOTOR.getAppliedOutput() > 0.1;

public static final BooleanSupplier amperStalled = ()-> AmperConstants.ROLLER_MOTOR.getStallCurrent() > 40;
public static final BooleanSupplier hopperStalled = ()-> HopperConstants.HOPPER_MOTOR.getStallCurrent() > 40;
public static final BooleanSupplier intakeStalled = ()-> IntakeConstants.ROLLER_MOTOR.getStallCurrent() > 40;

public static BooleanSupplier not(BooleanSupplier a) {
return ()-> !a.getAsBoolean();
}

public static BooleanSupplier both(BooleanSupplier a, BooleanSupplier b) {
return ()-> a.getAsBoolean() && b.getAsBoolean();
}

public static BooleanSupplier xor(BooleanSupplier a, BooleanSupplier b) {
return ()-> a.getAsBoolean() ^ b.getAsBoolean();
}
}
}


135 changes: 111 additions & 24 deletions src/main/java/frc/team3128/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,14 +13,11 @@
import edu.wpi.first.wpilibj2.command.button.Trigger;

import static edu.wpi.first.wpilibj2.command.Commands.*;
import static frc.team3128.Constants.ShooterConstants.EDGE_FEED_ANGLE;
import static frc.team3128.Constants.ShooterConstants.EDGE_FEED_RPM;
import static frc.team3128.Constants.ShooterConstants.KICK_POWER;
import static frc.team3128.Constants.ShooterConstants.KICK_SHOOTING_POWER;
import static frc.team3128.Constants.ShooterConstants.MAX_RPM;
import static frc.team3128.Constants.ShooterConstants.MIDDLE_FEED_ANGLE;
import static frc.team3128.Constants.ShooterConstants.MIDDLE_FEED_RPM;
import static frc.team3128.Constants.HopperConstants.HOPPER_INTAKE_POWER;
import static frc.team3128.Constants.ShooterConstants.*;
import static frc.team3128.Constants.IntakeConstants.*;
import static frc.team3128.Constants.AmperConstants.*;
import static frc.team3128.Constants.HopperConstants.*;
import static frc.team3128.Constants.Flags.*;
import static frc.team3128.commands.CmdManager.*;

import org.photonvision.PhotonPoseEstimator.PoseStrategy;
Expand Down Expand Up @@ -108,6 +105,7 @@ public RobotContainer() {
initCameras();

configureButtonBindings();
initTrigges();


// NAR_Shuffleboard.addData("Limelight", "ValidTarget", ()-> limelight.hasValidTarget(), 0, 0);
Expand Down Expand Up @@ -144,7 +142,7 @@ private void configureButtonBindings() {
controller.getButton(XboxButton.kY).onTrue(shooter.runShooter(0));
controller.getButton(XboxButton.kB).onTrue(shooter.runKickMotor(KICK_SHOOTING_POWER)).onFalse(shooter.runKickMotor(0));

controller.getButton(XboxButton.kY).whileTrue(ampUp()).onFalse(ampFinAndDown());
controller.getButton(XboxButton.kY).whileTrue(amper.partExtend()).onFalse(ampFinAndDown());

controller2.getButton(XboxButton.kA).onTrue(runOnce(()-> intake.disable()).andThen(intake.reset(0)));
controller2.getButton(XboxButton.kB).onTrue(runOnce(()-> amper.disable()).andThen(amper.reset(0)));
Expand All @@ -153,10 +151,79 @@ private void configureButtonBindings() {



// // new Trigger(()->true).onTrue(queueNote());

// //Shooting
// new Trigger(()-> shooter.getShooting())
// .onTrue(sequence(
// shooter.runKickMotor(KICK_POWER),
// hopper.runManipulator(.8)
// ))
// .onFalse(sequence(
// hopper.runManipulator(0),
// shooter.stopMotors()
// ));

// //Stops shooting when all notes are gone
// new Trigger(()-> !shooter.noteInRollers())
// .and(()-> !hopper.hasObjectPresent())
// .debounce(0.5)
// .onTrue(sequence(
// shooter.setShooting(false),
// runOnce(() -> leds.setLedColor(Colors.BLUE))
// ));

// //Queues note to hopper
// new Trigger(()-> intake.getMeasurement() > 90)
// .and(()->!hopper.hasObjectPresent())
// .onTrue(hopper.runManipulator(HOPPER_INTAKE_POWER))
// .onFalse(hopper.runManipulator(0));

// //Stops hopper if intake is retracted and is empty
// new Trigger(()-> intake.getMeasurement() < 20)
// .and(()->hopper.hasObjectPresent()).negate()
// .onTrue(hopper.runManipulator(0));

// //Queues note to shooter
// new Trigger(()-> shooter.noteInRollers()).negate()
// .and(()->hopper.hasObjectPresent())
// .and(() -> !shooter.getShooting())
// .onTrue(sequence(
// runOnce(() -> leds.blinkLEDColor(Colors.RED, Colors.GREEN, .25)),
// shooter.runKickMotor(KICK_POWER),
// hopper.runManipulator(HOPPER_INTAKE_POWER)
// ))
// .onFalse(sequence(
// shooter.runKickMotor(-.1),
// waitSeconds(.1),
// shooter.runKickMotor(0)
// ));

// // new Trigger(()-> !shooter.noteInRollers())
// // .debounce(0.25)
// // .onTrue(amper.retract());

// // new Trigger(() -> shouldEjectNote()).onTrue(sequence(
// // runOnce(() -> leds.setLedColor(Colors.PURPLE)),
// // ejectNote()
// // ));

// new Trigger(()-> amper.getMeasurement() > 3)
// .onTrue(amper.runRollers())
// .onFalse(amper.stopRollers());

// new Trigger(()-> amper.getMeasurement() > 3)
// .and(()-> !shooter.noteInRollers())
// .debounce(2)
// .onTrue(amper.retract());

}

private void initTrigges(){
// new Trigger(()->true).onTrue(queueNote());

//Shooting
new Trigger(()-> shooter.getShooting())
new Trigger(shooterRunning)
.onTrue(sequence(
shooter.runKickMotor(KICK_POWER),
hopper.runManipulator(.8)
Expand All @@ -167,29 +234,28 @@ private void configureButtonBindings() {
));

//Stops shooting when all notes are gone
new Trigger(()-> !shooter.noteInRollers())
.and(()-> !hopper.hasObjectPresent())
new Trigger(hasNoNotes)
.debounce(0.5)
.onTrue(sequence(
shooter.setShooting(false),
runOnce(() -> leds.setLedColor(Colors.BLUE))
));

//Queues note to hopper
new Trigger(()-> intake.getMeasurement() > 90)
.and(()->!hopper.hasObjectPresent())
new Trigger(intakeOut)
.and(not(hopperHasNote))
.onTrue(hopper.runManipulator(HOPPER_INTAKE_POWER))
.onFalse(hopper.runManipulator(0));

//Stops hopper if intake is retracted and is empty
new Trigger(()-> intake.getMeasurement() < 20)
.and(()->hopper.hasObjectPresent()).negate()
new Trigger(not(intakeOut))
.and(not(hopperHasNote))
.onTrue(hopper.runManipulator(0));

//Queues note to shooter
new Trigger(()-> shooter.noteInRollers()).negate()
.and(()->hopper.hasObjectPresent())
.and(() -> !shooter.getShooting())
//Queues note to shooter if not shooter and amper is retracted
new Trigger(readyForAdvance)
.and(not(shooterRunning))
.and(not(amperOut))
.onTrue(sequence(
runOnce(() -> leds.blinkLEDColor(Colors.RED, Colors.GREEN, .25)),
shooter.runKickMotor(KICK_POWER),
Expand All @@ -201,6 +267,24 @@ private void configureButtonBindings() {
shooter.runKickMotor(0)
));

//Wait 0.5s and queue note to shooter is amper it out
new Trigger(readyForAdvance)
.and(amperOut)
.onTrue(
sequence(
runOnce(() -> leds.blinkLEDColor(Colors.RED, Colors.GREEN, .25)),
shooter.runKickMotor(KICK_POWER),
waitSeconds(0.5),
hopper.runManipulator(HOPPER_INTAKE_POWER)
)
).onFalse(
sequence(
shooter.runKickMotor(-.1),
waitSeconds(.1),
shooter.runKickMotor(0)
)
);

// new Trigger(()-> !shooter.noteInRollers())
// .debounce(0.25)
// .onTrue(amper.retract());
Expand All @@ -210,15 +294,18 @@ private void configureButtonBindings() {
// ejectNote()
// ));

new Trigger(()-> amper.getMeasurement() > 3)
// run shooter is amper is out
new Trigger(amperOut)
.onTrue(shooter.runShooter(AMP_RPM));

new Trigger(amperOut)
.onTrue(amper.runRollers())
.onFalse(amper.stopRollers());

new Trigger(()-> amper.getMeasurement() > 3)
.and(()-> !shooter.noteInRollers())
new Trigger(amperOut)
.and(hasNoNotes)
.debounce(2)
.onTrue(amper.retract());

}

private Timer ejecTimer = new Timer();
Expand Down
24 changes: 0 additions & 24 deletions src/main/java/frc/team3128/commands/CmdManager.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,22 +130,6 @@ public static Command intakeAndStop(Intake.Setpoint setpoint) {
// );
// }

// public static Command queueNote() {
// return either(
// none(),
// sequence(
// shooter.runKickMotor(0.5),
// hopper.runManipulator(HOPPER_INTAKE_POWER),
// waitUntil(()-> shooter.noteInRollers()),
// hopper.runManipulator(0),
// shooter.runKickMotor(0)
// ),
// () -> (!hopper.hasObjectPresent() || shooter.noteInRollers()),
// hopper.runManipulator(0).onlyIf(()-> hopper.hasObjectPresent())

// );
// }

public static Command hopperOuttake() {
return sequence(
intake.pivotTo(Setpoint.NEUTRAL),
Expand All @@ -157,14 +141,6 @@ public static Command hopperOuttake() {
);
}

public static Command ampUp(){
return sequence(
amper.partExtend(),
amper.runRollers(),
shooter.runShooter(0.3)
);
}

public static Command ampFinAndDown(){
return sequence(
amper.fullExtend(),
Expand Down

0 comments on commit bf05f00

Please sign in to comment.