mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
aarav18
641 lines
40 KiB
Java
641 lines
40 KiB
Java
// 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.nio.file.Files;
|
|
import java.nio.file.StandardOpenOption;
|
|
import java.util.ArrayList;
|
|
import java.util.Arrays;
|
|
import java.util.Objects;
|
|
import java.util.logging.Logger;
|
|
|
|
import com.diffplug.common.base.Errors;
|
|
import com.pathplanner.lib.PathPlanner;
|
|
import com.pathplanner.lib.PathPlannerTrajectory;
|
|
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
|
|
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
|
|
|
|
import edu.wpi.first.math.controller.PIDController;
|
|
import edu.wpi.first.math.controller.ProfiledPIDController;
|
|
import edu.wpi.first.math.geometry.Pose2d;
|
|
import edu.wpi.first.math.geometry.Rotation2d;
|
|
import edu.wpi.first.util.sendable.Sendable;
|
|
import edu.wpi.first.util.sendable.SendableBuilder;
|
|
import edu.wpi.first.wpilibj.Filesystem;
|
|
import edu.wpi.first.wpilibj.GenericHID;
|
|
import edu.wpi.first.wpilibj.XboxController;
|
|
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
|
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
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.button.JoystickButton;
|
|
import frc4388.robot.Constants.OIConstants;
|
|
import frc4388.robot.Constants.StorageConstants;
|
|
import frc4388.robot.Constants.SwerveDriveConstants;
|
|
import frc4388.robot.commands.PathRecorder;
|
|
import frc4388.robot.commands.RunCommandForTime;
|
|
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
|
|
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
|
|
import frc4388.robot.commands.ShooterCommands.TrackTarget;
|
|
import frc4388.robot.subsystems.BoomBoom;
|
|
import frc4388.robot.subsystems.Claws;
|
|
import frc4388.robot.subsystems.Climber;
|
|
import frc4388.robot.subsystems.Extender;
|
|
import frc4388.robot.subsystems.Hood;
|
|
import frc4388.robot.subsystems.Intake;
|
|
import frc4388.robot.subsystems.Serializer;
|
|
import frc4388.robot.subsystems.Storage;
|
|
import frc4388.robot.subsystems.SwerveDrive;
|
|
import frc4388.robot.subsystems.Turret;
|
|
import frc4388.robot.subsystems.VisionOdometry;
|
|
import frc4388.utility.Vector2D;
|
|
import frc4388.utility.controller.ButtonBox;
|
|
import frc4388.utility.controller.DeadbandedXboxController;
|
|
|
|
/**
|
|
* 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 (including subsystems,
|
|
* commands, and button mappings) should be declared here.
|
|
*/
|
|
public class RobotContainer {
|
|
private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
|
|
|
|
// RobotMap
|
|
public final RobotMap m_robotMap = new RobotMap();
|
|
|
|
/* Subsystems */
|
|
public final Climber m_robotClimber = new Climber(m_robotMap.elbow);
|
|
public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
|
|
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
|
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
|
|
public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor);
|
|
public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor);
|
|
|
|
public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor);
|
|
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
|
public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
|
|
public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
|
|
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
|
|
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
|
|
|
|
/* Autonomous */
|
|
private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive);
|
|
// Controllers
|
|
private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
|
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
|
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
|
|
|
|
public static boolean softLimits = true;
|
|
|
|
// control mode switching
|
|
public static enum ControlMode { SHOOTER, CLIMBER };
|
|
public static ControlMode currentControlMode = ControlMode.SHOOTER;
|
|
|
|
// turret mode switching
|
|
private enum TurretMode { MANUAL, AUTONOMOUS };
|
|
private TurretMode currentTurretMode = TurretMode.MANUAL;
|
|
|
|
// climber mode switching
|
|
private enum ClimberMode { MANUAL, AUTONOMOUS };
|
|
private ClimberMode currentClimberMode = ClimberMode.MANUAL;
|
|
|
|
private SendableChooser<SequentialCommandGroup> quickAutoChooser = new SendableChooser<>();
|
|
|
|
/**
|
|
* SmartDash
|
|
* - Limelight cam X
|
|
* - Limit switches X
|
|
* - Shooter RPM X
|
|
* - Distance to target x
|
|
* - target locked
|
|
* - claws boolean
|
|
* - field
|
|
*/
|
|
|
|
// private SequentialCommandGroup makeTheWeirdGroup() {
|
|
// SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
|
// new ParallelCommandGroup(
|
|
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
|
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0)
|
|
// )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
|
// return weirdAutoShootingGroup;
|
|
// }
|
|
/**
|
|
* The container for the robot. Contains subsystems, OI devices, and commands.
|
|
*/
|
|
public RobotContainer() {
|
|
|
|
// double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
|
|
|
|
// double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
|
|
// double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
|
|
|
|
// Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
|
|
// Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
|
|
// Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
|
|
|
|
// var justShoot = new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
|
|
// makeTheWeirdGroup(), // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // * stop running storage
|
|
// // ! SHOOT FIRST BALL, THEN DRIVE OFF LINE (HOPEFULLY)
|
|
// var offTheLine = new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
|
|
// makeTheWeirdGroup(), // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
|
|
|
// new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, 0.25))); // * drive off line
|
|
|
|
// // ! TWO BALL AUTO (HOPEFULLY)
|
|
// var twoBall = new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
|
|
// makeTheWeirdGroup(), // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
|
|
|
// new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake
|
|
|
|
// new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path
|
|
|
|
// new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball
|
|
// // new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 376),
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
|
|
|
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
|
|
// makeTheWeirdGroup(), // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)))); // * stop running storage
|
|
|
|
// // ! THREE BALL AUTO (HOPEFULLY)
|
|
// var threeBall = new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
|
|
// makeTheWeirdGroup(), // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
|
|
|
// new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake
|
|
|
|
// new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path
|
|
|
|
// new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball
|
|
// //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 363)
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
|
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
|
|
// makeTheWeirdGroup(), // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
|
|
|
// //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y}, 0.5d),
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-firstToSecond.unit().x, -firstToSecond.unit().y, 0.0, 0.0}, (firstToSecond.magnitude() - offset) / distancePerSecond), // * drive to second ball
|
|
// //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363)
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
|
|
|
// new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, secondBallPosition.toDoubleArray()), // * aim to target
|
|
// makeTheWeirdGroup(), // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
|
|
|
|
// )));
|
|
|
|
|
|
// autoChoices = Map.of(
|
|
// "justShoot", justShoot,
|
|
// "offTheLine", offTheLine,
|
|
// "twoBall", twoBall,
|
|
// "threeBall", threeBall
|
|
// );
|
|
|
|
// SmartDashboard.putData(quickAutoChooser);
|
|
|
|
|
|
|
|
|
|
configureButtonBindings();
|
|
/* Default Commands */
|
|
// Swerve Drive with Input
|
|
m_robotSwerveDrive.setDefaultCommand(
|
|
new RunCommand(() -> {
|
|
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) {
|
|
m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(),
|
|
getDriverController().getLeftY(),
|
|
getDriverController().getRightX(),
|
|
getDriverController().getRightY(),
|
|
true); }
|
|
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) {
|
|
m_robotSwerveDrive.driveWithInput( 0,
|
|
0,
|
|
0,
|
|
0,
|
|
false);
|
|
}}
|
|
, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
|
|
|
|
// Intake with Triggers
|
|
m_robotIntake.setDefaultCommand(
|
|
new RunCommand(() -> m_robotIntake.runWithTriggers(
|
|
getOperatorController().getLeftTriggerAxis(),
|
|
getOperatorController().getRightTriggerAxis()),
|
|
m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
|
|
m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom));
|
|
|
|
// Serializer Manual
|
|
m_robotSerializer.setDefaultCommand(
|
|
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
|
|
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
|
|
|
|
// Turret Manual
|
|
m_robotTurret.setDefaultCommand(
|
|
new RunCommand(() -> {
|
|
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
|
|
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
|
|
}, m_robotTurret));
|
|
|
|
// Hood Manual
|
|
m_robotHood.setDefaultCommand(
|
|
new RunCommand(() -> {
|
|
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); }
|
|
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
|
|
}, m_robotHood));
|
|
|
|
// //Climber Manual
|
|
m_robotClimber.setDefaultCommand(
|
|
new RunCommand(() -> {
|
|
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
|
|
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getRightY()); }
|
|
}, m_robotClimber));
|
|
// m_robotClimber.setDefaultCommand(
|
|
// new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightY()), m_robotClimber));
|
|
|
|
m_robotBoomBoom.setDefaultCommand(
|
|
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
|
|
);
|
|
|
|
|
|
if (m_robotBoomBoom.readManualData) {
|
|
var tab = Shuffleboard.getTab("Manual Shooter Data");
|
|
var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withSize(2, 3);
|
|
manual.add("Manual Shooter Data", new Sendable() {
|
|
private double manualHoodExt;
|
|
private double manualDrumVel;
|
|
|
|
@Override
|
|
public void initSendable(SendableBuilder builder) {
|
|
builder.setSmartDashboardType("RobotPreferences");
|
|
builder.addBooleanProperty("Disable CSV", () -> m_robotBoomBoom.readManualData, b -> m_robotBoomBoom.readManualData = b);
|
|
builder.addDoubleProperty("Drum Velocity", () -> manualDrumVel, d -> manualDrumVel = d);
|
|
builder.addDoubleProperty("Hood Extension", () -> manualHoodExt, d -> manualHoodExt = d);
|
|
builder.addDoubleProperty("Measured Distance", () -> SmartDashboard.getNumber("Distance to Target", -1), System.out::println);
|
|
}
|
|
});
|
|
manual.add("Shooter Table Appender", new InstantCommand(() -> Errors.log().run(() -> Files.write(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath(), String.format("%s,%s,%s%n", SmartDashboard.getNumber("Manual Shooter Data/Distance to Target", -1), SmartDashboard.getNumber("Manual Shooter Data/Hood Extension", -1), SmartDashboard.getNumber("Manual Shooter Data/Drum Velocity", -1)).getBytes(), StandardOpenOption.WRITE, StandardOpenOption.APPEND))) {
|
|
@Override
|
|
public boolean runsWhenDisabled() {
|
|
return true;
|
|
}
|
|
}.withName("Append"));
|
|
var csv = tab.getLayout("Shooter Data", BuiltInLayouts.kList).withPosition(2,0).withSize(4, 3);
|
|
csv.add("Initial Shooter Data", builder -> Arrays.stream(m_robotBoomBoom.m_shooterTable).forEach(e -> builder.addDoubleArrayProperty(Double.toString(e.distance), () -> new double[] {e.hoodExt, e.drumVelocity}, a -> {})));
|
|
csv.add("Reload Data", new InstantCommand(m_robotBoomBoom::updateShooterTable) {
|
|
@Override
|
|
public boolean runsWhenDisabled() {
|
|
return true;
|
|
}
|
|
}.withName("Reload"));
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
* 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 Buttons
|
|
|
|
// Start > Calibrate Odometry
|
|
new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
|
|
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
|
|
|
// Start > Calibrate Odometry
|
|
new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
|
|
.whenPressed(m_robotSwerveDrive::resetGyro);
|
|
|
|
// Left Bumper > Shift Down
|
|
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
|
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
|
|
|
// Right Bumper > Shift Up
|
|
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
|
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
|
|
|
|
|
|
|
//! Operator Buttons
|
|
|
|
// Right Bumper > Storage Out
|
|
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
|
|
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
|
|
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
|
|
|
// Left Bumper > Storage In
|
|
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
|
|
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
|
|
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
|
|
|
// B > Toggle claws
|
|
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
|
.whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
|
|
|
|
// X > Toggles extender in and out
|
|
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
|
|
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
|
|
|
// A > Spit Out Ball
|
|
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
|
.whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret))
|
|
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
|
|
|
|
// Y > Full aim command
|
|
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
|
// .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
|
|
|
|
|
|
//! Test Buttons
|
|
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
|
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
|
|
|
|
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
|
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
|
|
|
|
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
|
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
|
|
|
|
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
|
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
|
|
|
|
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
|
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
|
|
|
|
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
|
// .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
|
|
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
|
|
|
|
|
|
|
|
//! Button Box Buttons
|
|
// Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender)
|
|
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
|
|
.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
|
|
.whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
|
|
|
|
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
|
|
.whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
|
|
|
|
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
|
|
|
|
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
|
|
.whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
|
|
|
|
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
|
|
.whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
|
|
|
|
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
|
|
|
|
.whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
|
|
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
|
|
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
|
|
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
|
|
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
|
|
|
|
// Middle Switch > Climber and Shooter mode switching
|
|
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
|
|
.whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
|
|
.whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
|
|
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
|
.whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
|
|
.whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
|
|
|
|
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
|
.whileHeld(new InstantCommand(() -> m_robotExtender.invertExtender(-1.0)))
|
|
.whenReleased(new InstantCommand(() -> m_robotExtender.invertExtender(1.0)));
|
|
|
|
// Left Button > Extender In
|
|
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
|
|
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
|
|
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
|
|
|
// Left Button > Extender Out
|
|
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value)
|
|
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
|
|
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
|
}
|
|
|
|
/**
|
|
* Generate autonomous
|
|
* @param maxVel max velocity for the path (null to override default value of 5.0)
|
|
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
|
|
* @param inputs strings (paths) or commands you want to run (in order)
|
|
* @return array of commands
|
|
*/
|
|
public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
|
|
maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY);
|
|
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
|
|
|
|
ArrayList<Command> commands = new ArrayList<Command>();
|
|
|
|
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
|
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
|
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
|
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
|
|
|
// parse input
|
|
for (int i=0; i<inputs.length; i++) {
|
|
if (inputs[i] instanceof String) {
|
|
|
|
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
|
|
PathPlannerState initState = traj.getInitialState();
|
|
|
|
Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation);
|
|
|
|
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive));
|
|
commands.add(new PPSwerveControllerCommand(
|
|
traj,
|
|
m_robotSwerveDrive::getOdometry,
|
|
m_robotSwerveDrive.m_kinematics,
|
|
xController,
|
|
yController,
|
|
thetaController,
|
|
m_robotSwerveDrive::setModuleStates,
|
|
m_robotSwerveDrive));
|
|
}
|
|
|
|
if (inputs[i] instanceof Command) {
|
|
commands.add((Command) inputs[i]);
|
|
}
|
|
}
|
|
|
|
commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive));
|
|
Command[] ret = new Command[commands.size()];
|
|
ret = commands.toArray(ret);
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* Use this to pass the autonomous command to the main {@link Robot} class.
|
|
*
|
|
* @return the command to run in autonomous
|
|
*/
|
|
public Command getAutonomousCommand() {
|
|
|
|
// ! ways to not coast
|
|
// // * 1. try zero joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, 0.0, false), m_robotSwerveDrive);
|
|
// * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive);
|
|
// * 3a. try permanently setting drive motors to brake, not coast, in RobotMap.java, and ask the driver how it feels.
|
|
// * 3b. try to only set the drive motors to brake if in auto mode.
|
|
// * 4. try new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
|
|
|
|
// ? 1.0 input, 1 second: 134 inches
|
|
// ? 0.75 input, 1 second: 48 inches
|
|
// ! POSITIVE Y IS LEFT, POSITIVE X IS BACKWARDS
|
|
|
|
double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
|
|
|
|
double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
|
|
double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
|
|
|
|
// ! ball positions are "unit tested"
|
|
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
|
|
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
|
|
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
|
|
|
|
SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
|
new ParallelCommandGroup(
|
|
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
|
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0)
|
|
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
|
|
|
// ! DRIVE BACKWARDS AND SHOOT
|
|
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
|
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (3.0 * 12) / SwerveDriveConstants.AUTO_INCHES_PER_SECOND_AT_FULL_POWER),//0.269), // * go backwards three feet
|
|
new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
|
|
|
|
weirdAutoShootingGroup, // * shoot
|
|
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
|
|
);
|
|
|
|
// ! DRIVE OFF LINE, THEN SHOOT BALL (HOPEFULLY)
|
|
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-0.5, 0, 0.0, 0.0}, 1.0), // * drive out of tarmac
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0, 0.0, 0.0}, 1.0), // * drive out of tarmac
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.5, 0.0, 0.0}, 1.0), // * drive out of tarmac
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, -0.5, 0.0, 0.0}, 1.0), // * drive out of tarmac
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive)); // * brake
|
|
|
|
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
|
|
// weirdAutoShootingGroup, // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
|
|
// );
|
|
|
|
// ! TWO BALL AUTO (HOPEFULLY)
|
|
// return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake
|
|
|
|
// new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path
|
|
|
|
// new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-1.0, 0.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball
|
|
// // new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 376),
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
|
|
|
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
|
|
// weirdAutoShootingGroup, // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)))); // * stop running storage
|
|
|
|
// ! THREE BALL AUTO (HOPEFULLY)
|
|
// return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
|
|
// weirdAutoShootingGroup, // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
|
|
|
// new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake
|
|
|
|
// new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path
|
|
|
|
// new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball
|
|
// //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 363)
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
|
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
|
|
// weirdAutoShootingGroup, // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
|
|
|
// //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y}, 0.5d),
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-firstToSecond.unit().x, -firstToSecond.unit().y, 0.0, 0.0}, (firstToSecond.magnitude() - offset) / distancePerSecond), // * drive to second ball
|
|
// //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363)
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
|
|
|
// new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, secondBallPosition.toDoubleArray()), // * aim to target
|
|
// weirdAutoShootingGroup, // * shoot
|
|
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
|
|
|
|
// )));
|
|
|
|
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
|
|
|
|
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, 1.0),
|
|
// new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive),
|
|
|
|
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
|
|
|
// new ParallelCommandGroup(
|
|
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
|
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0))
|
|
|
|
// );
|
|
|
|
}
|
|
|
|
public static XboxController getDriverController() {
|
|
return m_driverXbox;
|
|
}
|
|
|
|
public XboxController getOperatorController() {
|
|
return m_operatorXbox;
|
|
}
|
|
|
|
public ButtonBox getButtonBox() {
|
|
return m_buttonBox;
|
|
}
|
|
|
|
public static void setSoftLimits(boolean set) {
|
|
softLimits = set;
|
|
}
|
|
|
|
/**
|
|
* Get odometry.
|
|
*
|
|
* @return Odometry
|
|
*/
|
|
public Pose2d getOdometry() {
|
|
return m_robotSwerveDrive.getOdometry();
|
|
}
|
|
|
|
/**
|
|
* Set odometry to given pose.
|
|
*
|
|
* @param pose Pose to set odometry to.
|
|
*/
|
|
public void resetOdometry(Pose2d pose) {
|
|
m_robotSwerveDrive.resetOdometry(pose);
|
|
}
|
|
}
|