Skip to content

All code after arkanasas #63

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
TotalTaxAmount merged 19 commits into from
Mar 9, 2025
Merged

All code after arkanasas #63

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
286c98e
arm tuning, intake current limits
aaditsangvikar Mar 4, 2025
c7f12dc
Merge pull request #58 from aaditsangvikar/dev
TotalTaxAmount Mar 4, 2025
cf928f4
all: final changes
TotalTaxAmount Mar 5, 2025
7b3d6a3
Merge pull request #59 from TotalTaxAmount/dev
aaditsangvikar Mar 5, 2025
71368c1
changing to CommandXboxController
aaditsangvikar Mar 5, 2025
788f7b5
adding climber motor config, fixing robot container
aaditsangvikar Mar 5, 2025
d64a483
linting
aaditsangvikar Mar 5, 2025
45734cc
linting
aaditsangvikar Mar 5, 2025
27d73af
Revert "linting"
aaditsangvikar Mar 6, 2025
04badd3
Revert "Revert "linting""
aaditsangvikar Mar 6, 2025
b441117
Revert "adding climber motor config, fixing robot container"
aaditsangvikar Mar 6, 2025
c6e919d
Merge pull request #61 from aaditsangvikar/dev
aaditsangvikar Mar 6, 2025
deed47c
Delete src/main/java/org/blackknights/utils/Controller.java
TotalTaxAmount Mar 6, 2025
3800688
Merge branch 'dev' into new-controller-type
TotalTaxAmount Mar 6, 2025
0dd7220
Merge pull request #60 from aaditsangvikar/new-controller-type
TotalTaxAmount Mar 6, 2025
1d00d86
misc: fix ClimberCommand.java
TotalTaxAmount Mar 6, 2025
4e276f6
Merge pull request #62 from fairviewrobotics/Fix
TotalTaxAmount Mar 6, 2025
4fe996a
misc: All code after Arkansas
TotalTaxAmount Mar 9, 2025
6b1e93a
misc: lint
TotalTaxAmount Mar 9, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ repositories {
}


def ROBOT_MAIN_CLASS = "frc.robot.Main"
def ROBOT_MAIN_CLASS = "org.blackknights.Main"

// Define my targets (RoboRIO) and artifacts (deployable files)
// This is added by GradleRIO's backing project DeployUtils.
Expand Down
225 changes: 177 additions & 48 deletions src/main/java/org/blackknights/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,15 @@
/* Black Knights Robotics (C) 2025 */
package org.blackknights;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.*;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import java.util.function.Supplier;
import org.blackknights.commands.*;
import org.blackknights.constants.ScoringConstants;
Expand All @@ -28,8 +32,8 @@ public class RobotContainer {
ButtonBoardSubsystem buttonBoardSubsystem = new ButtonBoardSubsystem(buttonBoard);

// Controllers
Controller primaryController = new Controller(0);
Controller secondaryController = new Controller(1);
CommandXboxController primaryController = new CommandXboxController(0);
CommandXboxController secondaryController = new CommandXboxController(1);

private final NetworkTablesUtils NTTune = NetworkTablesUtils.getTable("debug");

Expand Down Expand Up @@ -61,17 +65,33 @@ public RobotContainer() {
SmartDashboard.putData(cqProfiles);
SmartDashboard.putData("Auto Chooser", superSecretMissileTech);

// Autos
superSecretMissileTech.addOption(
"Left",
"LEFT_3",
new SequentialCommandGroup(
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")),
getAutoIntakeCommand(),
getAutoIntakeCommand(IntakeSides.LEFT),
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")),
getAutoIntakeCommand(),
getAutoIntakeCommand(IntakeSides.LEFT),
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4"))));

superSecretMissileTech.addOption(
"One pose", getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")));
"RIGHT_3",
new SequentialCommandGroup(
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L4")),
getAutoIntakeCommand(IntakeSides.RIGHT),
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("6L4")),
getAutoIntakeCommand(IntakeSides.RIGHT),
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("7L4"))));

superSecretMissileTech.addOption(
"CENTER_LEFT",
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("12L4")));
superSecretMissileTech.addOption(
"CENTER_RIGHT",
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("1L4")));

superSecretMissileTech.addOption("INTAKE_TEST", getAutoIntakeCommand(IntakeSides.RIGHT));
}

