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.

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

Commit all, explain later #103

Open
wants to merge 26 commits into
base: district-4
Choose a base branch
from
Open

Commit all, explain later #103

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
a6f4e4a
Commit all, explain later
Se-gol Mar 26, 2022
22011a3
Better LEDs, revert if it creates problems
Se-gol Mar 26, 2022
4707714
Add constants for tolerances
Se-gol Mar 26, 2022
5661036
Delete some unused code
Se-gol Mar 26, 2022
ba19905
Quick fix to LEDs
Se-gol Mar 26, 2022
47b0ddb
Reformat code
vichik123 Mar 26, 2022
e159e83
Update zero positions
Se-gol Mar 26, 2022
78e3c9c
Add paths
GaiaZano05 Mar 26, 2022
6ad7776
Add auto path
GaiaZano05 Mar 26, 2022
ad97c3c
Add warmup function
GaiaZano05 Mar 26, 2022
c2acd5a
Change name
GaiaZano05 Mar 26, 2022
060df3c
Change path
GaiaZano05 Mar 26, 2022
d0ee300
add fade and (5,6) ball
AlonSchwierz Mar 26, 2022
3303309
Change path
GaiaZano05 Mar 26, 2022
37c06f2
Change path
GaiaZano05 Mar 26, 2022
3fae4ee
Change path
GaiaZano05 Mar 26, 2022
dd83b4b
Working 4 ball
Se-gol Mar 26, 2022
7ca6fde
Five ball stuff, and just shoot
Se-gol Mar 26, 2022
9ec682f
Merge branch 'autoPaths-newFour' into suppliers-and-caching
Se-gol Mar 26, 2022
58d6c7c
Fix warmup
Se-gol Mar 26, 2022
b37bec8
Merge remote-tracking branch 'origin/back-up-cases' into suppliers-an…
Se-gol Mar 26, 2022
4913360
Add wait for vision adjustment in shooting
vichik123 Mar 26, 2022
ae57656
Latest (five cargo)
Se-gol Mar 27, 2022
450c5a1
Warm up fix and a more reliable option
Mar 27, 2022
cb28fd6
Driver's practice and autonomous
Mar 27, 2022
4483aba
Change to 5
Se-gol Mar 27, 2022
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
8 changes: 5 additions & 3 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,17 +79,19 @@ public static final class SwerveDrive {
public static final WebConstant ADJUST_CONTROLLER_KP = WebConstant.of("Swerve", "kp_heading", 12.5);
public static final WebConstant ADJUST_CONTROLLER_KI = WebConstant.of("Swerve", "KI_heading", 0);
public static final WebConstant ADJUST_CONTROLLER_KD = WebConstant.of("Swerve", "Kd_heading", 0.1);

public static final double ODOMETRY_ADJUST_FINISHED_CONDITION = 2; // [deg]
public static final double VISION_ADJUST_FINISHED_CONDITION = 1.5; // [deg]
private static final double Rx = SwerveDrive.ROBOT_LENGTH / 2; // [m]
private static final double Ry = SwerveDrive.ROBOT_WIDTH / 2; // [m]

// Axis systems
public static final Translation2d[] SWERVE_POSITIONS = new Translation2d[]{
new Translation2d(Rx, -Ry),
new Translation2d(Rx, Ry),
new Translation2d(-Rx, -Ry),
new Translation2d(-Rx, Ry)
};


}

