// Uncomment the next line, set the desired RobotId, deploy, and then comment the line out
// RobotId.setRobotId(RobotId.SwerveCompetition);
- RobotController.setBrownoutVoltage(6.0);
+ RobotController.setBrownoutVoltage(4.6); //TODO might break on systemcores https://www.chiefdelphi.com/t/frc-1678-citrus-circuits-systemcore-alpha-testing-thread/506842
// obtain this robot's identity
RobotId robotId = RobotId.getRobotId();
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.AutoBuilderException;
-import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import choreo.auto.AutoChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import frc.robot.commands.DoNothing;
import frc.robot.commands.LogCommand;
import frc.robot.commands.auto_comm.ChoreoPathCommandBuilder;
import frc.robot.commands.auto_comm.DynamicAutoBuilder;
-import frc.robot.commands.drive_comm.DefaultDriveCommand;
import frc.robot.commands.drive_comm.SysIDDriveCommand;
-import frc.robot.commands.gpm.IntakeMovementCommand;
-import frc.robot.commands.gpm.LockedShoot;
-import frc.robot.commands.gpm.RunSpindexer;
-import frc.robot.commands.gpm.Superstructure;
-import frc.robot.commands.vision.ShutdownAllPis;
import frc.robot.constants.AutoConstants;
import frc.robot.constants.Constants;
import frc.robot.constants.VisionConstants;
import frc.robot.controls.BaseDriverConfig;
import frc.robot.controls.Operator;
import frc.robot.controls.PS5ControllerDriverConfig;
-import frc.robot.subsystems.Intake.Intake;
import frc.robot.subsystems.PowerControl.EMABreaker;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.spindexer.Spindexer;
-import frc.robot.subsystems.turret.Turret;
import frc.robot.util.PathGroupLoader;
import frc.robot.util.Vision.DetectedObject;
import frc.robot.util.Vision.Vision;
// The robot's subsystems are defined here...
private Drivetrain drive = null;
private Vision vision = null;
- private Turret turret = null;
- private Shooter shooter = null;
- private Hood hood = null;
- private Spindexer spindexer = null;
- private Intake intake = null;
// private LED led = null;
// Controllers are defined here
case PrimeJr: // AKA Valence
- spindexer = new Spindexer();
- intake = new Intake();
- // led = new LED();
- breaker = new EMABreaker();
case WaffleHouse: // AKA Betabot
- turret = new Turret();
- shooter = new Shooter();
- hood = new Hood();
case SwerveCompetition: // AKA "Vantage"
case Vertigo: // AKA "French Toast"
drive = new Drivetrain(vision, new GyroIOPigeon2());
- driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer);
+ driver = new PS5ControllerDriverConfig(drive);
operator = new Operator(drive);
initChoreo();
initializeAutoBuilder();
autoChooserInit();
- if (turret != null && drive != null && hood != null && shooter != null) {
- SmartDashboard.putData("Lock Shooting", new LockedShoot(turret, drive, hood, shooter));
- }
-
- if (turret != null) {
- turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
- }
if (drive != null && driver != null) {
// drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
}
- if (intake != null && hood != null && turret != null)
// CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret)); (no more crt for this)
// This is really annoying so it's disabled
DriverStation.silenceJoystickConnectionWarning(true);
// LiveWindow is causing periodic loop overruns
LiveWindow.disableAllTelemetry();
LiveWindow.setEnabled(false);
-
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis());
- }
}
private void initChoreo() {
Logger.recordOutput("Autos/StartingOrFinishing", startOrFinish);
});
- autoFactory.bind("hoodUp", new InstantCommand(() -> hood.forceHoodDown(false)));
- autoFactory.bind("hoodDown", new InstantCommand(() -> hood.forceHoodDown(true)));
-
// warmup command for choreo, prevents lag on auto startup
CommandScheduler.getInstance().schedule(autoFactory.warmupCmd().ignoringDisable(true));
}
drive);
}
- private boolean seizing;
public void registerCommands() {
- if (intake != null) {
-
- NamedCommands.registerCommand("Extend Intake", new InstantCommand(() -> {
- intake.extend();
- }));
- NamedCommands.registerCommand("Retract Intake", new InstantCommand(() -> intake.retract()));
- NamedCommands.registerCommand("Intermediate Extend", new InstantCommand(() -> intake.intermediateExtend()));
- NamedCommands.registerCommand("Spin Intake Rollers", new InstantCommand(() -> intake.spinStart()));
- NamedCommands.registerCommand("Stop Intake Rollers", new InstantCommand(() -> intake.spinStop()));
-
- NamedCommands.registerCommand("Start Intake Seizure", new InstantCommand(() -> {
- seizing = true;
- CommandScheduler.getInstance().schedule(new IntakeMovementCommand(intake).until(() -> !seizing));
- }));
- NamedCommands.registerCommand("Stop Intake Seizure", new InstantCommand(() -> {
- seizing = false;
- }));
- }
-
- if (turret != null && drive != null && hood != null && shooter != null && spindexer != null && intake != null) {
- Command runSpindexer = new RunSpindexer(spindexer, turret, hood, intake);
- NamedCommands.registerCommand("Start Spindexer",
- new InstantCommand(() -> CommandScheduler.getInstance().schedule(runSpindexer)));
- NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(() -> runSpindexer.cancel()));
- }
-
- if (hood != null) {
-
- NamedCommands.registerCommand("Hood Down", new InstantCommand(() -> {
- hood.forceHoodDown(true);
- }));
- NamedCommands.registerCommand("Stop Hood Down", new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }));
- }
-
- NamedCommands.registerCommand("After Depot", new InstantCommand());
- NamedCommands.registerCommand("Constraints Zone", new InstantCommand());
- NamedCommands.registerCommand("Depot", new InstantCommand());
- NamedCommands.registerCommand("Reset Spindexer", new InstantCommand());
}
public void addAuto(String name) {
*/
public void autoChooserInit() {
// add the options to the Chooser
- String leftSideAuto = "Left Week V1";
- String rightSideAuto = "Right Week V1";
- String shootOnlyAuto = "Shoot Only Left Week V1";
- String leftLiberalSwipe = "LeftLiberalDoubleSwipe";
- String rightLiberalSwipe = "RightLiberalDoubleSwipe";
- String leftLiberalSwipeTranslation = "LeftLiberalDoubleSwipeTranslation";
- String leftConservativeSwipe = "LeftConservativeDoubleSwipe";
- String leftDoNothing = "Left Do Nothing";
- String rightDoNothing = "Right Do Nothing";
- String centerDoNothing = "Center Do Nothing";
- String leftShallowDoubleSwipe = "LeftShallowDoubleSwipe";
- String rightShallowDoubleSwipe = "RightShallowDoubleSwipe";
- String leftBumpDepotCenter = "LeftBumpDepotCenter";
- String leftTrenchDepotCenter = "LeftTrenchDepotCenter";
- String depotCenterPath = "DepotCenterPath";
autoChooser.setDefaultOption("Default", getDefaultAuto());
- addAuto(leftSideAuto);
- addAuto(rightSideAuto);
- addAuto(shootOnlyAuto);
- addAuto(leftConservativeSwipe);
- addAuto(leftLiberalSwipe);
- addAuto(rightLiberalSwipe);
- addAuto(leftLiberalSwipeTranslation);
- addAuto(leftDoNothing);
- addAuto(rightDoNothing);
- addAuto(centerDoNothing);
- addAuto(leftShallowDoubleSwipe);
- addAuto(rightShallowDoubleSwipe);
- addAuto(leftBumpDepotCenter);
- addAuto(leftTrenchDepotCenter);
- addAuto(depotCenterPath);
-
-
- DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake);
+
+ DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder();
// names
String leftDynamicLiberalDoubleSwipe = "LeftDynamicDoubleLiberalSwipe";
addAuto(leftDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(true));
addAuto(rightDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(false));
- ChoreoPathCommandBuilder choreo = new ChoreoPathCommandBuilder(intake, spindexer, turret, hood);
+ ChoreoPathCommandBuilder choreo = new ChoreoPathCommandBuilder();
- addAuto("testChoreo", ChoreoPathCommandBuilder.basicTrajectoryAuto("test.traj", true, autoFactory));
- addChoreoAuto("choreoLiberalLeft", choreo.leftLiberal(autoFactory));
- addChoreoAuto("choreoLiberalRight", choreo.rightLiberal(autoFactory));
- addChoreoAuto("choreoConservativeLeft", choreo.leftConservative(autoFactory));
- addChoreoAuto("choreoConservativeRight", choreo.rightConservative(autoFactory));
- addChoreoAuto("choreoShallowLeft", choreo.leftShallow(autoFactory));
- addChoreoAuto("choreoShallowRight", choreo.rightShallow(autoFactory));
+ // addAuto("testChoreo", ChoreoPathCommandBuilder.basicTrajectoryAuto("test.traj", true, autoFactory));
// put the Chooser on the SmartDashboard
SmartDashboard.putData("Auto chooser", autoChooser);
}
public Command getDefaultAuto() {
- if (spindexer == null || turret == null || hood == null || intake == null) {
return new DoNothing();
- }
- ParallelCommandGroup defaultShoot = new ParallelCommandGroup(
- new RunSpindexer(spindexer, turret, hood, intake)
- );
- return defaultShoot;
}
public Command getAutoCommand() {
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.Constants;
import frc.robot.util.Elastic;
-import frc.robot.util.HubActive;
import frc.robot.util.Elastic.Notification;
import frc.robot.util.Elastic.NotificationLevel;
+/// Command for logging stuff
public class LogCommand extends Command {
private boolean hubActive = false;
return;
}
- boolean current = HubActive.isHubActive();
- Logger.recordOutput("HubActive", current);
-
- if (current && !hubActive) {
- Elastic.sendNotification(new Notification(NotificationLevel.INFO, "HUB ACTIVE", ""));
- } else if (!current && hubActive) {
- Elastic.sendNotification(new Notification(NotificationLevel.INFO, "HUB DEACTIVATED", ""));
- }
-
- hubActive = current;
}
@Override
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.commands.DoNothing;
-import frc.robot.commands.gpm.IntakeMovementCommand;
-import frc.robot.commands.gpm.RunSpindexer;
-import frc.robot.commands.gpm.RunSpindexerWithStop;
-import frc.robot.subsystems.Intake.Intake;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.spindexer.Spindexer;
-import frc.robot.subsystems.turret.Turret;
public class ChoreoPathCommandBuilder {
- private Intake intake;
- private Spindexer spindexer;
- private Turret turret;
- private Hood hood;
- public ChoreoPathCommandBuilder(Intake intake, Spindexer spindexer, Turret turret, Hood hood) {
- this.intake = intake;
- this.spindexer = spindexer;
- this.turret = turret;
+ public ChoreoPathCommandBuilder() {
}
resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(),
command);
}
-
- public AutoRoutine leftLiberal(AutoFactory factory) {
- AutoRoutine routine = factory.newRoutine("leftLiberal");
-
- AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0);
- AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1);
-
- routine.active().onTrue(
- Commands.sequence(
- liberalSwipe.resetOdometry(),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- liberalSwipe.cmd()));
-
- liberalSwipe.done()
- .onTrue(Commands.sequence(
- new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }),
- new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shallowSwipe.cmd()));
-
- shallowSwipe.done()
- .onTrue(Commands.sequence(
- new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }),
- new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shallowSwipe.cmd()));
-
- return routine;
-
- }
-
- public AutoRoutine rightLiberal(AutoFactory factory) {
- AutoRoutine routine = factory.newRoutine("rightLiberal");
-
- AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0).mirrorY();
- AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1).mirrorY();
-
- routine.active().onTrue(
- Commands.sequence(
- liberalSwipe.resetOdometry(),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- }),
- liberalSwipe.cmd()));
-
- liberalSwipe.done()
- .onTrue(Commands.sequence(
- new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
- shallowSwipe.cmd()));
-
- shallowSwipe.done()
- .onTrue(Commands.sequence(
- new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
- shallowSwipe.cmd()));
-
- return routine;
-
- }
-
- public AutoRoutine leftConservative(AutoFactory factory) {
- AutoRoutine routine = factory.newRoutine("leftConservative");
-
- AutoTrajectory liberalSwipe = routine.trajectory("conservative", 0);
- AutoTrajectory shallowSwipe = routine.trajectory("conservative", 1);
-
- routine.active().onTrue(
- Commands.sequence(
- liberalSwipe.resetOdometry(),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- liberalSwipe.cmd()));
-
- liberalSwipe.done()
- .onTrue(Commands.sequence(
- new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }),
- new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shallowSwipe.cmd()));
-
- shallowSwipe.done()
- .onTrue(Commands.sequence(
- new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }),
- new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shallowSwipe.cmd()));
-
- return routine;
-
- }
-
- public AutoRoutine rightConservative(AutoFactory factory) {
- AutoRoutine routine = factory.newRoutine("rightConservative");
-
- AutoTrajectory liberalSwipe = routine.trajectory("conservative", 0).mirrorY();
- AutoTrajectory shallowSwipe = routine.trajectory("conservative", 1).mirrorY();
-
- routine.active().onTrue(
- Commands.sequence(
- liberalSwipe.resetOdometry(),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- liberalSwipe.cmd()));
-
- liberalSwipe.done()
- .onTrue(Commands.sequence(
- new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }),
- new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shallowSwipe.cmd()));
-
- shallowSwipe.done()
- .onTrue(Commands.sequence(
- new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }),
- new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shallowSwipe.cmd()));
-
- return routine;
-
- }
-
- public AutoRoutine leftShallow(AutoFactory factory) {
- AutoRoutine routine = factory.newRoutine("leftShallow");
-
- AutoTrajectory shallowSwipe = routine.trajectory("shallowSwipe");
-
- routine.active().onTrue(
- Commands.sequence(
- shallowSwipe.resetOdometry(),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shallowSwipe.cmd()));
-
- shallowSwipe.done()
- .onTrue(Commands.sequence(
- new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }),
- new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shallowSwipe.cmd()));
-
- return routine;
-
- }
-
- public AutoRoutine rightShallow(AutoFactory factory) {
- AutoRoutine routine = factory.newRoutine("rightShallow");
-
- AutoTrajectory shallowSwipe = routine.trajectory("shallowSwipe").mirrorY();
-
- routine.active().onTrue(
- Commands.sequence(
- shallowSwipe.resetOdometry(),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shallowSwipe.cmd()));
-
- shallowSwipe.done()
- .onTrue(Commands.sequence(
- new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }),
- new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shallowSwipe.cmd()));
-
- return routine;
-
- }
-
- public AutoRoutine leftSuperShuttling(AutoFactory factory) {
- AutoRoutine routine = factory.newRoutine("leftSuperShuttling");
-
- AutoTrajectory shuttlingTrajectory = routine.trajectory("superShuttling");
-
- routine.active().onTrue(Commands.sequence(
- shuttlingTrajectory.resetOdometry(),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- }),
- shuttlingTrajectory.cmd()));
-
- routine.active().whileTrue(new RunSpindexer(spindexer, turret, hood, intake));
-
- shuttlingTrajectory.done().onTrue(Commands.sequence(
- new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }),
- new WaitCommand(2.0).raceWith(new IntakeMovementCommand(intake)),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shuttlingTrajectory.cmd()));
-
- return routine;
-
- }
-
- public AutoRoutine rightSuperShuttling(AutoFactory factory) {
- AutoRoutine routine = factory.newRoutine("rightSuperShuttling");
-
- AutoTrajectory shuttlingTrajectory = routine.trajectory("superShuttling");
-
- routine.active().onTrue(Commands.sequence(
- shuttlingTrajectory.resetOdometry(),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- }),
- shuttlingTrajectory.cmd()));
-
- routine.active().whileTrue(new RunSpindexer(spindexer, turret, hood, intake));
-
- shuttlingTrajectory.done().onTrue(Commands.sequence(
- new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }),
- new WaitCommand(2.0).raceWith(new IntakeMovementCommand(intake)),
- new InstantCommand(() -> {
- intake.extend();
- intake.spinStart();
- hood.forceHoodDown(true);
- }),
- shuttlingTrajectory.cmd()));
-
- return routine;
-
- }
+
}
package frc.robot.commands.auto_comm;
import edu.wpi.first.wpilibj2.command.*;
-import frc.robot.commands.gpm.RunSpindexerWithStop;
-import frc.robot.subsystems.Intake.Intake;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.spindexer.Spindexer;
-import frc.robot.subsystems.turret.Turret;
+import frc.robot.commands.DoNothing;
import com.pathplanner.lib.commands.PathPlannerAuto;
public class DynamicAutoBuilder {
- private final Spindexer spindexer;
- private final Turret turret;
- private final Hood hood;
- private final Intake intake;
- public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake) {
- this.spindexer = spindexer;
- this.turret = turret;
- this.hood = hood;
- this.intake = intake;
+ public DynamicAutoBuilder() {
}
/*
private Command departCommand() {
return new InstantCommand(() -> {
- hood.forceHoodDown(true);
- intake.extend();
- intake.spinStart();
- spindexer.stopSpindexer();
});
}
private Command runSpindexerWithAbort() {
// has an isFinnished so works
- return new RunSpindexerWithStop(spindexer, turret, hood, intake);
+ // return new RunSpindexerWithStop(spindexer, turret, hood, intake);
+ return new DoNothing();
}
private Command startShootingCommand() {
- return new InstantCommand(() -> hood.forceHoodDown(false));
+ // return new InstantCommand(() -> hood.forceHoodDown(false));
+ return new DoNothing();
}
}
import frc.robot.constants.swerve.DriveConstants;
import frc.robot.controls.BaseDriverConfig;
import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.util.TrenchAssist.TrenchAssist;
-import frc.robot.util.TrenchAssist.TrenchAssistConstants;
import frc.robot.util.Vision.DriverAssist;
/**
ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation);
ChassisSpeeds corrected = DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true);
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("TrenchAlign", swerve.getTrenchAlign());
- Logger.recordOutput("AlignZones", TrenchAssistConstants.ALIGN_ZONES);
- }
- // if (swerve.getTrenchAlign()) {
- // boolean inZone = false;
- // for (Rectangle2d rectangle : TrenchAssistConstants.ALIGN_ZONES) {
- // if (rectangle.contains(swerve.getPose().getTranslation())) {
- // inZone = true;
- // }
- // }
-
- // if (inZone) {
-
- // double yawDegrees = swerve.getYaw().getDegrees();
- // // double snappedDeg = Math.round(yawDegrees / 90.0) * 90.0;
- // if (Math.abs(yawDegrees) <= 90) {
- // swerve.setAlignAngle(Units.degreesToRadians(0.0));
- // } else {
- // swerve.setAlignAngle(Units.degreesToRadians(180.0));
- // }
- // // swerve.setAlignAngle(snappedDeg);
- // // Logger.recordOutput("snappy", snappedDeg);
- // } else {
- // swerve.setIsAlign(false);
- // }
- // } else {
- // swerve.setIsAlign(false);
- // }
- // if (!Constants.DISABLE_LOGGING) {
- // Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist());
- // }
-
- if (swerve.getTrenchAssist()) {
- drive(TrenchAssist.calculate(swerve, corrected, trenchAssistPid));
- } else {
- trenchAssistPid.reset();
- drive(corrected);
- }
}
/**
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.Constants;
-import frc.robot.subsystems.Intake.Intake;
-import frc.robot.subsystems.Intake.IntakeConstants;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.hood.HoodConstants;
-
-public class HardstopWarning extends Command {
- private Hood hood;
- private Intake intake;
-
- public HardstopWarning(Hood hood, Intake intake) {
- this.hood = hood;
- this.intake = intake;
- }
-
- @Override
- public boolean runsWhenDisabled() {
- return true;
- }
-
- @Override
- public void execute() {
- double epsilon = 0.05;
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putBoolean("Hood OK", hood.getPositionDeg() >= HoodConstants.MIN_ANGLE - epsilon);
- SmartDashboard.putBoolean("Intake OK", intake.getPosition() >= IntakeConstants.STARTING_POINT - epsilon);
- }
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Intake.Intake;
-
-public class IntakeCommand extends Command{
- private Intake intake;
-
- public IntakeCommand(Intake intake){
- this.intake = intake;
- addRequirements(intake);
- }
-
- @Override
- public void initialize() {
- intake.extend();
- intake.spinStart();
- }
-
- @Override
- public void end(boolean interrupted) {
- intake.intermediateExtend();
- intake.spinStop();
- }
-
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Intake.Intake;
-
-public class IntakeMovementCommand extends Command {
- private final Intake intake;
- private final double interval = 0.6; // Change this to make it faster/slower (seconds)
-
- public IntakeMovementCommand(Intake intake) {
- this.intake = intake;
- addRequirements(intake);
- }
-
- @Override
- public void execute() {
- intake.spinStart();
- if ((int) (Timer.getFPGATimestamp() / interval) % 2 == 0) {
- intake.extend();
- } else {
- intake.intermediateExtend();
- }
- }
-
- @Override
- public void end(boolean interrupted) {
- intake.extend();
- }
-}
\ No newline at end of file
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.math.filter.LinearFilter;
-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.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.Constants;
-import frc.robot.constants.FieldConstants;
-import frc.robot.constants.ShotInterpolation;
-import frc.robot.constants.ShuttleInterpolation;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.hood.HoodConstants;
-import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.turret.Turret;
-import frc.robot.subsystems.turret.TurretConstants;
-import frc.robot.util.PhaseManager;
-import frc.robot.util.ShooterPhysics;
-import frc.robot.util.ShooterPhysics.TurretState;
-
-public class LockedShoot extends Command {
- private Turret turret;
- private Drivetrain drive;
- private Hood hood;
- private Shooter shooter;
-
- private Pose2d drivepose;
-
- private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
-
- private Rotation2d targetAngle;
-
- private double lastHoodAngle;
- private double hoodAngle;
- private double hoodVelocity;
-
- private TurretState goalState;
-
- private double phaseDelay = 0.03; // Extrapolation delay due to latency
-
- private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d();
-
- private PhaseManager phaseManager = new PhaseManager();
-
- private double distanceFromTarget = 0.0;
-
- private double TOFAdjustment = 0.85;
-
- public LockedShoot(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter) {
- this.turret = turret;
- this.drive = drivetrain;
- this.hood = hood;
- this.shooter = shooter;
- drivepose = drivetrain.getPose();
-
- goalState = ShooterPhysics.getShotParams(
- Translation2d.kZero,
- FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
- 8.0); // Random initial goalState to prevent it being null
-
- addRequirements(turret, shooter);
- }
-
- public void updateSetpoints(Pose2d drivepose) {
- Pose2d turretPosition = drivepose.transformBy(
- new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero));
-
- // If the robot is moving, adjust the target position based on velocity
- ChassisSpeeds robotRelVel = drive.getChassisSpeeds();
- ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drive.getYaw());
-
- // Rotational adjustment is not being used, since turret is in center of robot
- double turretVelocityX =
- fieldRelVel.vxMetersPerSecond;
- double turretVelocityY =
- fieldRelVel.vyMetersPerSecond;
-
- double timeOfFlight = 0;
- Pose2d lookaheadPose = turretPosition;
-
- /*
- * Loop (max 20) until lookaheadPose converges BECAUSE -->
- * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s)
- * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s
- * So we make a bunch of guesses until it converges
- * Early exit when change < 1mm to avoid unnecessary iterations
- */
- for (int i = 0; i < 20; i++) {
- Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
-
- Translation3d target3d = new Translation3d(target.getX(), target.getY(),
- target.equals(FieldConstants.getHubTranslation().toTranslation2d()) ?
- FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub
-
- goalState = ShooterPhysics.getShotParams(
- Translation2d.kZero,
- target3d.minus(lookahead3d),
- 2.0);
-
- timeOfFlight = goalState.timeOfFlight() * TOFAdjustment;
- double offsetX = turretVelocityX * timeOfFlight;
- double offsetY = turretVelocityY * timeOfFlight;
- Pose2d newLookaheadPose =
- new Pose2d(
- turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
- turretPosition.getRotation());
-
- // early exit if converged (change < 1mm)
- if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) {
- lookaheadPose = newLookaheadPose;
- break;
- }
- lookaheadPose = newLookaheadPose;
- }
-
- // Get the field angle relative to the target pose
- targetAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
-
- targetAngle = target.minus(lookaheadPose.getTranslation()).getAngle().minus(new Rotation2d(turret.getPositionRad()));
-
-
- // Pitch is in radians
- hoodAngle = goalState.pitch();
- hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
- lastHoodAngle = hoodAngle;
-
- distanceFromTarget = target.getDistance(lookaheadPose.getTranslation());
- }
-
- public void updateDrivePose(){
- Pose2d currentPose = drive.getPose();
-
- drivepose = new Pose2d(
- currentPose.getTranslation(),
- // Uncomment this if robot is backwards
- currentPose.getRotation()//.plus(new Rotation2d(Math.PI))
- );
- ChassisSpeeds robotRelVel = drive.getChassisSpeeds();
-
- // Add a phase delay extrapolation component for latency delay
- drivepose = drivepose.exp(
- new Twist2d(
- robotRelVel.vxMetersPerSecond * phaseDelay,
- robotRelVel.vyMetersPerSecond * phaseDelay,
- robotRelVel.omegaRadiansPerSecond * phaseDelay));
- }
-
- /**
- * Stops and stows all subsystems involved in the command
- */
- public void stowEverything(){
- hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
- shooter.setShooter(0.0);
- //spindexer.stopSpindexer();
- }
-
- @Override
- public void execute() {
- updateDrivePose();
-
- // Phase manager stuff
- phaseManager.update(drivepose, shooter, turret);
- target = phaseManager.getTarget(drivepose);
-
- updateSetpoints(drivepose);
-
- if (phaseManager.isIdle()) {
- stowEverything();
- } else {
- drive.setAlignAngle(targetAngle.getRadians());
-
- boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at the hub, we're not shuttling
-
- // shuttling will move the hood so its angles very close to max (less arch)
- if (shuttling) {
- hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
- } else {
- hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
- }
-
- // if (FieldConstants.underTrench(x, y)) {
- // System.out.println("Hood forced down");
- // } else {
- // hood.forceHoodDown(false);
- // }
-
- // different maps for shuttling vs shooting. Less powerful when shuttling.
- if (shuttling) {
- shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget));
- } else {
- shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
- }
-
- }
-
-
- }
-
- @Override
- public void end(boolean interrupted) {
- stowEverything();
-
- drive.setIsAlign(false);
-
- turret.locked = false;
-
- }
-
- @Override
- public void initialize() {
- drive.setIsAlign(true);
- turret.locked = true;
- }
-
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.Constants;
-import frc.robot.constants.FieldConstants;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.hood.HoodConstants;
-import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.shooter.ShooterConstants;
-import frc.robot.subsystems.turret.Turret;
-import frc.robot.util.ShooterPhysics;
-
-public class PhysicsAutoShoot extends Command {
- private Turret turret;
- private Hood hood;
- private Shooter shooter;
- private Drivetrain drivetrain;
- private ShooterPhysics.Constraints constraints;
-
- public PhysicsAutoShoot(Turret turret, Hood hood, Shooter shooter, Drivetrain drivetrain) {
- this.turret = turret;
- this.hood = hood;
- this.shooter = shooter;
- this.drivetrain = drivetrain;
-
- this.constraints = new ShooterPhysics.Constraints(2.2, ShooterConstants.SHOOTER_VELOCITY,
- Units.degreesToRadians(HoodConstants.MIN_ANGLE), Units.degreesToRadians(HoodConstants.MAX_ANGLE));
-
- addRequirements(turret, hood, shooter);
- }
-
- @Override
- public void execute() {
- ChassisSpeeds chassisSpeeds = drivetrain.getChassisSpeeds();
- Translation2d robotVelocity = new Translation2d(chassisSpeeds.vxMetersPerSecond,
- chassisSpeeds.vyMetersPerSecond);
- Translation3d robotToTarget = FieldConstants.getHubTranslation()
- .minus(new Translation3d(drivetrain.getPose().getTranslation()));
-
- var stateOpt = ShooterPhysics.getConstrainedParams(
- robotVelocity,
- robotToTarget,
- this.constraints);
-
- // in one periodic
- var futureStateOpt = ShooterPhysics.getConstrainedParams(
- robotVelocity,
- robotToTarget.plus(new Translation3d(robotVelocity.times(Constants.LOOP_TIME))),
- this.constraints);
-
- if (stateOpt.isPresent() && futureStateOpt.isPresent()) {
- ShooterPhysics.TurretState state = stateOpt.get();
- ShooterPhysics.TurretState futureState = futureStateOpt.get();
-
- double yawSlope = (futureState.yaw().getRadians() - state.yaw().getRadians()) / Constants.LOOP_TIME;
- double hoodSlope = (futureState.pitch() - state.pitch()) / Constants.LOOP_TIME;
-
- turret.setFieldRelativeTarget(state.yaw(), yawSlope);
- hood.setFieldRelativeTarget(new Rotation2d(state.pitch()), hoodSlope);
- shooter.setShooter(state.exitVel());
-
- } else if (stateOpt.isPresent() && futureStateOpt.isEmpty()) {
- ShooterPhysics.TurretState state = stateOpt.get();
-
- turret.setFieldRelativeTarget(state.yaw(), 0.0);
- hood.setFieldRelativeTarget(new Rotation2d(state.pitch()), 0.0);
- shooter.setShooter(state.exitVel());
- }
- }
-
- @Override
- public void end(boolean interrupted) {
- // stop the turret where it is
- turret.setFieldRelativeTarget(new Rotation2d(turret.getPositionRad()), 0.0);
- hood.setFieldRelativeTarget(new Rotation2d(0), 0.0);
- shooter.setShooter(0);
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Intake.Intake;
-
-public class ReverseMotors extends Command {
- private Intake intake;
-
-
- public ReverseMotors(Intake intake){
- this.intake = intake;
-
- addRequirements(intake);
- }
-
- @Override
- public void initialize(){
-
- }
-
- @Override
- public void execute(){
- intake.extend();
- intake.spinReverse();
- //spindexer.reverseSpindexer();
- }
-
- @Override
- public void end(boolean interrupted){
- intake.extend();
- intake.spinStart();
- //spindexer.maxSpindexer();
- }
-
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.Constants;
-import frc.robot.subsystems.Intake.Intake;
-import frc.robot.subsystems.Intake.IntakeConstants;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.spindexer.Spindexer;
-import frc.robot.subsystems.spindexer.SpindexerConstants;
-import frc.robot.subsystems.turret.Turret;
-
-public class RunSpindexer extends Command {
- private Spindexer spindexer;
- private Turret turret;
- private Hood hood;
- private Intake intake;
-
- private Debouncer jam_debouncer = new Debouncer(SpindexerConstants.JAM_DEBOUNCE_TIME, DebounceType.kRising); // if there is jam I would think this is 0 -> 1
-
- private boolean reversing = false;
- private boolean wasHoodForcedDown = false;
-
- private Timer reverseTimer = new Timer();
-
- private double storedIntakeSpeed = 0.0;
-
- public RunSpindexer(Spindexer spindexer, Turret turret, Hood hood, Intake intake) {
- this.spindexer = spindexer;
- this.turret = turret;
- this.hood = hood;
- this.intake = intake;
-
- addRequirements(spindexer);
- }
-
- // public RunSpindexer(Spindexer spindexer) {
- // this.spindexer = spindexer;
- // addRequirements(spindexer);
- // }
-
- @Override
- public void initialize() {
- wasHoodForcedDown = hood.getHoodForcedDown();
- }
-
- @Override
- public void execute() {
- boolean hoodForcedDown = hood.getHoodForcedDown();
-
- if (wasHoodForcedDown && !hoodForcedDown) {
- spindexer.maxSpindexer();
- }
- wasHoodForcedDown = hoodForcedDown;
-
- if (!turret.atSetpoint() || hoodForcedDown || spindexer.noIndexing) {
- spindexer.stopSpindexer();
- reversing = false;
- return; // this is so the balls don't fly out when unaligned
- }
- boolean jammed = spindexer.getMotorOneVelocity() < SpindexerConstants.JAM_CURRENT_THRESHOLD && spindexer.getMotorOneStatorCurrent() > SpindexerConstants.JAM_CURRENT_THRESHOLD;
- Logger.recordOutput("SpindexerJammed", jammed);
- if (jam_debouncer.calculate(jammed)) {
- Logger.recordOutput("SpindexerJammedDebounced", jammed);
-
- reversing = true;
- reverseTimer.reset();
- reverseTimer.start();
- storedIntakeSpeed = intake.getSpeed();
- }
- if (!reversing) {
- spindexer.maxSpindexer();
- } else {
- spindexer.reverseSpindexer();
-
- if (intake.getPosition() > IntakeConstants.INTERMEDIATE_EXTENSION + 1.0) {
- intake.spinReverse();
- } else {
- intake.extend();
- }
-
- if (reverseTimer.hasElapsed(SpindexerConstants.REVERSE_DEBOUNCE_TIME)) {
- reversing = false;
- intake.spin(storedIntakeSpeed);
- }
- }
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putBoolean("Spindexer Jamming", reversing);
- }
- }
-
- @Override
- public void end(boolean interrupted) {
- spindexer.stopSpindexer();
- reversing = false;
- }
-
- // @Override
- // public boolean isFinished() {
- // return false; // never ends on its own
- // }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.Constants;
-import frc.robot.subsystems.Intake.Intake;
-import frc.robot.subsystems.Intake.IntakeConstants;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.spindexer.Spindexer;
-import frc.robot.subsystems.spindexer.SpindexerConstants;
-import frc.robot.subsystems.turret.Turret;
-
-public class RunSpindexerWithStop extends Command {
- private Spindexer spindexer;
- private Turret turret;
- private Hood hood;
- private Intake intake;
-
- // if there is jam I would think this is 0 -> 1
- private Debouncer jam_debouncer = new Debouncer(SpindexerConstants.JAM_DEBOUNCE_TIME, DebounceType.kRising);
-
- private boolean reversing = false;
- private boolean wasHoodForcedDown = false;
-
- private Timer reverseTimer = new Timer();
-
- private double storedIntakeSpeed = 0.0;
-
- private Timer runTimer = new Timer();
-
- private Debouncer debouncer = new Debouncer(0.3, DebounceType.kRising);
-
- private final double interval = 0.6; // Change this to make it faster/slower (seconds)
-
- public RunSpindexerWithStop(Spindexer spindexer, Turret turret, Hood hood, Intake intake) {
- this.spindexer = spindexer;
- this.turret = turret;
- this.hood = hood;
- this.intake = intake;
-
- addRequirements(spindexer);
- }
-
- @Override
- public void initialize() {
- wasHoodForcedDown = hood.getHoodForcedDown();
- runTimer.reset();
- runTimer.start();
- }
-
- @Override
- public void execute() {
- boolean hoodForcedDown = hood.getHoodForcedDown();
-
- if (wasHoodForcedDown && !hoodForcedDown) {
- spindexer.maxSpindexer();
- }
- wasHoodForcedDown = hoodForcedDown;
-
- if (!turret.atSetpoint() || hoodForcedDown || spindexer.noIndexing) {
- spindexer.stopSpindexer();
- reversing = false;
- return; // this is so the balls don't fly out when unaligned
- }
- boolean jammed = spindexer.getSubsystemStatorCurrent() / 2 > SpindexerConstants.JAM_CURRENT_THRESHOLD;
- Logger.recordOutput("SpindexerJammed", jammed);
- if (jam_debouncer.calculate(jammed)) {
- Logger.recordOutput("SpindexerJammedDebounced", jammed);
-
- reversing = true;
- reverseTimer.reset();
- reverseTimer.start();
- storedIntakeSpeed = intake.getSpeed();
- }
- if (!reversing) {
- spindexer.maxSpindexer();
- } else {
- spindexer.reverseSpindexer();
-
- if (intake.getPosition() > IntakeConstants.INTERMEDIATE_EXTENSION + 1.0) {
- intake.spinReverse();
- } else {
- intake.extend();
- }
-
- if (reverseTimer.hasElapsed(SpindexerConstants.REVERSE_DEBOUNCE_TIME)) {
- reversing = false;
- intake.spin(storedIntakeSpeed);
- }
- }
-
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putBoolean("Spindexer Jamming", reversing);
- }
-
- if ((int) (Timer.getFPGATimestamp() / interval) % 2 == 0) {
- intake.extend();
- } else {
- intake.intermediateExtend();
- }
-
- }
-
- @Override
- public void end(boolean interrupted) {
- spindexer.stopSpindexer();
- reversing = false;
-
- intake.extend();
- }
-
- @Override
- public boolean isFinished() {
- return (runTimer.hasElapsed(1.0) && debouncer.calculate(spindexer.spinningAir())) || runTimer.hasElapsed(4.0);
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.FieldConstants;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.shooter.ShooterConstants;
-import frc.robot.subsystems.turret.Turret;
-
-public class SimpleAutoShoot extends Command {
- private Turret turret;
- private Drivetrain drivetrain;
- private Shooter shooter;
-
- private double fieldAngleRad;
- private double turretSetpoint;
-
- private boolean SOTM = true;
- private Translation2d drivepose;
-
- public SimpleAutoShoot(Turret turret, Drivetrain drivetrain, Shooter shooter) {
- this.turret = turret;
- this.drivetrain = drivetrain;
- drivepose = drivetrain.getPose().getTranslation();
-
- addRequirements(turret);
- }
-
- public void updateTurretSetpoint(Translation2d drivepose) {
-
- //FieldZone currentZone = getZone(drivepose);
- Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
-
- double D_y;
- double D_x;
- double timeToGoal = 0.0;
-
- // If the robot is moving, adjust the target position based on velocity
- if (SOTM) {
- ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
- ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
- double xVel = fieldRelVel.vxMetersPerSecond;
- double yVel = fieldRelVel.vyMetersPerSecond;
-
- D_y = target.getY() - drivepose.getY() - timeToGoal * yVel;
- D_x = target.getX() - drivepose.getX() - timeToGoal * xVel;
- } else {
- D_y = target.getY() - drivepose.getY();
- D_x = target.getX() - drivepose.getX();
- }
-
- // Calculate the field-relative angle
- fieldAngleRad = Math.atan2(D_y, D_x);
-
- // Calculate robot heading and adjust for reverse drive
- double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive adjustment
-
- // Calculate turret setpoint (angle relative to robot heading)
- turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0);
-
- }
-
- @Override
- public void initialize() {
- // Initialize setpoint calculation and set the initial goal for the turret
- updateTurretSetpoint(drivepose);
- turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), 0);
- }
-
- @Override
- public void execute() {
- // Continuously update setpoints and adjust based on vision if available
- drivepose = drivetrain.getPose().getTranslation();
- updateTurretSetpoint(drivepose);
-
- turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2));
- shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY);
-
- }
-
- @Override
- public void end(boolean interrupted) {
- // Set the turret to a safe position when the command ends
- turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
- shooter.setShooter(0.0);
- }
-
-}
-
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.filter.LinearFilter;
-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.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.Constants;
-import frc.robot.constants.FieldConstants;
-import frc.robot.constants.ShotInterpolation;
-import frc.robot.constants.ShuttleInterpolation;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.hood.HoodConstants;
-import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.spindexer.Spindexer;
-import frc.robot.subsystems.turret.Turret;
-import frc.robot.subsystems.turret.TurretConstants;
-import frc.robot.util.PhaseManager;
-import frc.robot.util.ShooterPhysics;
-import frc.robot.util.ShooterPhysics.TurretState;
-
-public class Superstructure extends Command {
- private Turret turret;
- private Drivetrain drivetrain;
- private Hood hood;
- private Shooter shooter;
- private Spindexer spindexer;
-
- private double turretSetpoint;
- private double hoodSetpoint;
-
- private Pose2d drivepose;
-
- private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
- private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
-
- private Rotation2d lastTurretAngle;
- private Rotation2d turretAngle;
- private double turretVelocity;
-
- private double lastHoodAngle;
- private double hoodAngle;
- private double hoodVelocity;
-
- private TurretState goalState;
-
- private LoggedNetworkNumber phaseDelay = new LoggedNetworkNumber("/Tuning/OPERATOR/Phase Delay", 0.03); //Extrapolation delay due to latency
-
- private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d();
-
- private PhaseManager phaseManager = new PhaseManager();
-
- private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Hood Offset", 0.0);
-
- private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Turret Offet",0.0);
-
- private double distanceFromTarget = 0.0;
- private LoggedNetworkNumber TOFAdjustment = new LoggedNetworkNumber("/Tuning/OPERATOR/TOF Adjustment", 1.1);
-
- public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
- this.turret = turret;
- this.drivetrain = drivetrain;
- this.hood = hood;
- this.shooter = shooter;
- this.spindexer = spindexer;
- drivepose = drivetrain.getPose();
-
- goalState = ShooterPhysics.getShotParams(
- Translation2d.kZero,
- FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
- 8.0); // Random initial goalState to prevent it being null
-
- addRequirements(turret, shooter);
- }
-
- public void updateSetpoints(Pose2d drivepose) {
- Pose2d turretPosition = drivepose.transformBy(
- new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero));
-
- // If the robot is moving, adjust the target position based on velocity
- ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
- ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
-
- // Rotational adjustment is not being used, since turret is in center of robot
- double turretVelocityX =
- fieldRelVel.vxMetersPerSecond;
- double turretVelocityY =
- fieldRelVel.vyMetersPerSecond;
-
- double timeOfFlight = 0;
- Pose2d lookaheadPose = turretPosition;
-
- /*
- * Loop (max 20) until lookaheadPose converges BECAUSE -->
- * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s)
- * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s
- * So we make a bunch of guesses until it converges
- * Early exit when change < 1mm to avoid unnecessary iterations
- */
- for (int i = 0; i < 20; i++) {
- Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
-
- Translation3d target3d = new Translation3d(target.getX(), target.getY(),
- target.equals(FieldConstants.getHubTranslation().toTranslation2d()) ?
- FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub
-
- goalState = ShooterPhysics.getShotParams(
- Translation2d.kZero,
- target3d.minus(lookahead3d),
- 2.0);
-
- timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get();
- double offsetX = turretVelocityX * timeOfFlight;
- double offsetY = turretVelocityY * timeOfFlight;
- Pose2d newLookaheadPose =
- new Pose2d(
- turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
- turretPosition.getRotation());
-
- // early exit if converged (change < 1mm)
- if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) {
- lookaheadPose = newLookaheadPose;
- break;
- }
- lookaheadPose = newLookaheadPose;
- }
-
- // Get the field angle relative to the target pose
- turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
- if (lastTurretAngle == null) {
- lastTurretAngle = turretAngle;
- }
-
- // Take the filtered average as the turret's velocity when robot is moving translationally
- turretVelocity =
- turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
-
- lastTurretAngle = turretAngle;
-
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Turret/Target Pose", target);
- Logger.recordOutput("Lookahead Pose", lookaheadPose);
- }
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putNumber("Time of flight", timeOfFlight);
- SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX);
- SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY);
- }
-
- // Subtract the rotational angle of the robot from the setpoint
- double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
-
- // Shortest path
- double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
- double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset.get();
-
- // Stay within physical limits -- if shortest path is past max angle, we go long way around
- if (potentialSetpoint > TurretConstants.MAX_ANGLE) {
- potentialSetpoint -= 360;
- } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) {
- potentialSetpoint += 360;
- }
-
- turretSetpoint = potentialSetpoint;
-
- // Pitch is in radians
- hoodAngle = goalState.pitch();
- hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
- hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
- lastHoodAngle = hoodAngle;
-
- distanceFromTarget = target.getDistance(lookaheadPose.getTranslation());
- Logger.recordOutput("Shooting/distanceToTarget", distanceFromTarget);
- }
-
- public void updateDrivePose(){
- Pose2d currentPose = drivetrain.getPose();
-
- drivepose = new Pose2d(
- currentPose.getTranslation(),
- // Uncomment this if robot is backwards
- currentPose.getRotation()//.plus(new Rotation2d(Math.PI))
- );
- ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
-
- // Add a phase delay extrapolation component for latency delay
- // drivepose = drivepose.exp(
- // new Twist2d(
- // robotRelVel.vxMetersPerSecond * phaseDelay.get(),
- // robotRelVel.vyMetersPerSecond * phaseDelay.get(),
- // robotRelVel.omegaRadiansPerSecond * phaseDelay.get()));
- }
-
- /**
- * Stops and stows all subsystems involved in the command
- */
- public void stowEverything(){
- turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
- hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
- shooter.setShooter(0.0);
- spindexer.noIndexing = true;
- }
-
- public void underLadder(){
- spindexer.noIndexing = true;
- }
-
- // shoot higher
- public void bumpUpHoodOffset() {
- hoodOffset.set(hoodOffset.get() + 1.0); //1 deg
- }
-
- // shoot lower
- public void bumpDownHoodOffset() {
- hoodOffset.set(hoodOffset.get() - 1.0); //1 deg
- }
-
- // aim more left
- public void bumpUpTurretOffset() {
- turretOffset.set(turretOffset.get() + 2.5); //2.5 deg
- }
-
- // aim more right
- public void bumpDownTurretOffset() {
- turretOffset.set(turretOffset.get() - 2.5); //2.5 deg
- }
-
- @Override
- public void execute() {
- // Phase manager stuff
- phaseManager.update(drivepose, shooter, turret);
- target = phaseManager.getTarget(drivepose);
-
- updateDrivePose();
- updateSetpoints(drivepose);
-
- if (phaseManager.isIdle()) {
- underLadder();
- } else {
- if (spindexer.noIndexing) {
- spindexer.noIndexing = false;
- }
- turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2));
-
- boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at the hub, we're not shuttling
-
- // shuttling will move the hood so its angles very close to max (less arch)
- if (shuttling) {
- hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
- } else {
- hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
- }
-
- // if (FieldConstants.underTrench(x, y)) {
- // System.out.println("Hood forced down");
- // } else {
- // hood.forceHoodDown(false);
- // }
-
- // different maps for shuttling vs shooting. Less powerful when shuttling.
- if (shuttling) {
- shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget));
- } else {
- shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
- }
-
- if (!Constants.DISABLE_LOGGING) {
- // record when shuttling
- Logger.recordOutput("Shuttling", shuttling);
- // record distance for tuning if needed
- Logger.recordOutput("Distance From Target", distanceFromTarget);
- }
- }
-
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint);
- Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint);
- Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel());
-
- Logger.recordOutput("DistanceToTarget", target.getDistance(drivepose.getTranslation()));
- }
-
- // for operator
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString());
-
- } else {
- phaseDelay.set(0.03);
- }
- }
-
- @Override
- public void end(boolean interrupted) {
- stowEverything();
- }
-
-}
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
-import frc.robot.util.Zone;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.Robot;
import frc.robot.constants.swerve.DriveConstants;
/** Height of the field [meters] */
public static final double FIELD_WIDTH = field.getFieldWidth();
- public static final double RED_BORDER = FIELD_LENGTH / 2 + Units.inchesToMeters(155);
- public static final double BLUE_BORDER = FIELD_LENGTH / 2 - Units.inchesToMeters(155);
- public static final double LEFT_SIDE_TARGET = FIELD_WIDTH * 0.225;
- public static final double RIGHT_SIDE_TARGET = FIELD_WIDTH * 0.775;
-
- /** Location of hub target */
- public static final Translation3d HUB_BLUE = new Translation3d(Units.inchesToMeters(182.11), FIELD_WIDTH / 2,
- Units.inchesToMeters(72));
-
- public static final Translation3d HUB_RED = new Translation3d(FIELD_LENGTH - Units.inchesToMeters(182.11),
- FIELD_WIDTH / 2, Units.inchesToMeters(72));
-
- // shuttle safety constants
- public static final double LADDER_ZONE_WIDTH = 1.251; // meters
- public static final double LADDER_ZONE_DEPTH = 1.143; // meters
-
- // tower positions (center of the zone)
- public static final double LADDER_Y_OFFSET = 1.7526 * 2; // two driver stations
- public static final Pose2d BLUE_CLIMB_LOCATION = new Pose2d(LADDER_ZONE_DEPTH / 2,
- FIELD_WIDTH - LADDER_Y_OFFSET - LADDER_ZONE_WIDTH / 2, new Rotation2d());
- public static final Pose2d RED_CLIMB_LOCATION = new Pose2d(FIELD_LENGTH - LADDER_ZONE_DEPTH / 2,
- LADDER_Y_OFFSET + LADDER_ZONE_WIDTH / 2, new Rotation2d());
-
- public static final Pose2d getClimbLocation() {
- if (Robot.getAlliance() == Alliance.Blue) {
- return BLUE_CLIMB_LOCATION;
- } else {
- return RED_CLIMB_LOCATION;
- }
- }
-
- public static final Translation3d NEUTRAL_LEFT = new Translation3d(FIELD_LENGTH / 2, LEFT_SIDE_TARGET, 0);
-
- public static final Translation3d NEUTRAL_RIGHT = new Translation3d(FIELD_LENGTH / 2, RIGHT_SIDE_TARGET, 0);
-
- // previous hub + a few feet further back
- public static final Translation3d ALLIANCE_LEFT_BLUE = new Translation3d(BLUE_BORDER - 3.2, LEFT_SIDE_TARGET, 0);
-
- public static final Translation3d ALLIANCE_RIGHT_BLUE = new Translation3d(BLUE_BORDER - 2.2, RIGHT_SIDE_TARGET, 0);
- // previous hub + a few feet further back
-
- public static final Translation3d ALLIANCE_LEFT_RED = new Translation3d(RED_BORDER + 2.2, LEFT_SIDE_TARGET, 0);
-
- public static final Translation3d ALLIANCE_RIGHT_RED = new Translation3d(RED_BORDER + 3.2, RIGHT_SIDE_TARGET, 0);
-
- public static final Translation3d ALLIANCE_CENTER_BLUE = new Translation3d(BLUE_BORDER - 2, FIELD_WIDTH / 2, 0);
-
- public static final Translation3d ALLIANCE_CENTER_RED = new Translation3d(RED_BORDER + 2, FIELD_WIDTH / 2, 0);
-
- public static final double BLUE_ALLIANCE_LINE = BLUE_BORDER; // That's the distance from one side to the blue bump
- public static final double RED_ALLIANCE_LINE = RED_BORDER; // That's the distance from one side to the red bump
-
- // my zones
- public static final double leftNeutralLine = FIELD_LENGTH * 0.25;
- public static final double rightNeutralLine = FIELD_LENGTH * 0.75;
- public static final double centerLengthLine = FIELD_LENGTH * 0.5;
- public static final double centerWidthLine = FIELD_WIDTH * 0.5;
- public static final double redLine = 179.111250;
- public static final double blueLine = FIELD_LENGTH - 179.111250;
-
- public static final double hubWidthLeft = FIELD_WIDTH / 2 - (47.0 / 2);
- public static final double hubWidthRight = FIELD_WIDTH / 2 + (47.0 / 2);
- public static final double hubBackRed = FIELD_LENGTH + 120.0;
- public static final double hubBackBlue = FIELD_LENGTH - 120.0;
-
- public static final double ladderRedLeft = FIELD_WIDTH + 13.0;
- public static final double ladderBlueLeft = FIELD_WIDTH - 12.375;
- public static final double ladderRedRight = FIELD_WIDTH - 35.75;
- public static final double ladderBlueRight = FIELD_WIDTH + 35.75;
-
- public static final double TRENCH_CENTER_CHANNEL_WIDTH_INCHES = 50.0;
- public static final double TRENCH_X_MIN_INCHES = 152.5;
- public static final double TRENCH_X_MAX_INCHES = 187.5;
-
- public static final Zone neutralStrip = new Zone(centerLengthLine, centerWidthLine,
- rightNeutralLine - leftNeutralLine, redLine - blueLine);
- public static final Zone neutralLeft = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine,
- redLine - blueLine);
- public static final Zone neutralRight = new Zone(centerLengthLine, centerWidthLine,
- rightNeutralLine - leftNeutralLine, redLine - blueLine);
- public static final Zone blueHubOut = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine,
- redLine - blueLine);
- public static final Zone redHubOut = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine,
- redLine - blueLine);
-
- public enum ShootingTarget {
- HUB,
- NEUTRAL,
- ALLIANCE,
- OPPOSITION, // not sure why you'd ever do this :)
- }
-
- public enum FieldZone {
- ALLIANCE,
- NEUTRAL,
- OPPOSITION,
- TRENCH_BUMP,
- UNDER_LADDER,
- UNDER_MY_HUB
- }
-
- /** checks if robot is under climb structure */
- public static boolean underLadder(Translation2d drivepose) {
- Pose2d ladderPos = getClimbLocation();
- double dx = Math.abs(drivepose.getX() - ladderPos.getX());
- double dy = Math.abs(drivepose.getY() - ladderPos.getY());
- return (dx < LADDER_ZONE_DEPTH / 2.0) && (dy < LADDER_ZONE_WIDTH / 2.0);
- }
-
- public static Translation3d getHubTranslation() {
- if (Robot.getAlliance() == Alliance.Blue) {
- return HUB_BLUE;
- } else {
- return HUB_RED;
- }
- }
-
- public static Translation3d getNeutralTranslation(boolean sideLeft) {
- if (sideLeft) {
- return NEUTRAL_LEFT;
- } else {
- return NEUTRAL_RIGHT;
- }
- }
-
- public static Translation3d getAllianceSideTranslation(boolean sideLeft) {
- if (sideLeft) {
- if (Robot.getAlliance() == Alliance.Blue) {
- return ALLIANCE_LEFT_BLUE;
- } else {
- return ALLIANCE_LEFT_RED;
- }
- } else {
- if (Robot.getAlliance() == Alliance.Blue) {
- return ALLIANCE_RIGHT_BLUE;
- } else {
- return ALLIANCE_RIGHT_RED;
- }
- }
- }
-
- public static Translation3d getAllianceCenterTranslation() {
- if (Robot.getAlliance() == Alliance.Blue) {
- return ALLIANCE_CENTER_BLUE;
- } else {
- return ALLIANCE_CENTER_RED;
- }
- }
-
- public static FieldZone getZone(Translation2d drivepose) {
- double x = drivepose.getX();
-
- if ((x < FIELD_LENGTH / 2 - Units.inchesToMeters(120.0)
- && x > (BLUE_ALLIANCE_LINE + (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2)) // blue alliance line
- || x > FIELD_LENGTH / 2 + Units.inchesToMeters(120.0)
- && x < (RED_ALLIANCE_LINE - (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2)) {
- return FieldZone.TRENCH_BUMP;
- }
-
- // if (underLadder(drivepose)) {
- // return FieldZone.UNDER_LADDER;
- // }
-
- if (x > FieldConstants.RED_ALLIANCE_LINE - (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2) {
- return (Robot.getAlliance() == Alliance.Red) ? FieldZone.ALLIANCE : FieldZone.OPPOSITION;
- } else if (x < FieldConstants.BLUE_ALLIANCE_LINE + (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2) {
- return (Robot.getAlliance() == Alliance.Blue) ? FieldZone.ALLIANCE : FieldZone.OPPOSITION;
- } else {
- return FieldZone.NEUTRAL;
- }
- }
-
- /**
- * returns true if robot is currently underneath or touching the climbing ladder
- */
- public static boolean isUnderLadder(Translation2d drivepose) {
- return getZone(drivepose) == FieldZone.UNDER_LADDER;
- }
-
- public static boolean underTrench(double x, double y) {
- // ensures we aren't in center channel
- if (y > Units.inchesToMeters(TRENCH_CENTER_CHANNEL_WIDTH_INCHES)
- && y < FIELD_WIDTH - Units.inchesToMeters(TRENCH_CENTER_CHANNEL_WIDTH_INCHES)) {
- return false;
- }
- // if our location is to far away from right underneath trench in terms of x
- // in between blue alliance trench
- if (!(x > Units.inchesToMeters(TRENCH_X_MIN_INCHES) && x < Units.inchesToMeters(TRENCH_X_MAX_INCHES))
- && !(x < FIELD_LENGTH - Units.inchesToMeters(TRENCH_X_MIN_INCHES)
- && x > FIELD_LENGTH - Units.inchesToMeters(TRENCH_X_MAX_INCHES))) {
- return false;
- }
- return true;
- }
/**
*
return drivepose.getY() > FIELD_WIDTH / 2;
}
- public static Translation3d getOppositionTranslation(boolean sideLeft) {
- if (sideLeft) {
- if (Robot.getAlliance() == Alliance.Blue) {
- return ALLIANCE_LEFT_RED;
- } else {
- // Reversed it so we shoot same side, but probably need to change this
- return ALLIANCE_LEFT_BLUE;
- }
- } else {
- if (Robot.getAlliance() == Alliance.Blue) {
- return ALLIANCE_RIGHT_RED;
- } else {
- return ALLIANCE_RIGHT_BLUE;
- }
- }
- }
-
}
+++ /dev/null
-package frc.robot.constants;
-
-import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
-
-public class ShotInterpolation {
- public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap();
- public static final InterpolatingDoubleTreeMap shooterPowerMap = new InterpolatingDoubleTreeMap();
- public static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap();
-
- public static final InterpolatingDoubleTreeMap exitVelocityMap = new InterpolatingDoubleTreeMap();
-
- public static final InterpolatingDoubleTreeMap shooterVelocityMap = new InterpolatingDoubleTreeMap();
-
- public static final InterpolatingDoubleTreeMap newHoodMap = new InterpolatingDoubleTreeMap();
-
- static{
- timeOfFlightMap.put(0.0, 0.67);
- timeOfFlightMap.put(1.0, 0.67);
-
- shooterPowerMap.put(0.0, 1.0);
- shooterPowerMap.put(1.0, 1.0);
-
- //hoodAngleMap.put(HoodConstants.MAX_ANGLE, HoodConstants.MAX_ANGLE);
- hoodAngleMap.put(81.3, 70.25);
- hoodAngleMap.put(79.0, 65.9);
- hoodAngleMap.put(58.5, 48.5);
- //hoodAngleMap.put(1.0, Units.degreesToRadians(90));
-
- exitVelocityMap.put(0.0, 0.0);
- exitVelocityMap.put(1.0, 2.2);
- exitVelocityMap.put(2.0, 4.4);
- exitVelocityMap.put(7.0, 12.0);
- exitVelocityMap.put(7.78, 16.8);
- exitVelocityMap.put(7.8, 15.2);
- exitVelocityMap.put(7.9, 17.1);
- exitVelocityMap.put(8.0, 17.9);
- exitVelocityMap.put(8.08, 19.0);
- exitVelocityMap.put( 21.0, 19.0);
-
- exitVelocityMap.put(9.90, 14.0);
- exitVelocityMap.put(9.95, 16.5);
- exitVelocityMap.put(10.0, 19.2);
- exitVelocityMap.put(11.0, 26.0);
- exitVelocityMap.put(25.0, 25.0* 3.2);
-
- // currently regresses to y = 1.34959x + 9.79618
- shooterVelocityMap.put(0.0, 9.55 * 1.05);
- shooterVelocityMap.put(1.00, 11.5 * 1.05);
- shooterVelocityMap.put(2.00, 12.3 * 1.1);
- shooterVelocityMap.put(3.00, 14.0 * 1.075);
- shooterVelocityMap.put(4.00, 15.5 * 1.075);
- shooterVelocityMap.put(5.00, 17.0 * 1.05);
- shooterVelocityMap.put(5.60, 18.0 * 1.05);
- shooterVelocityMap.put(25.0, 43.44 * 1.05);
-
-
- newHoodMap.put(0.0, 75.9);
- newHoodMap.put(1.00, 78.0);
- newHoodMap.put(1.49, 72.0 - 2.0 - 1.0);
- newHoodMap.put(2.09, 70.0 - 4.0 - 2.0);
- newHoodMap.put(2.95, 68.0 - 4.0 - 2.0);
- newHoodMap.put(4.07, 65.0 - 3.0 - 2.0);
- newHoodMap.put(5.05, 60.0 - 1.0 - 1.0);
- newHoodMap.put(5.79, 59.0 - 2.0);
- newHoodMap.put(27.99, 0.0);
- }
-}
+++ /dev/null
-package frc.robot.constants;
-
-import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
-
-public class ShuttleInterpolation {
- public static final InterpolatingDoubleTreeMap newHoodMap = new InterpolatingDoubleTreeMap();
- public static final InterpolatingDoubleTreeMap shooterVelocityMap = new InterpolatingDoubleTreeMap();
- /*
- * guide to tuning:
- * modify the left values in the parenthesis to change the distance of the shot.
- * setup the robot at that distance and shoot. Then modifty the right value to
- * be more or less until it hits target perfectly.
- * Repeat
- * OR
- * Run robot, estimate distance, and tweak values when nobodies looking until it
- * works.
- */
- static {
- // we can be less aggressive: y = 0.65 * (1.34959x + 9.79618)
- // will likely be this that requires tuning.
- shooterVelocityMap.put(0.0, 9.0);
- shooterVelocityMap.put(4.0, 12.0 * 1.3); // tuned by wesley
- shooterVelocityMap.put(8.0, 22.0 * 1.075); // tuned by wesley
- shooterVelocityMap.put(16.0, 100.0); // tuned by taren
-
- // always shoot at low angle to ground.
- newHoodMap.put(0.0, 55.0); // min angle (w/ 0.5 deg buffer)
- newHoodMap.put(27.99, 55.0); // min angle (w/ 0.5 deg buffer)
- }
-}
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.robot.Robot;
import frc.robot.commands.drive_comm.SysIDDriveCommand;
-import frc.robot.commands.gpm.IntakeMovementCommand;
-import frc.robot.commands.gpm.ReverseMotors;
-import frc.robot.commands.gpm.RunSpindexer;
-import frc.robot.commands.gpm.Superstructure;
import frc.robot.constants.Constants;
-import frc.robot.subsystems.Intake.Intake;
import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.spindexer.Spindexer;
-import frc.robot.subsystems.turret.Turret;
import lib.controllers.PS5Controller;
import lib.controllers.PS5Controller.DPad;
import lib.controllers.PS5Controller.PS5Axis;
* Driver controls for the PS5 controller
*/
public class PS5ControllerDriverConfig extends BaseDriverConfig {
- private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = () -> false;
- private boolean intakeBoolean = true;
- private Command autoShoot = null;
- private Shooter shooter;
- private Turret turret;
- private Hood hood;
- private Intake intake;
- private Spindexer spindexer;
-
- public PS5ControllerDriverConfig(
- Drivetrain drive,
- Shooter shooter,
- Turret turret,
- Hood hood,
- Intake intake,
- Spindexer spindexer) {
- super(drive);
- this.shooter = shooter;
- this.turret = turret;
- this.hood = hood;
- this.intake = intake;
- this.spindexer = spindexer;
- }
-
- public void configureControls() {
- // Reset the yaw. Mainly useful for testing/driver practice
- controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
-
- // Cancel commands
- controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
- getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
-
- // Reverse motors
- if (intake != null && spindexer != null) {
- controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake));
- }
-
- // Intake
- if (intake != null) {
- // Toggle intake
- controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
- if (intakeBoolean) {
- intake.extend();
- intake.spinStart();
- intakeBoolean = false;
- } else {
- intake.intermediateExtend();
- intake.spinStop();
- intakeBoolean = true;
- }
- }, intake));
-
- // Retract if hold for 2 seconds
- controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> {
- intake.retract();
- intakeBoolean = true;
- intake.spinStop();
- }, intake));
-
- // Make the intake go in and out while shooting
- controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake)
- .alongWith(new InstantCommand(()-> intakeBoolean = true)));
-
- // Calibration: you can now calibrate easily using this button
- if (hood != null && intake != null) {
- controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
- intake.calibrate();
- hood.calibrate();
- }, intake, hood)).onFalse(new InstantCommand(() -> {
- intake.stopCalibrating();
- hood.stopCalibrating();
- }, intake, hood));
- }
-
- // Stop intake roller
- controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{
- if(intakeBoolean){
- intake.spinStart();
- intakeBoolean = false;
- } else{
- intake.spinStop();
- intakeBoolean = true;
- }
- }));
- }
-
- // Spindexer
- if (spindexer != null && turret != null && hood != null && intake != null) {
-
- // Toggle spindexer
- controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
- new RunSpindexer(spindexer, turret, hood, intake)
- );
- }
-
- // Auto shoot
- if (turret != null && hood != null && shooter != null && spindexer != null) {
- autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
- controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot);
- }
-
-
- // Hood
- if (hood != null) {
- // Set the hood down -- for safety measures under trench
- controller.get(DPad.LEFT).onTrue(new InstantCommand(()->{
- hood.forceHoodDown(true);
- }, hood)).onFalse(new InstantCommand(()->{
- hood.forceHoodDown(false);
- }));
- }
- }
-
- @Override
- public double getRawSideTranslation() {
- return controller.get(PS5Axis.LEFT_X);
- }
-
- @Override
- public double getRawForwardTranslation() {
- return controller.get(PS5Axis.LEFT_Y);
- }
-
- @Override
- public double getRawRotation() {
- return controller.get(PS5Axis.RIGHT_X);
- }
-
- @Override
- public double getRawHeadingAngle() {
- return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
- }
-
- @Override
- public double getRawHeadingMagnitude() {
- return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
- }
-
- @Override
- public boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
- }
-
- @Override
- public boolean getIsAlign() {
- return false;
- }
+ private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = () -> false;
+
+ public PS5ControllerDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
+
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(PS5Axis.LEFT_X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(PS5Axis.LEFT_Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return controller.get(PS5Axis.RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
}
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.Robot;
-import frc.robot.commands.gpm.ReverseMotors;
-import frc.robot.commands.gpm.Superstructure;
import frc.robot.constants.Constants;
import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.Intake.Intake;
-import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.spindexer.Spindexer;
-import frc.robot.subsystems.turret.Turret;
import lib.controllers.GameController;
import lib.controllers.GameController.Axis;
import lib.controllers.GameController.Button;
*/
public class PS5XboxModeDriverConfig extends BaseDriverConfig {
- private final GameController controller = new GameController(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = () -> false;
- private boolean intakeBoolean = true;
- private Command autoShoot = null;
- private Command reverseMotors = null;
- private Shooter shooter;
- private Turret turret;
- private Hood hood;
- private Intake intake;
- private Spindexer spindexer;
-
- // PS5 button aliases
- // private final Button CROSS = Button.A;
- private final Button CIRCLE = Button.B;
- private final Button SQUARE = Button.X;
- // private final Button TRIANGLE = Button.Y;
- // private final Button LB = Button.LB;
- private final Button RB = Button.RB;
- private final Button CREATE = Button.BACK;
- // private final Button OPTIONS = Button.START;
- private final Button LEFT_JOY = Button.LEFT_JOY;
- private final Button RIGHT_JOY = Button.RIGHT_JOY;
-
- // PS5 trigger buttons
- private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON;
- private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON;
-
- // PS5 axis aliases
- private final Axis LEFT_X = Axis.LEFT_X;
- private final Axis LEFT_Y = Axis.LEFT_Y;
- private final Axis RIGHT_X = Axis.RIGHT_X;
- private final Axis RIGHT_Y = Axis.RIGHT_Y;
- // private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
- // private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
-
- public PS5XboxModeDriverConfig(
- Drivetrain drive,
- Shooter shooter,
- Turret turret,
- Hood hood,
- Intake intake,
- Spindexer spindexer) {
- super(drive);
- this.shooter = shooter;
- this.turret = turret;
- this.hood = hood;
- this.intake = intake;
- this.spindexer = spindexer;
- }
-
- public void configureControls() {
- // Reset the yaw. Mainly useful for testing/driver practice
- controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
-
- // Cancel commands
- controller.get(RB).onTrue(new InstantCommand(() -> {
- getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
-
- // Align wheels
- controller.get(DPad.RIGHT).onTrue(new FunctionalCommand(
- () -> getDrivetrain().setStateDeadband(false),
- getDrivetrain()::alignWheels,
- interrupted -> getDrivetrain().setStateDeadband(true),
- () -> false, getDrivetrain()).withTimeout(2));
-
- // Trench align
- controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
- getDrivetrain().setTrenchAssist(true);
- getDrivetrain().setTrenchAlign(true);
- }))
- .onFalse(new InstantCommand(() -> {
- getDrivetrain().setTrenchAssist(false);
- getDrivetrain().setTrenchAlign(false);
- }));
-
- // Reverse motors
- if (intake != null && spindexer != null && shooter != null) {
- controller.get(CIRCLE).onTrue(new InstantCommand(() -> {
- reverseMotors = new ReverseMotors(intake);
- CommandScheduler.getInstance().schedule(reverseMotors);
- })).onFalse(new InstantCommand(() -> {
- if (reverseMotors != null) {
- reverseMotors.cancel();
- }
- }));
- }
-
- // Intake
- if (intake != null) {
- // Toggle intake
- controller.get(RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
- if (intakeBoolean) {
- intake.extend();
- intake.spinStart();
- intakeBoolean = false;
- } else {
- intake.intermediateExtend();
- intake.spinStop();
- intakeBoolean = true;
- }
- }));
-
- // Retract if hold for 3 seconds
- controller.get(RIGHT_TRIGGER_BUTTON).debounce(3.0).onTrue(new InstantCommand(() -> {
- intake.retract();
- intakeBoolean = true;
- }));
-
- // Calibration
- controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
- intake.calibrate();
- })).onFalse(new InstantCommand(() -> {
- intake.stopCalibrating();
- }));
- }
-
- // Spindexer
- if (spindexer != null) {
- // Will only run if we are not calling default shoot command
- controller.get(LEFT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
- .onFalse(new InstantCommand(() -> spindexer.stopSpindexer()));
- }
-
- // Auto shoot
- if (turret != null && hood != null && shooter != null && spindexer != null) {
- autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
- controller.get(SQUARE).toggleOnTrue(autoShoot);
- }
-
- // Hood
- if (hood != null) {
- controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
- hood.calibrate();
- })).onFalse(new InstantCommand(() -> {
- hood.stopCalibrating();
- }));
- }
-
- // Rumble test
- controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup(
- new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)),
- new WaitCommand(0.5),
- new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF))));
- }
-
- @Override
- public double getRawSideTranslation() {
- return controller.get(LEFT_X);
- }
-
- @Override
- public double getRawForwardTranslation() {
- return controller.get(LEFT_Y);
- }
-
- @Override
- public double getRawRotation() {
- return controller.get(RIGHT_X);
- }
-
- @Override
- public double getRawHeadingAngle() {
- return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2;
- }
-
- @Override
- public double getRawHeadingMagnitude() {
- return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y));
- }
-
- @Override
- public boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
- }
-
- @Override
- public boolean getIsAlign() {
- return false;
- }
-
- public void startRumble() {
- controller.setRumble(GameController.RumbleStatus.RUMBLE_ON);
- }
-
- public void endRumble() {
- controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF);
- }
+ private final GameController controller = new GameController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = () -> false;
+ private boolean intakeBoolean = true;
+ private Command autoShoot = null;
+ private Command reverseMotors = null;
+
+ // PS5 button aliases
+ // private final Button CROSS = Button.A;
+ private final Button CIRCLE = Button.B;
+ private final Button SQUARE = Button.X;
+ // private final Button TRIANGLE = Button.Y;
+ // private final Button LB = Button.LB;
+ private final Button RB = Button.RB;
+ private final Button CREATE = Button.BACK;
+ // private final Button OPTIONS = Button.START;
+ private final Button LEFT_JOY = Button.LEFT_JOY;
+ private final Button RIGHT_JOY = Button.RIGHT_JOY;
+
+ // PS5 trigger buttons
+ private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON;
+ private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON;
+
+ // PS5 axis aliases
+ private final Axis LEFT_X = Axis.LEFT_X;
+ private final Axis LEFT_Y = Axis.LEFT_Y;
+ private final Axis RIGHT_X = Axis.RIGHT_X;
+ private final Axis RIGHT_Y = Axis.RIGHT_Y;
+ // private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
+ // private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
+
+ public PS5XboxModeDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
+
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller.get(RB).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+
+ // Align wheels
+ controller.get(DPad.RIGHT).onTrue(new FunctionalCommand(
+ () -> getDrivetrain().setStateDeadband(false),
+ getDrivetrain()::alignWheels,
+ interrupted -> getDrivetrain().setStateDeadband(true),
+ () -> false, getDrivetrain()).withTimeout(2));
+
+ // Rumble test
+ controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup(
+ new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)),
+ new WaitCommand(0.5),
+ new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF))));
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(LEFT_X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(LEFT_Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return controller.get(RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+
+ public void startRumble() {
+ controller.setRumble(GameController.RumbleStatus.RUMBLE_ON);
+ }
+
+ public void endRumble() {
+ controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF);
+ }
}
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.Constants;
import frc.robot.constants.IdConstants;
-import frc.robot.util.HubActive;
public class LED extends SubsystemBase {
private boolean underSecsToFlip(double secs) {
- Optional<Double> timeToActive = HubActive.timeToActive();
- Optional<Double> timeToInactive = HubActive.timeToInactive();
+ // Optional<Double> timeToActive = HubActive.timeToActive();
+ // Optional<Double> timeToInactive = HubActive.timeToInactive();
+ Optional<Double> timeToActive = Optional.empty();
+ Optional<Double> timeToInactive = Optional.empty();
if (timeToActive.isEmpty() && timeToInactive.isEmpty()) {
return false;
Arrays.stream(modules).forEach(Module::stop);
}
- // GETTERS AND SETTERS
-
- private boolean trenchAssist = false;
- private boolean trenchAlign = false;
-
- public boolean getTrenchAssist() {
- return trenchAssist;
- }
-
- public boolean getTrenchAlign() {
- return trenchAlign;
- }
-
- public void setTrenchAssist(boolean target) {
- trenchAssist = target;
- }
-
- public void setTrenchAlign(boolean target) {
- trenchAlign = target;
- }
-
// for current limit setting (brownout protection)
public void applyNewModuleCurrents(
double steerCurrentStator, double steerCurrentSupply,