/** Configure controller bindings */
Expand All @@ -82,33 +102,104 @@ private void configureBindings() {
swerveSubsystem.setDefaultCommand(
new DriveCommands(
swerveSubsystem,
() -> primaryController.getLeftY() * 2.5,
() -> primaryController.getLeftX() * 2.5,
() -> -primaryController.getRightX() * Math.PI,
() ->
primaryController.getLeftY()
* ConfigManager.getInstance().get("driver_max_speed", 3.5),
() ->
primaryController.getLeftX()
* ConfigManager.getInstance().get("driver_max_speed", 3.5),
() ->
-primaryController.getRightX()
* Math.toRadians(
ConfigManager.getInstance()
.get("driver_max_speed_rot", 360)),
true,
true));

primaryController.rightBumper.whileTrue(
getPlaceCommand(() -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext()));
primaryController
.leftBumper()
.whileTrue(
getPlaceCommand(
() -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext()));

primaryController.leftBumper.whileTrue(
new ParallelCommandGroup(
new ElevatorArmCommand(
elevatorSubsystem,
armSubsystem,
() -> ScoringConstants.ScoringHeights.INTAKE),
new IntakeCommand(
intakeSubsystem, IntakeCommand.IntakeMode.INTAKE, () -> false)));
primaryController
.rightBumper()
.whileTrue(
new ParallelCommandGroup(
new DriveCommands(
swerveSubsystem,
primaryController::getLeftY,
primaryController::getLeftX,
() -> -primaryController.getRightX() * Math.PI,
true,
true),
new ElevatorArmCommand(
elevatorSubsystem,
armSubsystem,
() -> ScoringConstants.ScoringHeights.INTAKE),
new IntakeCommand(
intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)));

elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem));

primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro()));

secondaryController
.rightStick()
.onTrue(new InstantCommand(ScoringConstants::recomputeCoralPositions));

// SECONDARY CONTROLLER

climberSubsystem.setDefaultCommand(
new ClimberCommand(climberSubsystem, secondaryController));

primaryController.dpadDown.whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro()));
secondaryController
.a()
.whileTrue(
new ElevatorArmCommand(
elevatorSubsystem,
armSubsystem,
() -> ScoringConstants.ScoringHeights.L1));

secondaryController
.b()
.whileTrue(
new ElevatorArmCommand(
elevatorSubsystem,
armSubsystem,
() -> ScoringConstants.ScoringHeights.L2));

secondaryController
.x()
.whileTrue(
new ElevatorArmCommand(
elevatorSubsystem,
armSubsystem,
() -> ScoringConstants.ScoringHeights.L3));

secondaryController
.y()
.whileTrue(
new ElevatorArmCommand(
elevatorSubsystem,
armSubsystem,
() -> ScoringConstants.ScoringHeights.L4));

secondaryController
.leftBumper()
.onTrue(new InstantCommand(() -> coralQueue.stepForwards())); //

secondaryController.leftBumper.onTrue(new InstantCommand(() -> coralQueue.stepBackwards()));
secondaryController.rightBumper.onTrue(new InstantCommand(() -> coralQueue.stepForwards()));
secondaryController
.rightBumper()
.onTrue(new InstantCommand(() -> coralQueue.stepBackwards()));

secondaryController
.rightTrigger(0.2)
.whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE));

secondaryController
.leftTrigger(0.2)
.whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE));
}

/** Runs once when the code starts */
Expand All @@ -125,7 +216,10 @@ public void robotPeriodic() {

/** Runs ones when enabled in teleop */
public void teleopInit() {
CoralQueue.getInstance().loadProfile(cqProfiles.getSelected());
buttonBoardSubsystem.bind();

if (cqProfiles.getSelected() != null)
CoralQueue.getInstance().loadProfile(cqProfiles.getSelected());
}

/**
Expand Down Expand Up @@ -171,22 +265,32 @@ private Command getPlaceCommand(
false,
"rough"),
new BaseCommand(elevatorSubsystem, armSubsystem)),
new ParallelCommandGroup(
new SequentialCommandGroup(
new AlignCommand(
new ParallelRaceGroup(
new AlignCommand(
swerveSubsystem,
() -> currentSupplier.get().getPose(),
true,
"fine"),
new IntakeCommand(
intakeSubsystem,
IntakeCommand.IntakeMode.OUTTAKE,
() -> armSubsystem.hasGamePiece())
.withTimeout(2)),
"fine")
.withTimeout(
ConfigManager.getInstance()
.get("align_fine_max_time", 3.0)),
new RunCommand(
() ->
intakeSubsystem.setSpeed(
ConfigManager.getInstance()
.get("intake_slow_voltage", -2.0)),
intakeSubsystem),
new ElevatorArmCommand(
elevatorSubsystem,
armSubsystem,
() -> nextSupplier.get().getHeight())));
() -> currentSupplier.get().getHeight())),
new ParallelRaceGroup(
new ElevatorArmCommand(
elevatorSubsystem,
armSubsystem,
() -> nextSupplier.get().getHeight()),
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)
.withTimeout(1)));
}

/**
Expand All @@ -204,27 +308,52 @@ private Command getLocationPlaceCommand(CoralQueue.CoralPosition position) {
*
* @return The command to goto the feeder
*/
private Command getAutoIntakeCommand() {
return new ParallelDeadlineGroup(
private Command getAutoIntakeCommand(IntakeSides side) {
Pose2d intakePose = getPose2d(side);

Pose2d intakePoseFinal =
intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI)));

return new ParallelRaceGroup(
new SequentialCommandGroup(
new AlignCommand(
swerveSubsystem,
() ->
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get()
== DriverStation.Alliance.Blue
? ScoringConstants.INTAKE_BLUE
: ScoringConstants.INTAKE_RED,
true,
"rough"),
new IntakeCommand(
intakeSubsystem,
IntakeCommand.IntakeMode.INTAKE,
() -> armSubsystem.hasGamePiece())
.withTimeout(2)),
AlignUtils.getXDistBack(
intakePoseFinal,
ConfigManager.getInstance()
.get("autointake_first_back", 1.0)),
false,
"auto"),
new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "fine")),
new ElevatorArmCommand(
elevatorSubsystem,
armSubsystem,
() -> ScoringConstants.ScoringHeights.INTAKE));
() -> ScoringConstants.ScoringHeights.INTAKE),
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE));
}

