Skip to content
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.

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

Create new command tests and add extra fixes #14

Merged
merged 13 commits into from
Jun 19, 2024
5 changes: 4 additions & 1 deletion .github/workflows/build-and-test.yml
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,11 @@ on : [push, pull_request]
jobs:
# This workflow contains a single job called "build"
build:
strategy:
matrix:
os: [ubuntu-latest, windows-latest]
# The type of runner that the job will run on
runs-on: windows-latest
runs-on: ${{ matrix.os }}

# Steps represent a sequence of tasks that will be executed as part of the job
steps:
Expand Down
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
plugins {
id "java"
id "jacoco"
id "edu.wpi.first.GradleRIO" version "2024.2.1"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
id "com.diffplug.spotless" version "6.23.3"
}

Expand Down
43 changes: 43 additions & 0 deletions src/main/deploy/pathplanner/autos/Intake 2 Notes.auto
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
{
"version": 1.0,
"startingPose": {
"position": {
"x": 2,
"y": 7.0
},
"rotation": 0
},
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Intake First"
}
},
{
"type": "named",
"data": {
"name": "Shoot To Amp"
}
},
{
"type": "path",
"data": {
"pathName": "Intake Second"
}
},
{
"type": "named",
"data": {
"name": "Shoot To Amp"
}
}
]
}
},
"folder": null,
"choreoAuto": false
}
Original file line number Diff line number Diff line change
Expand Up @@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 1.9173805620338513,
"y": 2.02
"x": 2.0,
"y": 7.0
},
"prevControl": null,
"nextControl": {
"x": 2.9515678771512954,
"y": 2.02
"x": 3.0,
"y": 7.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.9248851224975176,
"y": 2.02
"x": 4.0,
"y": 7.0
},
"prevControl": {
"x": 1.928803934697425,
"y": 2.02
"x": 3.0039188121999074,
"y": 7.0
},
"nextControl": null,
"isLocked": false,
Expand All @@ -30,7 +30,25 @@
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"eventMarkers": [
{
"name": "New Event Marker",
"waypointRelativePos": 0.05,
"command": {
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Intake"
}
}
]
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
Expand Down
86 changes: 86 additions & 0 deletions src/main/deploy/pathplanner/paths/Intake Second.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,86 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 4.0,
"y": 7.0
},
"prevControl": null,
"nextControl": {
"x": 2.5,
"y": 7.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.0,
"y": 6.0
},
"prevControl": {
"x": 2.0,
"y": 6.0
},
"nextControl": {
"x": 3.2,
"y": 6.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.0,
"y": 6.0
},
"prevControl": {
"x": 3.75,
"y": 6.0
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [
{
"name": "New Event Marker",
"waypointRelativePos": 0.65,
"command": {
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Intake"
}
}
]
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 480.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 0,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": 0,
"velocity": 0
},
"useDefaultConstraints": true
}
14 changes: 7 additions & 7 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,8 @@
import frc.robot.commands.BasicIntakeCommand;
import frc.robot.commands.DefaultDriveCommand;
import frc.robot.commands.ShootToAmpCommand;
import frc.robot.commands.ShootToSpeakerCommand;
import frc.robot.commands.SmartIntakeCommand;
import frc.robot.commands.SmartShootCommand;
import frc.robot.commands.led_commands.LEDIdleCommand;
import frc.robot.simulationSystems.PhotonSim;
import frc.robot.subsystems.ArmSubsystem;
Expand Down Expand Up @@ -58,7 +58,7 @@ public RobotContainer() {
autoChooser = AutoBuilder.buildAutoChooser();
autoChooser.addOption(
"Shoot To Shooter",
new SmartShootCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem));
new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem));
PhotonCameraSystem.getAprilTagWithID(0); // Load the class before enable.
SmartDashboard.putData("Auto Chooser", autoChooser);
if (RobotBase.isSimulation()) {
Expand Down Expand Up @@ -108,7 +108,7 @@ private void setupNamedCommands() {
NamedCommands.registerCommand("Intake", new BasicIntakeCommand(intakeSubsystem));
NamedCommands.registerCommand(
"Shoot To Speaker",
new SmartShootCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem));
new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem));
NamedCommands.registerCommand(
"Shoot To Amp", new ShootToAmpCommand(shooterSubsystem, intakeSubsystem, armSubsystem));
}
Expand Down Expand Up @@ -142,7 +142,7 @@ private void configureJoystickBindings() {

new JoystickButton(controller, Button.kY.value) // Shoot, smart (Fully Shoot)
.whileTrue(
new SmartShootCommand(
new ShootToSpeakerCommand(
shooterSubsystem,
intakeSubsystem,
armSubsystem,
Expand Down Expand Up @@ -171,7 +171,7 @@ private void configureJoystickBindings() {
.whileTrue(new RunCommand(() -> shooterSubsystem.setShooterSpeed(-1), shooterSubsystem));

new JoystickButton(controller, Button.kLeftBumper.value)
.whileTrue(new SmartShootCommand(shooterSubsystem, intakeSubsystem));
.whileTrue(new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem));

new Trigger(() -> controller.getPOV() == 0)
// Move arm to 0.5, and set it there until the button is released.
Expand All @@ -189,10 +189,10 @@ private void configureMidiBindings() {
.onTrue(armSubsystem.runOnce(armSubsystem::resetEncoder).ignoringDisable(true));

new JoystickButton(midiController, 3)
.whileTrue(new SmartShootCommand(shooterSubsystem, intakeSubsystem));
.whileTrue(new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem));

new JoystickButton(midiController, 4)
.whileTrue(new SmartShootCommand(shooterSubsystem, intakeSubsystem));
.whileTrue(new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem));

new JoystickButton(midiController, 16)
.onTrue(
Expand Down
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/commands/BasicIntakeCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,9 @@ public class BasicIntakeCommand extends ParallelRaceGroup {
public BasicIntakeCommand(IntakeSubsystem intakeSubsystem) {
super(
intakeSubsystem
.run(() -> intakeSubsystem.setIntakeSpeed(IntakeConstants.kIntakeSpeed))
.runEnd(
() -> intakeSubsystem.setIntakeSpeed(IntakeConstants.kIntakeSpeed),
intakeSubsystem::stopMotors)
.until(intakeSubsystem::hasNote),
new LEDBlinkRedCommand(LEDSystem.getInstance()));
}
Expand Down
16 changes: 6 additions & 10 deletions src/main/java/frc/robot/commands/BasicRunShooterCommand.java
Original file line number Diff line number Diff line change
@@ -1,25 +1,21 @@
package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.Constants.ShooterConstants;
import frc.robot.commands.led_commands.LEDLoadingWaitCommand;
import frc.robot.subsystems.ShooterSubsystem;

public class BasicRunShooterCommand extends SequentialCommandGroup {
public class BasicRunShooterCommand extends ParallelRaceGroup {
public BasicRunShooterCommand(ShooterSubsystem shooterSubsystem) {
super(
shooterSubsystem.run(
() -> shooterSubsystem.setShooterSpeed(ShooterConstants.kShooterSpeed)));
shooterSubsystem.runEnd(
() -> shooterSubsystem.setShooterSpeed(ShooterConstants.kShooterSpeed),
shooterSubsystem::stopShooter));
}

public BasicRunShooterCommand(ShooterSubsystem shooterSubsystem, double untilTimeSeconds) {
super(
new ParallelRaceGroup(
new BasicRunShooterCommand(shooterSubsystem), // Run the shooter
new LEDLoadingWaitCommand(untilTimeSeconds)),
shooterSubsystem.runOnce(
() -> shooterSubsystem.setShooterSpeed(0) // Stop the shooter after the wait
));
new BasicRunShooterCommand(shooterSubsystem), // Run the shooter
new LEDLoadingWaitCommand(untilTimeSeconds));
}
}
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/commands/LoadToShooterCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,9 @@
public class LoadToShooterCommand extends ParallelRaceGroup {
public LoadToShooterCommand(IntakeSubsystem intakeSubsystem) {
super(
intakeSubsystem.run(
() -> intakeSubsystem.setIntakeSpeed(IntakeConstants.kIntakeSpeed, 0, true)),
intakeSubsystem.runEnd(
() -> intakeSubsystem.setIntakeSpeed(IntakeConstants.kIntakeSpeed, 0, true),
intakeSubsystem::stopMotors),
new WaitANDConditionCommand(0.5, () -> !intakeSubsystem.hasNote())
.alongWith(new PrintCommand("Loading To Shooter"))
.andThen(new PrintCommand("Loaded to Shooter!")));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,8 @@ public MoveArmToPositionCommand(ArmSubsystem armSubsystem, double position) {
public MoveArmToPositionCommand(ArmSubsystem armSubsystem, DoubleSupplier position) {
super(
armSubsystem
.run(() -> armSubsystem.setArmToPosition(position.getAsDouble()))
.runEnd(
() -> armSubsystem.setArmToPosition(position.getAsDouble()), armSubsystem::stopArm)
.until(() -> armSubsystem.isArmAtPosition(position.getAsDouble())));
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/ShootToAmpCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ public ShootToAmpCommand(
LEDSystem.getBlinkColorCommand(ExtraFunctions.getAllianceColor())),
new ParallelRaceGroup(
new LoadToShooterCommand(intakeSubsystem),
new BasicRunShooterCommand(shooterSubsystem)),
LEDSystem.getBlinkColorCommand(Color.kGreen));
new BasicRunShooterCommand(shooterSubsystem),
LEDSystem.getBlinkColorCommand(Color.kGreen)));
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,10 @@
import frc.robot.subsystems.ShooterSubsystem;
import java.util.function.DoubleSupplier;

public class SmartShootCommand extends ParallelRaceGroup {
public class ShootToSpeakerCommand extends ParallelRaceGroup {
private static final double waitTime = 2.5;

public SmartShootCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) {
public ShootToSpeakerCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) {
super(
new SequentialCommandGroup(
new BasicRunShooterCommand(shooterSubsystem, waitTime),
Expand All @@ -20,18 +20,18 @@ public SmartShootCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem inta
new LoadToShooterCommand(intakeSubsystem))));
}

public SmartShootCommand(
public ShootToSpeakerCommand(
ShooterSubsystem shooterSubsystem,
IntakeSubsystem intakeSubsystem,
ArmSubsystem armSubsystem,
DriveSubsystem driveSubsystem) {
super(
// Constantly move the arm until all the other commands finish.
new MoveArmToShooterCommand(armSubsystem, driveSubsystem),
new SmartShootCommand(shooterSubsystem, intakeSubsystem));
new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem));
}

public SmartShootCommand(
public ShootToSpeakerCommand(
ShooterSubsystem shooterSubsystem,
IntakeSubsystem intakeSubsystem,
ArmSubsystem armSubsystem,
Expand All @@ -43,6 +43,6 @@ public SmartShootCommand(
new MoveArmToShooterCommand(armSubsystem, driveSubsystem),
new DriveFacingShooter(driveSubsystem, xSpeed, ySpeed),
// The finishing command will be this one:
new SmartShootCommand(shooterSubsystem, intakeSubsystem));
new ShootToSpeakerCommand(shooterSubsystem, intakeSubsystem));
}
}
Loading