Skip to content

Commit

Permalink
actual testable mode
Browse files Browse the repository at this point in the history
  • Loading branch information
@CrolineCrois
CrolineCrois committed Feb 7, 2025
1 parent a8ca983 commit 6a88c39
Show file tree
Hide file tree
Showing 2 changed files with 185 additions and 184 deletions.
305 changes: 153 additions & 152 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,11 @@

import com.choreo.lib.ChoreoTrajectory;
import com.fasterxml.jackson.databind.util.Named;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.util.PathPlannerLogging;
// import com.pathplanner.lib.auto.AutoBuilder;
// import com.pathplanner.lib.auto.NamedCommands;
// import com.pathplanner.lib.commands.PathPlannerAuto;
// import com.pathplanner.lib.path.PathPlannerPath;
// import com.pathplanner.lib.util.PathPlannerLogging;

import edu.wpi.first.cscore.MjpegServer;
import edu.wpi.first.cscore.UsbCamera;
Expand Down Expand Up @@ -101,15 +101,15 @@ public class RobotContainer {
private final BaseDriveController driveController;
private final SwerveSubsystem swerveSubsystem;

// private final IntakePivotSubsystem intakePivotSubsystem;
// private final IntakeRollerSubsystem intakeRollerSubsystem;
private final IntakePivotSubsystem intakePivotSubsystem;
private final IntakeRollerSubsystem intakeRollerSubsystem;

// private final ShooterFlywheelSubsystem shooterFlywheelSubsystem;
// private final ShooterPivotSubsystem shooterPivotSubsystem;
private final ShooterFlywheelSubsystem shooterFlywheelSubsystem;
private final ShooterPivotSubsystem shooterPivotSubsystem;

// private final ClimbSubsystem climbSubsystem;

// private final ElevatorSubsystem elevatorSubsystem;
private final ElevatorSubsystem elevatorSubsystem;

private final FieldManagementSubsystem fmsSubsystem;
private final LightBarSubsystem lightBarSubsystem;
Expand Down Expand Up @@ -185,7 +185,7 @@ public RobotContainer() {
listEntry = autonTable.getEntry("AutonList");
selectedAutonEntry = autonTable.getEntry("Selected");
// System.out.print("Available Autons: " + AutoBuilder.getAllAutoNames().toArray(new String[0]));
listEntry.setStringArray(AutoBuilder.getAllAutoNames().toArray(new String[0]));
// listEntry.setStringArray(AutoBuilder.getAllAutoNames().toArray(new String[0]));
selectedAutonEntry.setString(autonValue);
autonTable.addListener("Auton", EnumSet.of(NetworkTableEvent.Kind.kValueAll), (table, key, event) -> {
this.autonValue = event.valueData.value.getString();
Expand All @@ -199,19 +199,19 @@ public RobotContainer() {
swerveSubsystem = new SwerveSubsystem(fmsSubsystem::isRedAlliance);
swerveSubsystem.setVerbose(false); // SET THIS TO true FOR TUNING VALUES

// intakePivotSubsystem = new IntakePivotSubsystem();
// intakeRollerSubsystem = new IntakeRollerSubsystem(lightBarSubsystem);
intakePivotSubsystem = new IntakePivotSubsystem();
intakeRollerSubsystem = new IntakeRollerSubsystem(lightBarSubsystem);

// shooterPivotSubsystem = new ShooterPivotSubsystem(
// swerveSubsystem::getShootingDistance,
// fmsSubsystem::isRedAlliance
// );
// shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(
// swerveSubsystem::getShootingDistance,
// fmsSubsystem::isRedAlliance
// );
shooterPivotSubsystem = new ShooterPivotSubsystem(
swerveSubsystem::getShootingDistance,
fmsSubsystem::isRedAlliance
);
shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(
swerveSubsystem::getShootingDistance,
fmsSubsystem::isRedAlliance
);

// elevatorSubsystem = new ElevatorSubsystem();
elevatorSubsystem = new ElevatorSubsystem();

// climbSubsystem = new ClimbSubsystem();

Expand Down Expand Up @@ -342,7 +342,7 @@ private void configureBindings() {
xError.setValue(xPID.getPositionError());
yError.setValue(yPID.getPositionError());

}, swerveSubsystem));}
}, swerveSubsystem));

// /* Pressing the button resets the field axes to the current robot axes. */
// driveController.getDriverHeadingResetButton().onTrue(new InstantCommand(() -> {
Expand Down Expand Up @@ -490,8 +490,8 @@ private void configureBindings() {
// elevatorToZero.onTrue(new ElevatorToLimitSwitchCommand(elevatorSubsystem));
/* INTAKE TEST */

// xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3),
// intakePivotSubsystem));
xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3),
intakePivotSubsystem));