private static Pose2d getPose2d(IntakeSides side) {
Pose2d intakePose;
if (DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) {
if (side == IntakeSides.LEFT) {
intakePose = ScoringConstants.INTAKE_BLUE_LEFT;
} else {
intakePose = ScoringConstants.INTAKE_BLUE_RIGHT;
}
} else {
if (side == IntakeSides.LEFT) {
intakePose = ScoringConstants.INTAKE_RED_LEFT;
} else {
intakePose = ScoringConstants.INTAKE_RED_RIGHT;
}
}
return intakePose;
}

private enum IntakeSides {
LEFT,
RIGHT
}
}
2 changes: 1 addition & 1 deletion src/main/java/org/blackknights/commands/AlignCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -252,6 +252,6 @@ public boolean isFinished() {

@Override
public void end(boolean interrupted) {
if (stopWhenFinished) swerveSubsystem.drive(0, 0, 0, false, false, false);
if (stopWhenFinished) swerveSubsystem.zeroVoltage();
}
}
3 changes: 2 additions & 1 deletion src/main/java/org/blackknights/commands/BaseCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import org.blackknights.subsystems.ArmSubsystem;
import org.blackknights.subsystems.ElevatorSubsystem;
import org.blackknights.utils.ConfigManager;

/** Default command to keep the elevator and arm at rest */
public class BaseCommand extends Command {
Expand All @@ -30,7 +31,7 @@ public void initialize() {

@Override
public void execute() {
armSubsystem.setPivotAngle(-0.1);
armSubsystem.setPivotAngle(ConfigManager.getInstance().get("arm_base_angle", 0.1));
if (armSubsystem.getPivotAngle() <= -Math.PI / 4 || armSubsystem.getPivotAngle() >= 0.2) {
elevatorSubsystem.holdPosition();
} else {
Expand Down
24 changes: 13 additions & 11 deletions src/main/java/org/blackknights/commands/ClimberCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,39 +1,41 @@
/* Black Knights Robotics (C) 2025 */
package org.blackknights.commands;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import org.blackknights.subsystems.ClimberSubsystem;
import org.blackknights.utils.Controller;

/** Climber command to control the climber */
public class ClimberCommand extends Command {
public ClimberSubsystem climberSubsystem;
public Controller controller;
public CommandXboxController controller;

/**
* Command to controller the climber, right now over pure voltage
*
* @param climberSubsystem The instance of {@link ClimberSubsystem}
* @param controller A {@link Controller} to control the climber
* @param controller A {@link CommandXboxController} to control the climber
*/
public ClimberCommand(ClimberSubsystem climberSubsystem, Controller controller) {
public ClimberCommand(ClimberSubsystem climberSubsystem, CommandXboxController controller) {
this.climberSubsystem = climberSubsystem;
this.controller = controller;
addRequirements(climberSubsystem);
}

@Override
public void execute() {
if (controller.dpadDown.getAsBoolean()) {
climberSubsystem.setClimberSpeed(1);
} else if (controller.dpadUp.getAsBoolean()) {
climberSubsystem.setClimberSpeed(-1);
} else if (controller.dpadLeft.getAsBoolean()) {
if (!MathUtil.isNear(controller.getLeftY(), 0.0, 0.1)) {
climberSubsystem.setClimberSpeed(controller.getLeftY());
} else {
climberSubsystem.setClimberSpeed(0);
}

if (controller.povLeft().getAsBoolean()) {
climberSubsystem.setLockSpeed(0.5);
} else if (controller.dpadRight.getAsBoolean()) {
} else if (controller.povRight().getAsBoolean()) {
climberSubsystem.setLockSpeed(-0.5);
} else {
climberSubsystem.setClimberSpeed(0);
climberSubsystem.setLockSpeed(0);
}
}
Expand Down
Loading