mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-08 16:28:05 -06:00
Shikhar
688 lines
33 KiB
Java
688 lines
33 KiB
Java
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
|
/* the project. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
package frc4388.robot;
|
|
|
|
import java.io.File;
|
|
import java.util.Optional;
|
|
|
|
import com.pathplanner.lib.auto.NamedCommands;
|
|
import com.pathplanner.lib.commands.PathPlannerAuto;
|
|
|
|
import edu.wpi.first.math.geometry.Pose2d;
|
|
import edu.wpi.first.math.geometry.Rotation2d;
|
|
import edu.wpi.first.math.geometry.Translation2d;
|
|
import edu.wpi.first.wpilibj.DriverStation;
|
|
import edu.wpi.first.wpilibj.GenericHID;
|
|
import edu.wpi.first.wpilibj.RobotBase;
|
|
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
// Commands
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
|
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
|
import edu.wpi.first.wpilibj2.command.RunCommand;
|
|
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
|
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
|
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
|
|
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
|
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
|
import frc4388.robot.commands.waitSupplier;
|
|
import frc4388.robot.commands.Swerve.StayInPosition;
|
|
import frc4388.robot.commands.alignment.AutoAlign;
|
|
import frc4388.robot.constants.Constants;
|
|
import frc4388.robot.constants.FieldConstants;
|
|
import frc4388.robot.constants.Constants.OIConstants;
|
|
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
|
import frc4388.robot.subsystems.intake.Intake;
|
|
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
|
|
import frc4388.robot.subsystems.led.LED;
|
|
import frc4388.robot.subsystems.shooter.Shooter;
|
|
import frc4388.robot.subsystems.shooter.ShooterConstants;
|
|
import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
|
|
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
|
import frc4388.robot.subsystems.vision.Lidar;
|
|
import frc4388.robot.subsystems.vision.Vision;
|
|
import frc4388.utility.DeferredBlock;
|
|
import frc4388.utility.compute.FieldPositions;
|
|
import frc4388.utility.compute.TimesNegativeOne;
|
|
import frc4388.utility.controller.DeadbandedXboxController;
|
|
// Autos
|
|
import frc4388.utility.controller.VirtualController;
|
|
import frc4388.utility.controller.XboxController;
|
|
|
|
/**
|
|
* This class is where the bulk of the robot should be declared. Since
|
|
* Command-based is a "declarative" paradigm, very little robot logic should
|
|
* actually be handled in the {@link Robot} periodic methods (other than the
|
|
* scheduler calls). Instead, the structure of the robot (2including subsystems,
|
|
* commands, and button mappings) should be declared here.
|
|
*/
|
|
public class RobotContainer {
|
|
/* RobotMap */
|
|
|
|
public final RobotMap m_robotMap = new RobotMap(RobotBase.isReal() ? Mode.REAL : Mode.SIM);
|
|
|
|
/*Limit Switch */
|
|
// public final DigitalInput m_armLimitSwitch = new DigitalInput(9);
|
|
|
|
/* Subsystems */
|
|
// public final Lidar m_lidar = new Lidar();
|
|
public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID);
|
|
public final SimpleSwerveSim m_robotSwerveSIM = new SimpleSwerveSim();
|
|
//Testing of Colors
|
|
public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
|
|
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
|
public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO, m_robotSwerveDrive);
|
|
public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
|
|
|
|
|
|
/* Controllers */
|
|
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
|
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
|
|
|
|
|
// private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
|
|
|
|
// public List<Subsystem> subsystems = new ArrayList<>();
|
|
private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive);
|
|
|
|
private Pose2d currentPose = new Pose2d(0, 0, new Rotation2d());
|
|
// ! Teleop Commands
|
|
public void stop() {
|
|
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
|
m_robotSwerveDrive.stopModules();
|
|
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
|
|
}
|
|
|
|
// ! /* Autos */
|
|
private SendableChooser<String> autoChooser;
|
|
private Command autoCommand;
|
|
|
|
|
|
private Command IntakeExtended = new SequentialCommandGroup(
|
|
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendingRolling), m_robotIntake)
|
|
);
|
|
|
|
private Command LabubuGrowl = new SequentialCommandGroup(
|
|
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.LabubuGrowl), m_robotIntake)
|
|
);
|
|
|
|
// private Command LidarIntake = new SequentialCommandGroup(
|
|
// // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
|
|
// // RobotIntakeDown,
|
|
// // new WaitCommand(1),
|
|
// // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
|
|
// new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
|
|
|
|
// // new RunCommand(
|
|
// // () -> m_robotSwerveDrive.driveWithInput(
|
|
// // m_lidar.getClosestBall(),
|
|
// // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
|
|
// // false
|
|
// // ),
|
|
// // m_robotSwerveDrive
|
|
// // )
|
|
// // .withTimeout(5.0)
|
|
// // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
|
|
// );
|
|
|
|
private Command RobotRev = new SequentialCommandGroup(
|
|
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
|
|
IntakeExtended,
|
|
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExpelBalls), m_robotIntake)
|
|
);
|
|
|
|
private Command WaitIntakeReference =
|
|
new WaitUntilCommand(m_robotIntake::intakeAtReference);
|
|
|
|
private Command RobotShootDriving = new SequentialCommandGroup(
|
|
new RunCommand(() ->
|
|
m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION, ShooterConstants.AIM_LEAD_TIME.get(), FieldPositions.HUB_POSITION)
|
|
).withTimeout(20)
|
|
).finallyDo((interrupted) ->
|
|
m_robotSwerveDrive.disableRotationOverride()
|
|
);
|
|
|
|
private Command IntakeRetracted = new SequentialCommandGroup(
|
|
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RectractTorque), m_robotIntake)
|
|
);
|
|
|
|
private Command RobotHalfShoot = new SequentialCommandGroup(
|
|
new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
|
|
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake),
|
|
new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
|
|
new WaitCommand(2.0),
|
|
new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
|
|
new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter)
|
|
);
|
|
|
|
private Command RobotFullShoot = new SequentialCommandGroup(
|
|
// TEST NEW AUTO ALIGN
|
|
//new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false),
|
|
new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
|
|
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake),
|
|
new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
|
|
new WaitCommand(2.5),
|
|
// new WaitCommand(2.25),
|
|
IntakeRetracted,
|
|
new WaitCommand(5),
|
|
// new WaitCommand(4.25),
|
|
new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
|
|
new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter)
|
|
);
|
|
|
|
|
|
public RobotContainer() {
|
|
|
|
configureButtonBindings();
|
|
|
|
// Called on first robot enable
|
|
DeferredBlock.addBlock(() -> {
|
|
m_robotSwerveDrive.resetGyro();
|
|
}, false);
|
|
|
|
// Called on every robot enable
|
|
DeferredBlock.addBlock(() -> {
|
|
// m_robotIntake.setMode(IntakeMode.Idle);
|
|
// m_robotShooter.spinUpIdle();
|
|
m_robotIntake.io.updateGains();
|
|
TimesNegativeOne.update();
|
|
FieldPositions.update();
|
|
m_robotShooter.io.updateGains();
|
|
}, true);
|
|
|
|
NamedCommands.registerCommand("Robot Rev Up", RobotRev);
|
|
NamedCommands.registerCommand("Intake Retracted", IntakeRetracted);
|
|
NamedCommands.registerCommand("Robot Shoot", RobotFullShoot);
|
|
NamedCommands.registerCommand("Robot Half Shot", RobotHalfShoot);
|
|
// NamedCommands.registerCommand("Lidar Intake", LidarIntake);
|
|
NamedCommands.registerCommand("Intake Extended", IntakeExtended);
|
|
NamedCommands.registerCommand("Labubu Growl", LabubuGrowl);
|
|
NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving);
|
|
NamedCommands.registerCommand("Intake Reference", WaitIntakeReference);
|
|
NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed));
|
|
NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter));
|
|
NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter));
|
|
NamedCommands.registerCommand("SpinUpShooting", new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter));
|
|
NamedCommands.registerCommand("SpinUpIdle", new InstantCommand(() -> m_robotShooter.spinUpIdle(), m_robotShooter));
|
|
|
|
NamedCommands.registerCommand("BumpOffsetForward", new InstantCommand(() -> {
|
|
if (TimesNegativeOne.isRed) {
|
|
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED_FRONT);
|
|
} else {
|
|
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE_FRONT);
|
|
}
|
|
}));
|
|
|
|
NamedCommands.registerCommand("BumpOffsetBackward", new InstantCommand(() -> {
|
|
if (TimesNegativeOne.isRed) {
|
|
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED_BACK);
|
|
} else {
|
|
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE_BACK);
|
|
}
|
|
}));
|
|
|
|
|
|
DriverStation.silenceJoystickConnectionWarning(true);
|
|
|
|
// Drive normally
|
|
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
|
m_robotSwerveDrive.driveWithInput(
|
|
getDeadbandedDriverController().getLeft(),
|
|
getDeadbandedDriverController().getRight(),true);
|
|
|
|
}, m_robotSwerveDrive)
|
|
.withName("SwerveDrive DefaultCommand"));
|
|
|
|
m_robotSwerveDrive.setToSlow();
|
|
|
|
makeAutoChooser();
|
|
SmartDashboard.putData("Auto Chooser", autoChooser);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
* Use this method to define your button->command mappings. Buttons can be
|
|
* created by instantiating a {@link GenericHID} or one of its subclasses
|
|
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
|
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
|
*/
|
|
private void configureButtonBindings() {
|
|
|
|
//Driver controls
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
|
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
|
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
|
|
|
|
|
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotIntake.io.updateGains();
|
|
m_robotShooter.io.updateGains();
|
|
}));
|
|
|
|
|
|
// TEST-> the driver is holding the left trigger, drive slow and rotation up
|
|
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotSwerveDrive.setPercentOutput(0.10);
|
|
m_robotSwerveDrive.shiftUpRot();
|
|
}))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotSwerveDrive.setToFast();
|
|
// m_robotSwerveDrive.shiftDownRot();
|
|
}));
|
|
|
|
//TEST - > X positino on wheels
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
|
.whileTrue(new RunCommand(() -> {
|
|
m_robotSwerveDrive.defenseXPosition();
|
|
}, m_robotSwerveDrive))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotSwerveDrive.stopDefenseXPosition();
|
|
}));
|
|
|
|
//TEST - > PID positinon
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
|
.onTrue(new InstantCommand(() -> {
|
|
currentPose = m_robotSwerveDrive.getCurrentPose();
|
|
}))
|
|
.whileTrue(new RunCommand(() -> {
|
|
m_stayInPosition.goToTargetPose(currentPose);
|
|
}, m_robotSwerveDrive))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotSwerveDrive.softStop();
|
|
|
|
}));
|
|
|
|
|
|
|
|
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
|
|
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotSwerveDrive.setToSlow();
|
|
}))
|
|
.whileTrue(new RunCommand(() -> {
|
|
m_robotSwerveDrive.driveFacingPosition(
|
|
getDeadbandedDriverController().getLeft(),
|
|
FieldPositions.HUB_POSITION,
|
|
ShooterConstants.AIM_LEAD_TIME.get()
|
|
);
|
|
}, m_robotSwerveDrive))
|
|
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
|
|
|
|
// D-PAD fine alignment
|
|
new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
|
|
.whileTrue(new RunCommand(
|
|
() -> m_robotSwerveDrive.driveFine(
|
|
new Translation2d(
|
|
1,
|
|
Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
|
|
),
|
|
getDeadbandedDriverController().getRight(), 0.15
|
|
), m_robotSwerveDrive))
|
|
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
|
|
|
|
//Operator Controls
|
|
// new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5)
|
|
// .onTrue(new InstantCommand(() -> {
|
|
// m_robotIntake.setMode(IntakeMode.Extended);
|
|
// }));
|
|
|
|
|
|
//allow shooting with right trigger
|
|
new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotShooter.allowShooting();
|
|
})).onFalse(new InstantCommand(() -> {
|
|
m_robotShooter.denyShooting();
|
|
}));
|
|
|
|
|
|
// manually shoot from climb post/ feed balls
|
|
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotShooter.spinUpFeeding();
|
|
m_robotIntake.rollerStop();
|
|
}))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotShooter.spinUpIdle();
|
|
}));
|
|
|
|
|
|
//set shooter ready (rev) with left trigger hold
|
|
new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.Idle);
|
|
m_robotShooter.spinUpShooting();
|
|
}))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotShooter.spinUpIdle();
|
|
}));
|
|
|
|
// new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
|
|
// .onTrue(new InstantCommand(() -> {
|
|
// m_robotIntake.setMode(IntakeMode.RetractedREMOVEME);
|
|
// }));
|
|
|
|
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
|
|
// .onTrue(new InstantCommand(() -> {
|
|
// m_robotIntake.setMode(IntakeMode.ExtendedREMOVEME);
|
|
// }));
|
|
|
|
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.ExtendingRolling);
|
|
}));
|
|
|
|
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.LabubuGrowl);
|
|
}))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.ExtendingRolling);
|
|
}));
|
|
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.Retracting);
|
|
}))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.ArmIdleRollingNot);
|
|
}));
|
|
|
|
|
|
|
|
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.ExtendingIdle);
|
|
}))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.Idle);
|
|
}));
|
|
|
|
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.Retracting);
|
|
}))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.Idle);
|
|
}));
|
|
|
|
|
|
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.RectractTorque);
|
|
}))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.Idle);
|
|
}));
|
|
|
|
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.ExpelBalls);
|
|
}))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.Idle);
|
|
}));
|
|
|
|
|
|
// .onFalse(new InstantCommand(() -> {
|
|
// m_robotIntake.setMode(IntakeMode.Idle);
|
|
// }));
|
|
|
|
// new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
|
|
// .whileTrue(
|
|
// new PathPlannerAuto("Right_AutoClimb")
|
|
// )
|
|
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
|
|
|
|
// new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
|
|
// .whileTrue(
|
|
// new PathPlannerAuto("Left_AutoClimb")
|
|
// )
|
|
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
|
|
|
|
// new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
|
|
// .onTrue(new InstantCommand(() -> {
|
|
// m_robotClimber.toggleDeployed();
|
|
// }));
|
|
|
|
|
|
}
|
|
|
|
|
|
private void configureSINGLEBindings() {
|
|
|
|
//Driver controls
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
|
|
|
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
|
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED)));
|
|
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
|
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
|
|
|
|
|
|
|
|
|
// TEST-> the driver is holding the left trigger, drive slow and rotation up
|
|
// new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
|
// .onTrue(new InstantCommand(() -> {
|
|
// m_robotSwerveDrive.setToSlow();
|
|
// m_robotSwerveDrive.shiftUpRot();
|
|
// }))
|
|
// .onFalse(new InstantCommand(() -> {
|
|
// m_robotSwerveDrive.setToFast();
|
|
// m_robotSwerveDrive.shiftDownRot();
|
|
// }));
|
|
|
|
//TEST - > X positino on wheels
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
|
.whileTrue(new RunCommand(() -> {
|
|
m_robotSwerveDrive.defenseXPosition();
|
|
}, m_robotSwerveDrive))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotSwerveDrive.stopDefenseXPosition();
|
|
}));
|
|
|
|
//TEST - > PID positinon
|
|
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
|
.onTrue(new InstantCommand(() -> {
|
|
currentPose = m_robotSwerveDrive.getCurrentPose();
|
|
}))
|
|
.whileTrue(new RunCommand(() -> {
|
|
m_stayInPosition.goToTargetPose(currentPose);
|
|
}, m_robotSwerveDrive))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotSwerveDrive.softStop();
|
|
|
|
}));
|
|
|
|
|
|
|
|
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
|
|
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotSwerveDrive.setToSlow();
|
|
}))
|
|
.whileTrue(new RunCommand(() -> {
|
|
m_robotSwerveDrive.driveFacingPosition(
|
|
getDeadbandedDriverController().getLeft(),
|
|
FieldPositions.HUB_POSITION,
|
|
ShooterConstants.AIM_LEAD_TIME.get()
|
|
);
|
|
}, m_robotSwerveDrive)
|
|
);
|
|
|
|
// D-PAD fine alignment
|
|
new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
|
|
.whileTrue(new RunCommand(
|
|
() -> m_robotSwerveDrive.driveFine(
|
|
new Translation2d(
|
|
1,
|
|
Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
|
|
),
|
|
getDeadbandedDriverController().getRight(), 0.15
|
|
), m_robotSwerveDrive))
|
|
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
|
|
|
|
|
|
//allow shooting with right trigger
|
|
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotShooter.allowShooting();
|
|
})).onFalse(new InstantCommand(() -> {
|
|
m_robotShooter.denyShooting();
|
|
}));
|
|
|
|
|
|
|
|
//set shooter ready (rev) with left trigger hold
|
|
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
|
.onTrue(new InstantCommand(() -> {
|
|
m_robotIntake.setMode(IntakeMode.Idle);
|
|
m_robotIntake.rollerStop();
|
|
m_robotShooter.spinUpShooting();
|
|
}))
|
|
.onFalse(new InstantCommand(() -> {
|
|
m_robotShooter.spinUpIdle();
|
|
}));
|
|
|
|
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
|
// .onTrue(new InstantCommand(() -> {
|
|
// m_robotIntake.setMode(IntakeMode.ExtendedIdle);
|
|
// }))
|
|
// .onFalse(new InstantCommand(() -> {
|
|
// m_robotIntake.setMode(IntakeMode.Idle);
|
|
// }));
|
|
|
|
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
|
// .onTrue(new InstantCommand(() -> {
|
|
// m_robotIntake.setMode(IntakeMode.Retracting);
|
|
// }))
|
|
// .onFalse(new InstantCommand(() -> {
|
|
// m_robotIntake.setMode(IntakeMode.Idle);
|
|
// }));
|
|
|
|
}
|
|
|
|
//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT)));
|
|
|
|
/**
|
|
* Use this to pass the autonomous command to the main {@link Robot} class.
|
|
*
|
|
* @return the command to run in autonomous
|
|
*/
|
|
public Command getAutonomousCommand() {
|
|
|
|
|
|
//return autoPlayback;
|
|
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
|
|
//return autoChooser.getSelected();
|
|
// try{
|
|
// // // Load the path you want to follow using its name in the GUI
|
|
// return autoCommand;
|
|
// } catch (Exception e) {
|
|
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
|
|
return autoCommand;
|
|
// }
|
|
// return new PathPlannerAuto("Line-up-no-arm");
|
|
// zach told me to do the below comment
|
|
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
|
|
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
|
|
}
|
|
|
|
public boolean autoChooserUpdated = false;
|
|
public void makeAutoChooser() {
|
|
autoChooser = new SendableChooser<String>();
|
|
autoChooser.setDefaultOption("None", "None");
|
|
File dir;
|
|
|
|
if(RobotBase.isReal()) {
|
|
dir = new File("/home/lvuser/deploy/pathplanner/autos/");
|
|
} else {
|
|
// dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
|
|
dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2026KPopRobotHunters\\src\\main\\deploy\\pathplanner\\autos\\");
|
|
}
|
|
|
|
String[] autos = dir.list();
|
|
|
|
if(autos == null) return;
|
|
|
|
for (String auto : autos) {
|
|
if (auto.endsWith(".auto"))
|
|
autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
|
|
// System.out.println(auto);
|
|
}
|
|
|
|
autoChooser.onChange((filename) -> {
|
|
autoChooserUpdated = true;
|
|
if (filename == null || filename.equals("None")) {
|
|
autoCommand = null;
|
|
return;
|
|
}
|
|
// if (filename.equals("Taxi%")) {
|
|
// autoCommand = new SequentialCommandGroup(
|
|
// new MoveForTimeCommand(m_robotSwerveDrive,
|
|
// new Translation2d(0, -1),
|
|
// new Translation2d(), 1000, true
|
|
// ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
|
|
// } else {
|
|
autoCommand = new PathPlannerAuto(filename);
|
|
// }
|
|
System.out.println("Robot Auto Changed " + filename);
|
|
|
|
//----
|
|
PathPlannerAuto auto = new PathPlannerAuto(filename);
|
|
m_robotSwerveDrive.setInitalPose(auto.getStartingPose());
|
|
//-----
|
|
});
|
|
SmartDashboard.putData(autoChooser);
|
|
|
|
}
|
|
|
|
/**
|
|
* A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller}
|
|
* @param joystickA A controller
|
|
* @param joystickB A controller
|
|
* @param buttonNumber The button to bind to
|
|
*/
|
|
public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) {
|
|
return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
|
|
}
|
|
|
|
public DeadbandedXboxController getDeadbandedDriverController() {
|
|
return this.m_driverXbox;
|
|
}
|
|
|
|
public DeadbandedXboxController getDeadbandedOperatorController() {
|
|
return this.m_operatorXbox;
|
|
}
|
|
|
|
// public ButtonBox getButtonBox() {
|
|
// return this.m_buttonBox;
|
|
// }
|
|
} |