public static class Shooter {
Expand All @@ -105,7 +107,7 @@ public static class Shooter {
public static final WebConstant kI = WebConstant.of("Shooter", "kI", 0.0000075);
public static final WebConstant kD = WebConstant.of("Shooter", "kD", 9);
public static final WebConstant kF = WebConstant.of("Shooter", "kf", 0.0465);
// public static final WebConstant kP = WebConstant.of("Shooter", "kP", 0.03);
// public static final WebConstant kP = WebConstant.of("Shooter", "kP", 0.03);
// public static final WebConstant kI = WebConstant.of("Shooter", "kI", 0.00);
// public static final WebConstant kD = WebConstant.of("Shooter", "kD", 0);
// public static final WebConstant kF = WebConstant.of("Shooter", "kf", 0.0475);
Expand Down
86 changes: 36 additions & 50 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,15 +15,14 @@
import frc.robot.subsystems.conveyor.Conveyor;
import frc.robot.subsystems.conveyor.commands.Convey;
import frc.robot.subsystems.drivetrain.SwerveDrive;
import frc.robot.subsystems.drivetrain.commands.DriveAndAdjustWithVision;
import frc.robot.subsystems.drivetrain.commands.OverpoweredDrive;
import frc.robot.subsystems.drivetrain.commands.TurnToAngle;
import frc.robot.subsystems.flap.Flap;
import frc.robot.subsystems.helicopter.Helicopter;
import frc.robot.subsystems.helicopter.commands.JoystickPowerHelicopter;
import frc.robot.subsystems.hood.Hood;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.shooter.commands.BackAndShootCargo;
import frc.robot.subsystems.shooter.commands.ReachVelocity;
import frc.robot.subsystems.shooter.commands.Shoot;
import frc.robot.utils.LedSubsystem;
Expand All @@ -39,21 +38,10 @@ public class RobotContainer {
public static boolean hardCodedVelocity = false;
public static double hardCodedDistance = 3.8;
public static double hardCodedVelocityValue = Shoot.getSetpointVelocity(hardCodedDistance);
public static boolean shooting = false;
public static DoubleSupplier proximity;
public static boolean shooting = false; // If this is true, don't change the setpoint of the shooter during teleop
public static double setpointVelocity = 0; // Setpoint velocity for the shooter to reach at all times during teleop

// The robot's subsystems and commands are defined here...
public static DoubleSupplier setpointSupplierForShooterFromVision;
public static DoubleSupplier distanceSupplierFromVision;
public static DoubleSupplier odometrySetpointSupplier;
public static DoubleSupplier odometryDistanceSupplier;
public static double cachedSetpointForShooter = 0;
public static double cachedDistanceForHood = 0;
public static double odometryCachedSetpoint = 0;
public static double odometryCachedDistance = 0;
public static boolean cachedHasTarget = true;

public static DoubleSupplier yawSupplierFromVision;

public static boolean warmUpShooting = false;

Expand All @@ -75,22 +63,16 @@ public class RobotContainer {
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
distanceSupplierFromVision = photonVisionModule::getDistance;
setpointSupplierForShooterFromVision = () -> Shoot.getSetpointVelocity(distanceSupplierFromVision.getAsDouble());
odometrySetpointSupplier = () -> Shoot.getSetpointVelocity(swerve.getOdometryDistance());
odometryDistanceSupplier = swerve::getOdometryDistance;
hasTarget = photonVisionModule::hasTargets;

yawSupplierFromVision = () -> photonVisionModule.getYaw().orElse(0);
proximity = conveyor::getColorSensorProximity;

// autonomousCommand = null;
// autonomousCommand = new FiveCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule);
// autonomousCommand = new TaxiFrom(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule);
// Configure the button bindings and default commands
// autonomousCommand = new FourCargoAuto(shooter, swerve, conveyor, intake, hood, flap, photonVisionModule);
autonomousCommand = new FourBallAuto(swerve, shooter, conveyor, intake, hood, flap, photonVisionModule);
configureDefaultCommands();
initSuppliers();
if (Robot.debug) {
startFireLog();
}
Expand All @@ -100,16 +82,22 @@ public RobotContainer() {

}

private void initSuppliers() {
Suppliers.shooterVelocity = shooter::getVelocity;
Suppliers.yawSupplier = () -> photonVisionModule.getYaw().orElse(0);
Suppliers.distanceSupplier = photonVisionModule::getDistance;
Suppliers.odometryDistanceSupplier = swerve::getOdometryDistance;
Suppliers.preFlapBlocked = () -> !conveyor.isPreFlapBeamConnected();
Suppliers.postFlapBlocked = () -> !conveyor.isPostFlapBeamConnected();
}

private void configureDefaultCommands() {
swerve.setDefaultCommand(
new DriveAndAdjustWithVision(
new OverpoweredDrive(
swerve,
() -> -Joysticks.leftJoystick.getY() * speedMultiplier,
() -> -Joysticks.leftJoystick.getX() * speedMultiplier,
() -> -Joysticks.rightJoystick.getX() * thetaMultiplier,
() -> photonVisionModule.getYaw().orElse(0),
Joysticks.rightTrigger::get,
photonVisionModule::hasTargets
() -> -Joysticks.rightJoystick.getX() * thetaMultiplier
)
);
helicopter.setDefaultCommand(new JoystickPowerHelicopter(helicopter, () -> -Xbox.controller.getLeftY()));
Expand All @@ -119,39 +107,21 @@ private void configureDefaultCommands() {
private void configureButtonBindings() {

{ // Xbox controller button bindings.
Xbox.b.whileHeld(new BackAndShootCargo2(
shooter, hood, conveyor, flap,
() -> 0)).whenInactive(() -> shooting = false);
Xbox.lt.whileActiveContinuous(new PickUpCargo(conveyor, flap, intake, 0.7, () -> Utils.map(MathUtil.clamp(Math.hypot(swerve.getChassisSpeeds().vxMetersPerSecond, swerve.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 0.25, 0.1)));

Xbox.lb.whileHeld(new Outtake(intake, conveyor, flap, shooter, hood, () -> false));
Xbox.rb.whileHeld(new Convey(conveyor, -Constants.Conveyor.SHOOT_POWER));

Xbox.x.whenPressed(intake::toggleRetractor);
// Xbox.x.whileHeld(()->shooter.setVelocity(3600));
// Xbox.y.whenPressed(new RunCommand(() -> shooter.setVelocity(3530.0), shooter).withInterrupt(Xbox.rt::get));
Xbox.y.whenPressed(new InstantCommand(() -> warmUpShooting = !warmUpShooting));
Xbox.a.whileHeld(() -> playWithoutVision = true).whenReleased(() -> playWithoutVision = false);

Xbox.leftPov.whileActiveOnce(new InstantCommand(hood::toggle));
Xbox.downPov.whileActiveOnce(new LowGoalShot(shooter, flap, hood));
Xbox.upPov.whileActiveContinuous(new JoystickPowerHelicopter(helicopter, () -> -0.1));

Xbox.rt.whileActiveContinuous(new BackAndShootCargo(
shooter, hood, conveyor, flap,
() -> 0)).whenInactive(() -> shooting = false);
Comment on lines -135 to -137
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you think it's a good idea to have Ma'ayan shoot the cargo? If the adjustment doesn't work it could cause problems.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Well, I also thought about it. Paulo recommended it and I also think it helps with the flow of the game. All Maayan has to do is press and hold once until balls go in. We'll see how well that, and the adjustment work. If the adjustment doesn't work we'll find safeties or split it. And if Maayan prefers it, we'll let you do it

Xbox.lt.whileActiveContinuous(new PickUpCargo(conveyor, flap, intake, 0.7, () -> Utils.map(MathUtil.clamp(Math.hypot(swerve.getChassisSpeeds().vxMetersPerSecond, swerve.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 0.25, 0.1)));
// Xbox.lt.whileActiveContinuous(new SmartIndexing(shooter, conveyor, intake, flap, () -> Utils.map(MathUtil.clamp(Math.hypot(swerve.getChassisSpeeds().vxMetersPerSecond, swerve.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 0.7, 0.4)));
// Xbox.lt.whileActiveContinuous(new ParallelCommandGroup(
// new InstantCommand(flap::blockShooter),
// new InstantCommand(intake::openRetractor),
// new IntakeWithPID(intake, () -> Utils.map(MathUtil.clamp(Math.hypot(swerve.getChassisSpeeds().vxMetersPerSecond, swerve.getChassisSpeeds().vyMetersPerSecond), 0, 4), 0, 4, 20, 10)),
// new Convey(conveyor, Constants.Conveyor.DEFAULT_POWER::get)
// ));
Xbox.lb.whileHeld(new Outtake(intake, conveyor, flap, shooter, hood, () -> false));
Xbox.rb.whileHeld(new Convey(conveyor, -Constants.Conveyor.SHOOT_POWER));

Xbox.back.whenPressed(new OneBallOuttake(intake, conveyor, () -> conveyor.getColorSensorProximity() >= 150));

Xbox.rightJoystickButton
.whileHeld(() -> hardCodedVelocity = true)
.whenReleased(() -> hardCodedVelocity = false);

}

{ // Joystick button bindings.
Expand All @@ -163,6 +133,13 @@ private void configureButtonBindings() {
thetaMultiplier = 1.5 * speedMultiplier;
});

Joysticks.rightTrigger.whileHeld(new OneButtonAdjustAndShoot(swerve, conveyor, flap, hood)).whenReleased(
() -> {
shooting = false;
ledSubsystem.setCurrentLedMode(LedSubsystem.LedMode.STATIC);
}
);

Joysticks.leftTwo.whenPressed((Runnable) Robot::resetAngle);
Joysticks.rightTwo.whileHeld(new TurnToAngle(swerve, Rotation2d::new));
}
Expand Down Expand Up @@ -228,4 +205,13 @@ private static final class Xbox {

public static final JoystickButton rightJoystickButton = new JoystickButton(controller, XboxController.Button.kRightStick.value);
}

public static final class Suppliers {
public static DoubleSupplier shooterVelocity = () -> 0;
public static DoubleSupplier yawSupplier = () -> 0;
public static DoubleSupplier distanceSupplier = () -> 0;
public static DoubleSupplier odometryDistanceSupplier = () -> 0;
public static BooleanSupplier preFlapBlocked = () -> false;
public static BooleanSupplier postFlapBlocked = () -> false;
}
}
32 changes: 17 additions & 15 deletions src/main/java/frc/robot/auto/FiveCargoAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,14 +2,18 @@

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.robot.RobotContainer;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import frc.robot.Constants;
import frc.robot.subsystems.conveyor.Conveyor;
import frc.robot.subsystems.conveyor.commands.Convey;
import frc.robot.subsystems.drivetrain.SwerveDrive;
import frc.robot.subsystems.drivetrain.commands.AdjustToTargetOnCommand;
import frc.robot.subsystems.flap.Flap;
import frc.robot.subsystems.hood.Hood;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.shooter.commands.Shoot;
import frc.robot.utils.PhotonVisionModule;

public class FiveCargoAuto extends SaarIsAutonomous {
Expand All @@ -20,23 +24,21 @@ public class FiveCargoAuto extends SaarIsAutonomous {
*/
public FiveCargoAuto(Shooter shooter, SwerveDrive swerveDrive, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) {
super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "FiveCargoAutoPart1");
addCommands(shoot3(0.8));
new InstantCommand(() -> RobotContainer.shooting = true);

addCommands(reachVelocityByDistance(2.16));
addCommands(new WaitUntilCommand(() -> Math.abs(shooter.getVelocity() - Shoot.getSetpointVelocity(2.16)) < Constants.Shooter.SHOOTER_VELOCITY_DEADBAND.get()));
confirmShooting().withTimeout(0.6);
addCommands(followPathAndPickup("FiveCargoAutoPart1"));

addCommands(turnToAngle(() -> Rotation2d.fromDegrees(40.15)));

addCommands(quickReleaseBackShootAndAdjust(1.3)); // 1.8
new InstantCommand(() -> RobotContainer.shooting = false);


addCommands(new ParallelDeadlineGroup(
turnToAngle(() -> Rotation2d.fromDegrees(40.15)),
new Convey(conveyor, -0.25).withTimeout(0.075)
));
addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3));
addCommands(confirmShooting().withTimeout(2));
addCommands(reachVelocityByDistance(3.89));
addCommands(followPathAndPickup("FiveCargoAutoPart2"));

addCommands(followPathAndPickup("FiveCargoAutoPart3"));

addCommands(new InstantCommand(swerveDrive::terminate));

addCommands(quickReleaseBackShootAndAdjust(5));
addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3));
addCommands(confirmShooting());
}
}
32 changes: 12 additions & 20 deletions src/main/java/frc/robot/auto/FourBallAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,40 +1,32 @@
package frc.robot.auto;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import frc.robot.RobotContainer;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import frc.robot.subsystems.conveyor.Conveyor;
import frc.robot.subsystems.conveyor.commands.ConveyToShooter;
import frc.robot.subsystems.conveyor.commands.Convey;
import frc.robot.subsystems.drivetrain.SwerveDrive;
import frc.robot.subsystems.drivetrain.commands.AdjustToTargetOnCommand;
import frc.robot.subsystems.flap.Flap;
import frc.robot.subsystems.hood.Hood;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.shooter.commands.Shoot;
import frc.robot.utils.PhotonVisionModule;

public class FourBallAuto extends SaarIsAutonomous {
public FourBallAuto(SwerveDrive swerveDrive, Shooter shooter, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) {
super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "Alon 4 ball #1");
addCommands(new InstantCommand(() -> RobotContainer.cachedDistanceForHood = 3.4));
addCommands(new InstantCommand(() -> RobotContainer.cachedSetpointForShooter = Shoot.getSetpointVelocity(RobotContainer.cachedDistanceForHood)));
addCommands(new InstantCommand(() -> shooter.setVelocity(RobotContainer.cachedSetpointForShooter)));
addCommands(reachVelocityByDistance(3.35));
addCommands(followPathAndPickup("Alon 4 ball #1"));
addCommands(turnToAngle(() -> Rotation2d.fromDegrees(79.28)));
new ParallelCommandGroup(
new ConveyToShooter(conveyor, () -> !conveyor.isPreFlapBeamConnected(), shooter::getVelocity),
new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets)
).withTimeout(3);
addCommands(new ParallelDeadlineGroup(
turnToAngle(() -> Rotation2d.fromDegrees(79.28)),
new Convey(conveyor, -0.25).withTimeout(0.075)
));
addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3));
addCommands(confirmShooting().withTimeout(2));
addCommands(reachVelocityByDistance(4.03));
addCommands(followPathAndPickup("Alon 4 ball #2"));
addCommands(followPath("Alon 4 ball #3"));
addCommands(new InstantCommand(() -> RobotContainer.cachedDistanceForHood = 4.5));
addCommands(new InstantCommand(() -> RobotContainer.cachedSetpointForShooter = Shoot.getSetpointVelocity(RobotContainer.cachedDistanceForHood)));
addCommands(new InstantCommand(() -> shooter.setVelocity(RobotContainer.cachedSetpointForShooter)));
new ParallelCommandGroup(
new ConveyToShooter(conveyor, () -> !conveyor.isPreFlapBeamConnected(), shooter::getVelocity),
new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets)
).withTimeout(3);
addCommands(new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3));
addCommands(confirmShooting());
}
}
26 changes: 12 additions & 14 deletions src/main/java/frc/robot/auto/FourCargoAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,12 +1,10 @@
package frc.robot.auto;