// rightBumper.onTrue(new InstantCommand(() ->
// intakePivotSubsystem.setPosition(0), intakePivotSubsystem));
Expand Down Expand Up @@ -562,138 +562,139 @@ private void configureBindings() {
// );


// // roll behaviors
// leftBumper.onTrue(
// new ConditionalCommand(
// new InstantCommand(),
// new ConditionalCommand(
// new InstantCommand(),
// new WaitCommand(.05).andThen(
// new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot
// new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
// .until(intakeRollerSubsystem::getFrontSensorReached)
// ),
// intakeRollerSubsystem::getAmpSensor
// ), // raise the elevator
// () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos
// || elevatorSubsystem.getTargetState() == ElevatorState.TRAP)
// );
// roll behaviors
leftBumper.onTrue(
new ConditionalCommand(
new InstantCommand(),
new ConditionalCommand(
new InstantCommand(),
new WaitCommand(.05).andThen(
new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot
new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
.until(intakeRollerSubsystem::getFrontSensorReached)
),
intakeRollerSubsystem::getAmpSensor
), // raise the elevator
() -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos
|| elevatorSubsystem.getTargetState() == ElevatorState.TRAP)
);



// aButton runs the intake sequence
aButton.onTrue(
Commands.runOnce(() -> intakePivotSubsystem.setPosition(1), intakePivotSubsystem).alongWith(
new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)).andThen(
new ConditionalCommand(
new WaitCommand(.3).andThen(
new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem).andThen(
new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)
)
),

new InstantCommand(
() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem
),
aButton
)
).until(() -> mechController.getLeftTriggerAxis() > .1) // cancel if try to outtake

);

// bButton stops the rollers
bButton.onTrue(Commands.idle(intakeRollerSubsystem).withTimeout(0));

// xButton toggles the intake being stowed
xButton.onTrue(new InstantCommand(() -> {
double outPosition = 1;
double stowPosition = 0;
if (elevatorSubsystem.getExtensionPercent() > .5
&& elevatorSubsystem.getTargetState() == ElevatorState.TRAP) {
outPosition = .45; // push intake out
} else if (elevatorSubsystem.getExtensionPercent() > .5
&& elevatorSubsystem.getTargetState() == ElevatorState.AMP) {
stowPosition = .2;
}

intakePivotSubsystem.setPosition(
intakePivotSubsystem.getEncoderPosition() < (outPosition + stowPosition) / 2
? outPosition
: stowPosition
);

}, intakePivotSubsystem));

// // aButton runs the intake sequence
// aButton.onTrue(
// Commands.runOnce(() -> intakePivotSubsystem.setPosition(1), intakePivotSubsystem).alongWith(
// new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)).andThen(
// new ConditionalCommand(
// new WaitCommand(.3).andThen(
// new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem).andThen(
// new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)
// )
// ),

// new InstantCommand(
// () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem
// ),
// aButton
// )
// ).until(() -> mechController.getLeftTriggerAxis() > .1) // cancel if try to outtake

// );
// yButton runs the flywheels

// // bButton stops the rollers
// bButton.onTrue(Commands.idle(intakeRollerSubsystem).withTimeout(0));

// // xButton toggles the intake being stowed
// xButton.onTrue(new InstantCommand(() -> {
// double outPosition = 1;
// double stowPosition = 0;
// if (elevatorSubsystem.getExtensionPercent() > .5
// && elevatorSubsystem.getTargetState() == ElevatorState.TRAP) {
// outPosition = .45; // push intake out
// } else if (elevatorSubsystem.getExtensionPercent() > .5
// && elevatorSubsystem.getTargetState() == ElevatorState.AMP) {
// stowPosition = .2;
// }

// intakePivotSubsystem.setPosition(
// intakePivotSubsystem.getEncoderPosition() < (outPosition + stowPosition) / 2
// ? outPosition
// : stowPosition
// );

// }, intakePivotSubsystem));

// // yButton runs the flywheels

// yButton.onTrue(
// new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(
// new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem),
// new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition)
// ).unless(intakeRollerSubsystem::getRockwellSensorValue).andThen(
// new IntakePivotSetPositionCommand(intakePivotSubsystem, 0)
// )
// );
yButton.onTrue(
new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(
new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem),
new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition)
).unless(intakeRollerSubsystem::getRockwellSensorValue).andThen(
new IntakePivotSetPositionCommand(intakePivotSubsystem, 0)
)
);


// shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> {
// if (yButton.getAsBoolean()) {
// lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2);
// // shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning
shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> {
if (yButton.getAsBoolean()) {
lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2);
// shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning

// shooterFlywheelSubsystem.setShooterMotorSpeed();
shooterFlywheelSubsystem.setShooterMotorSpeed();

// shooterPivotSubsystem.setAutoAimBoolean(true);
// if (shooterFlywheelSubsystem.atSpeed()) {
// mechController.setRumble(RumbleType.kBothRumble, .4);
// } else {
// mechController.setRumble(RumbleType.kBothRumble, 0);
// if (lightBarSubsystem.getLightBarMechStatus() == LightBarStatus.SHOOTER_SPIN_UP) {
// double top = shooterFlywheelSubsystem.getTopSpeed()
// / shooterFlywheelSubsystem.getTargetTopRPS();
// double bottom = shooterFlywheelSubsystem.getBottomSpeed()
// / shooterFlywheelSubsystem.getTargetBottomRPS();
// double avg = (top + bottom) / 2; // in case they're different, this just shows the average.

// lightBarSubsystem.updateShooterSpeedPercentage(avg);
// }
shooterPivotSubsystem.setAutoAimBoolean(true);
if (shooterFlywheelSubsystem.atSpeed()) {
mechController.setRumble(RumbleType.kBothRumble, .4);
} else {
mechController.setRumble(RumbleType.kBothRumble, 0);
if (lightBarSubsystem.getLightBarMechStatus() == LightBarStatus.SHOOTER_SPIN_UP) {
double top = shooterFlywheelSubsystem.getTopSpeed()
/ shooterFlywheelSubsystem.getTargetTopRPS();
double bottom = shooterFlywheelSubsystem.getBottomSpeed()
/ shooterFlywheelSubsystem.getTargetBottomRPS();
double avg = (top + bottom) / 2; // in case they're different, this just shows the average.

lightBarSubsystem.updateShooterSpeedPercentage(avg);
}

// }
// } else {
// // if(fmsSubsystem.getMatchStatus() != MatchStatus.AUTON){
// // shooterPivotSubsystem.setAutoAimBoolean(false);
// // }
// if (mechController.getPOV() == 90) {

// if (shooterFlywheelSubsystem.atSpeed()) {
// mechController.setRumble(RumbleType.kBothRumble, .4);
// } else {
// mechController.setRumble(RumbleType.kBothRumble, 0);
// }
// } else {
// shooterFlywheelSubsystem.stopShooter();
// mechController.setRumble(RumbleType.kBothRumble, 0);
// }
// }
// if (shooterFlywheelSubsystem.atSpeed()) {
// mechController.setRumble(RumbleType.kBothRumble, .4);
// } else {
// mechController.setRumble(RumbleType.kBothRumble, 0);
// }

// if (noteInBack
// && !intakeRollerSubsystem.getRockwellSensorValue()
// && intakeRollerSubsystem.getIntegrationSpeed() > 0
// ) {
// System.out.println("Dist: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getShootingDistance())
// + " Angle: " + GRTUtil.twoDecimals(Units.radiansToDegrees(shooterPivotSubsystem.getPosition()))
// + " Speed: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getSplineSpeed()));
// }
// noteInBack = intakeRollerSubsystem.getRockwellSensorValue();

// // if we are at speed, rumble the mech controller
}
} else {
// if(fmsSubsystem.getMatchStatus() != MatchStatus.AUTON){
// shooterPivotSubsystem.setAutoAimBoolean(false);
// }
if (mechController.getPOV() == 90) {

if (shooterFlywheelSubsystem.atSpeed()) {
mechController.setRumble(RumbleType.kBothRumble, .4);
} else {
mechController.setRumble(RumbleType.kBothRumble, 0);
}
} else {
shooterFlywheelSubsystem.stopShooter();
mechController.setRumble(RumbleType.kBothRumble, 0);
}
}
if (shooterFlywheelSubsystem.atSpeed()) {
mechController.setRumble(RumbleType.kBothRumble, .4);
} else {
mechController.setRumble(RumbleType.kBothRumble, 0);
}

if (noteInBack
&& !intakeRollerSubsystem.getRockwellSensorValue()
&& intakeRollerSubsystem.getIntegrationSpeed() > 0
) {
System.out.println("Dist: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getShootingDistance())
+ " Angle: " + GRTUtil.twoDecimals(Units.radiansToDegrees(shooterPivotSubsystem.getPosition()))
+ " Speed: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getSplineSpeed()));
}
noteInBack = intakeRollerSubsystem.getRockwellSensorValue();

// if we are at speed, rumble the mech controller

// }, shooterFlywheelSubsystem
// ));
}, shooterFlywheelSubsystem
));

// dPadRight.onTrue(new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(
// new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem),
Expand All @@ -712,12 +713,12 @@ private void configureBindings() {
// // }, intakePivotSubsystem));

// // The triggers intake/outtake the rollers
// intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> {
intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> {

// double power = .7 * (mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis());
double power = .7 * (mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis());

// intakeRollerSubsystem.setRollSpeeds(power, power);
// }, intakeRollerSubsystem));
intakeRollerSubsystem.setRollSpeeds(power, power);
}, intakeRollerSubsystem)); }

// // Offset buttons to correct the shooter if needed
// offsetUpButton.onTrue(new InstantCommand(
Expand Down
Loading

0 comments on commit 6a88c39

Please sign in to comment.