import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.RobotContainer;
import edu.wpi.first.wpilibj2.command.*;
import frc.robot.subsystems.conveyor.Conveyor;
import frc.robot.subsystems.conveyor.commands.Convey;
import frc.robot.subsystems.drivetrain.SwerveDrive;
import frc.robot.subsystems.drivetrain.commands.AdjustToTargetOnCommand;
import frc.robot.subsystems.flap.Flap;
import frc.robot.subsystems.hood.Hood;
import frc.robot.subsystems.intake.Intake;
Expand All @@ -19,12 +17,14 @@ public class FourCargoAuto extends SaarIsAutonomous {
// go to terminal, pickup cargo, park near low tarmac, shoot.(9)
public FourCargoAuto(Shooter shooter, SwerveDrive swerveDrive, Conveyor conveyor, Intake intake, Hood hood, Flap flap, PhotonVisionModule visionModule) {
super(swerveDrive, shooter, conveyor, intake, hood, flap, visionModule, "p2 - Taxi from low right tarmac and pickup low cargo(7.1)");
addCommands(new InstantCommand(() -> RobotContainer.warmUpShooting = true));
addCommands(reachVelocityByDistance(3.36));
addCommands(followPathAndPickup("p2 - Taxi from low right tarmac and pickup low cargo(7.1)"));

addCommands(shootAndAdjust(2));
addCommands(new InstantCommand(() -> RobotContainer.shooting = false));

addCommands(new ParallelDeadlineGroup(
new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3),
new Convey(conveyor, -0.25).withTimeout(0.075)
));
addCommands(confirmShooting().withTimeout(2));
addCommands(reachVelocityByDistance(4.05));
addCommands(new ParallelRaceGroup(
followPath("p3 - Going to terminal(9.3)"),
new SequentialCommandGroup(
Expand All @@ -33,14 +33,12 @@ public FourCargoAuto(Shooter shooter, SwerveDrive swerveDrive, Conveyor conveyor
pickup(10)
)
));

addCommands(new ParallelRaceGroup(
followPath("p3 - Going to middle tarmac(9.4.2)"),
pickup(10)
)
);
addCommands(shootAndAdjust(10));
addCommands(new InstantCommand(() -> RobotContainer.shooting = false));

new AdjustToTargetOnCommand(swerveDrive, () -> visionModule.getYaw().orElse(0), visionModule::hasTargets).withTimeout(0.3);
addCommands(confirmShooting());
}
}
Loading