diff --git a/build.gradle b/build.gradle index b4f40fb..e5b44d6 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-3" + id 'com.diffplug.spotless' version '6.6.1' } sourceCompatibility = JavaVersion.VERSION_11 @@ -87,3 +88,19 @@ wpi.java.configureExecutableTasks(jar) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } + +spotless { + enforceCheck false + + java { + target fileTree('.') { + include '**/*.java' + exclude '**/build/**', '**/build-*/**' + } + toggleOffOn() + palantirJavaFormat() + removeUnusedImports() + trimTrailingWhitespace() + endWithNewline() + } +} \ No newline at end of file diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/lib/controller/CustomXboxController.java b/src/main/java/frc/lib/controller/CustomXboxController.java index 71a3189..f41d4de 100644 --- a/src/main/java/frc/lib/controller/CustomXboxController.java +++ b/src/main/java/frc/lib/controller/CustomXboxController.java @@ -2,8 +2,9 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; + import java.util.HashMap; public class CustomXboxController { @@ -52,6 +53,8 @@ public CustomXboxController(int port) { rightTrigger = new JoystickAxis(joystick, 3); rightXAxis = new JoystickAxis(joystick, 4); rightYAxis = new JoystickAxis(joystick, 5); + + buttonPurposeHashMap.put("type", "CustomXboxController"); } public Trigger getButtonA() { @@ -189,4 +192,4 @@ public void nameRightXAxis(String purpose) { public void nameRightYAxis(String purpose) { buttonPurposeHashMap.put("rightYAxis", purpose); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/lib/controller/LogitechController.java b/src/main/java/frc/lib/controller/LogitechController.java index 0a07986..d7ff96e 100644 --- a/src/main/java/frc/lib/controller/LogitechController.java +++ b/src/main/java/frc/lib/controller/LogitechController.java @@ -1,11 +1,16 @@ package frc.lib.controller; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import java.util.HashMap; +import java.util.Map; +import java.util.stream.Collectors; public class LogitechController { + private int port; + private final Joystick joystick; private final Trigger A; @@ -38,6 +43,8 @@ public class LogitechController { * @param port The port the controller is on */ public LogitechController(int port) { + this.port = port; + joystick = new Joystick(port); A = new JoystickButton(joystick, 1); @@ -66,6 +73,8 @@ public LogitechController(int port) { leftYAxis.setInverted(true); rightYAxis.setInverted(true); dPadYAxis.setInverted(true); + + buttonPurposeHashMap.put("type", "LogitechController"); } public Trigger getA() { @@ -220,27 +229,47 @@ public void nameDPadLeft(String purpose) { buttonPurposeHashMap.put("dPadLeft", purpose); } - public void namLftXAxis(String purpose) { + public void nameLeftXAxis(String purpose) { buttonPurposeHashMap.put("leftXAxis", purpose); } - public void namLftYAxis(String purpose) { + public void nameLeftYAxis(String purpose) { buttonPurposeHashMap.put("leftYAxis", purpose); } - public void namRghtXAxis(String purpose) { + public void nameRightXAxis(String purpose) { buttonPurposeHashMap.put("rightXAxis", purpose); } - public void namRghtYAxis(String purpose) { + public void nameRightYAxis(String purpose) { buttonPurposeHashMap.put("rightYAxis", purpose); } - public void namDadXAxis(String purpose) { + public void nameDPadXAxis(String purpose) { buttonPurposeHashMap.put("dPadXAxis", purpose); } - public void namDadYAxis(String purpose) { + public void nameDPadYAxis(String purpose) { buttonPurposeHashMap.put("dPadYAxis", purpose); } + + public void sendButtonNamesToNT() { + NetworkTableInstance.getDefault() + .getTable("Controllers") + .getEntry(port + "") + .setString(toJSON()); + } + + /** + * @return Button names as a JSON String + */ + public String toJSON() { + return buttonPurposeHashMap.entrySet().stream() + .map((Map.Entry buttonEntry) -> stringifyButtonName(buttonEntry)) + .collect(Collectors.joining(", ", "{", "}")); + } + + private String stringifyButtonName(Map.Entry buttonEntry) { + return "\"" + buttonEntry.getKey() + "\": " + "\"" + buttonEntry.getValue() + "\""; + } } diff --git a/src/main/java/frc/lib/controller/ThrustmasterJoystick.java b/src/main/java/frc/lib/controller/ThrustmasterJoystick.java index 9e91306..35968b4 100644 --- a/src/main/java/frc/lib/controller/ThrustmasterJoystick.java +++ b/src/main/java/frc/lib/controller/ThrustmasterJoystick.java @@ -1,11 +1,16 @@ package frc.lib.controller; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import java.util.HashMap; +import java.util.Map; +import java.util.stream.Collectors; public class ThrustmasterJoystick { + private int port; + private final Joystick joystick; private final Trigger trigger; @@ -36,6 +41,8 @@ public class ThrustmasterJoystick { * @param port The port the controller is on */ public ThrustmasterJoystick(int port) { + this.port = port; + joystick = new Joystick(port); trigger = new JoystickButton(joystick, 1); @@ -60,6 +67,8 @@ public ThrustmasterJoystick(int port) { zAxis = new JoystickAxis(joystick, 2); sliderAxis = new JoystickAxis(joystick, 3); sliderAxis.setInverted(true); + + buttonPurposeHashMap.put("type", "ThrustmasterJoystick"); } public Trigger getTrigger() { @@ -221,4 +230,24 @@ public void nameZAxis(String purpose) { public void nameSliderAxis(String purpose) { buttonPurposeHashMap.put("sliderAxis", purpose); } + + public void sendButtonNamesToNT() { + NetworkTableInstance.getDefault() + .getTable("Controllers") + .getEntry(port + "") + .setString(toJSON()); + } + + /** + * @return Button names as a JSON String + */ + public String toJSON() { + return buttonPurposeHashMap.entrySet().stream() + .map((Map.Entry buttonEntry) -> stringifyButtonName(buttonEntry)) + .collect(Collectors.joining(", ", "{", "}")); + } + + private String stringifyButtonName(Map.Entry buttonEntry) { + return "\"" + buttonEntry.getKey() + "\": " + "\"" + buttonEntry.getValue() + "\""; + } } diff --git a/src/main/java/frc/lib/loops/UpdateManager.java b/src/main/java/frc/lib/loops/UpdateManager.java index 17e1e75..1758ecc 100644 --- a/src/main/java/frc/lib/loops/UpdateManager.java +++ b/src/main/java/frc/lib/loops/UpdateManager.java @@ -1,6 +1,8 @@ package frc.lib.loops; import edu.wpi.first.wpilibj.TimesliceRobot; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Subsystem; public class UpdateManager { private TimesliceRobot robot; @@ -9,7 +11,19 @@ public UpdateManager(TimesliceRobot robot) { this.robot = robot; } - public void schedule(Updatable subsystem, double updateTimeslice) { - robot.schedule(() -> subsystem.update(), updateTimeslice); + /** + * @param subsystem The subsystem getting registered. + */ + public void schedule(Subsystem subsystem) { + CommandScheduler.getInstance().registerSubsystem(subsystem); } -} + + /** + * @param subsystem The subsystem getting registered. + * @param updateTimeslice The corresponding timeslice of the subsystem. + */ + public void schedule(Subsystem subsystem, double updateTimeslice) { + schedule(subsystem); + robot.schedule(() -> ((Updatable) subsystem).update(), updateTimeslice); + } +} \ No newline at end of file diff --git a/src/main/java/frc/lib/swerve/SwerveModule.java b/src/main/java/frc/lib/swerve/SwerveModule.java index 463ab25..b7cf98b 100644 --- a/src/main/java/frc/lib/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/swerve/SwerveModule.java @@ -3,7 +3,8 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_CANCoder; + import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -14,9 +15,9 @@ public class SwerveModule { public int moduleNumber; private double angleOffset; - private WPI_TalonFX mAngleMotor; - private WPI_TalonFX mDriveMotor; - private CANCoder angleEncoder; + private WPI_TalonFX angleMotor; + private WPI_TalonFX driveMotor; + private WPI_CANCoder angleEncoder; private double lastAngle; private boolean inverted; @@ -31,18 +32,18 @@ public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) { /* Angle Encoder Config */ angleEncoder = moduleConstants.canivoreName.isEmpty() - ? new CANCoder(moduleConstants.cancoderID) - : new CANCoder(moduleConstants.cancoderID, moduleConstants.canivoreName.get()); + ? new WPI_CANCoder(moduleConstants.cancoderID) + : new WPI_CANCoder(moduleConstants.cancoderID, moduleConstants.canivoreName.get()); configAngleEncoder(); /* Angle Motor Config */ - mAngleMotor = moduleConstants.canivoreName.isEmpty() + angleMotor = moduleConstants.canivoreName.isEmpty() ? new WPI_TalonFX(moduleConstants.angleMotorID) : new WPI_TalonFX(moduleConstants.angleMotorID, moduleConstants.canivoreName.get()); configAngleMotor(); /* Drive Motor Config */ - mDriveMotor = moduleConstants.canivoreName.isEmpty() + driveMotor = moduleConstants.canivoreName.isEmpty() ? new WPI_TalonFX(moduleConstants.driveMotorID) : new WPI_TalonFX(moduleConstants.driveMotorID, moduleConstants.canivoreName.get()); configDriveMotor(); @@ -59,13 +60,13 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) if (isOpenLoop) { double percentOutput = desiredState.speedMetersPerSecond / Constants.SwerveConstants.maxSpeed; - mDriveMotor.set(ControlMode.PercentOutput, percentOutput); + driveMotor.set(ControlMode.PercentOutput, percentOutput); } else { double velocity = Conversions.MPSToFalcon( desiredState.speedMetersPerSecond, Constants.SwerveConstants.wheelCircumference, Constants.SwerveConstants.driveGearRatio); - mDriveMotor.set( + driveMotor.set( ControlMode.Velocity, velocity, DemandType.ArbitraryFeedForward, @@ -76,7 +77,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) ? lastAngle : desiredState.angle .getDegrees(); // Prevent rotating module if speed is less then 1%. Prevents Jittering. - mAngleMotor.set( + angleMotor.set( ControlMode.Position, Conversions.degreesToFalcon(angle, Constants.SwerveConstants.angleGearRatio)); lastAngle = angle; } @@ -84,7 +85,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) public void resetToAbsolute() { double absolutePosition = Conversions.degreesToFalcon( getCanCoder().getDegrees() - angleOffset, Constants.SwerveConstants.angleGearRatio); - mAngleMotor.setSelectedSensorPosition(absolutePosition); + angleMotor.setSelectedSensorPosition(absolutePosition); } private void configAngleEncoder() { @@ -93,21 +94,21 @@ private void configAngleEncoder() { } private void configAngleMotor() { - mAngleMotor.configFactoryDefault(); - mAngleMotor.configAllSettings(Robot.ctreConfigs.swerveAngleFXConfig); - mAngleMotor.setInverted(Constants.SwerveConstants.angleMotorInvert); - mAngleMotor.setNeutralMode(Constants.SwerveConstants.angleNeutralMode); - mAngleMotor.enableVoltageCompensation(true); + angleMotor.configFactoryDefault(); + angleMotor.configAllSettings(Robot.ctreConfigs.swerveAngleFXConfig); + angleMotor.setInverted(Constants.SwerveConstants.angleMotorInvert); + angleMotor.setNeutralMode(Constants.SwerveConstants.angleNeutralMode); + angleMotor.enableVoltageCompensation(true); resetToAbsolute(); } private void configDriveMotor() { - mDriveMotor.configFactoryDefault(); - mDriveMotor.configAllSettings(Robot.ctreConfigs.swerveDriveFXConfig); - mDriveMotor.setNeutralMode(Constants.SwerveConstants.driveNeutralMode); - mDriveMotor.setSelectedSensorPosition(0); - mDriveMotor.enableVoltageCompensation(true); - mDriveMotor.setInverted(inverted); + driveMotor.configFactoryDefault(); + driveMotor.configAllSettings(Robot.ctreConfigs.swerveDriveFXConfig); + driveMotor.setNeutralMode(Constants.SwerveConstants.driveNeutralMode); + driveMotor.setSelectedSensorPosition(0); + driveMotor.enableVoltageCompensation(true); + driveMotor.setInverted(inverted); } public Rotation2d getCanCoder() { @@ -116,19 +117,19 @@ public Rotation2d getCanCoder() { public SwerveModuleState getState() { double velocity = Conversions.falconToMPS( - mDriveMotor.getSelectedSensorVelocity(), + driveMotor.getSelectedSensorVelocity(), Constants.SwerveConstants.wheelCircumference, Constants.SwerveConstants.driveGearRatio); Rotation2d angle = Rotation2d.fromDegrees(Conversions.falconToDegrees( - mAngleMotor.getSelectedSensorPosition(), Constants.SwerveConstants.angleGearRatio)); + angleMotor.getSelectedSensorPosition(), Constants.SwerveConstants.angleGearRatio)); return new SwerveModuleState(velocity, angle); } public double getDriveTemperature() { - return mDriveMotor.getTemperature(); + return driveMotor.getTemperature(); } public double getSteerTemperature() { - return mAngleMotor.getTemperature(); + return angleMotor.getTemperature(); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c18a127..e9f67d4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,6 +12,19 @@ public static final class GlobalConstants { public static final double targetVoltage = 12.0; // Used for voltage compensation } + public static final class ControllerConstants { + public static final int LEFT_DRIVE_CONTROLLER = 0; + public static final int RIGHT_DRIVE_CONTROLLER = 1; + public static final int OPERATOR_CONTROLLER = 2; + } + + public static final class TimesliceConstants { + public static final double ROBOT_PERIODIC_ALLOCATION = 0.004; + public static final double CONTROLLER_PERIOD = 0.010; + + public static final double DRIVETRAIN_PERIOD = 0.0025; + } + public static final class SwerveConstants extends Mk4SwerveConstants {} public static class Mk4SwerveConstants { @@ -231,18 +244,4 @@ public static final class Mod3 { new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset, canivoreName); } } - - - public static final class ControllerConstants { - public static final int LEFT_DRIVE_CONTROLLER = 0; - public static final int RIGHT_DRIVE_CONTROLLER = 1; - public static final int OPERATOR_CONTROLLER = 2; - } - - public static final class TimesliceConstants { - public static final double ROBOT_PERIODIC_ALLOCATION = 0.002; - public static final double CONTROLLER_PERIOD = 0.005; - - public static final double DRIVETRAIN_PERIOD = 0.0015; - } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 14149b0..5ee53f7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,34 +1,32 @@ package frc.robot; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimesliceRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.lib.loops.UpdateManager; import frc.robot.Constants.TimesliceConstants; public class Robot extends TimesliceRobot { public static CTREConfigs ctreConfigs = new CTREConfigs(); - private RobotContainer robotContainer = new RobotContainer(); - - // Create an object to manage the timeslices for the subsystems - private UpdateManager updateManager = new UpdateManager(this); + private RobotContainer robotContainer; public Robot() { super(TimesliceConstants.ROBOT_PERIODIC_ALLOCATION, TimesliceConstants.CONTROLLER_PERIOD); + robotContainer = new RobotContainer(this); + // Prevents the logging of many errors with our controllers DriverStation.silenceJoystickConnectionWarning(true); - } - @Override - public void robotInit() { - scheduleUpdateFunctions(); + // Begin logging networktables, controller inputs, and more + if (isReal()) { + DriverStation.startDataLog(DataLogManager.getLog()); // getLog() automatically starts the logging + } } - private void scheduleUpdateFunctions() { - updateManager.schedule(robotContainer.getSwerveDriveSubsystem(), TimesliceConstants.DRIVETRAIN_PERIOD); - } + @Override + public void robotInit() {} @Override public void robotPeriodic() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 45491a8..8dd37e7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,12 +3,14 @@ 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.TimesliceRobot; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.lib.controller.Axis; import frc.lib.controller.ThrustmasterJoystick; +import frc.lib.loops.UpdateManager; import frc.robot.Constants.ControllerConstants; +import frc.robot.Constants.TimesliceConstants; import frc.robot.commands.*; import frc.robot.subsystems.*; import frc.robot.util.AutonomousManager; @@ -24,17 +26,16 @@ public class RobotContainer { private AutonomousManager autonomousManager; private TrajectoryLoader trajectoryLoader; + private UpdateManager updateManager; - public RobotContainer() { + public RobotContainer(TimesliceRobot robot) { trajectoryLoader = new TrajectoryLoader(); - + updateManager = new UpdateManager(robot); autonomousManager = new AutonomousManager(trajectoryLoader, this); - CommandScheduler.getInstance().registerSubsystem(swerveDriveSubsystem); + updateManager.schedule(swerveDriveSubsystem, TimesliceConstants.DRIVETRAIN_PERIOD); - CommandScheduler.getInstance() - .setDefaultCommand( - swerveDriveSubsystem, + swerveDriveSubsystem.setDefaultCommand( new DriveCommand( swerveDriveSubsystem, getDriveForwardAxis(), @@ -55,6 +56,9 @@ private void configureControllerLayout() { .onTrue(new InstantCommand(() -> swerveDriveSubsystem.resetPose(new Pose2d( new Translation2d(), swerveDriveSubsystem.getGyroRotation2d().rotateBy(Rotation2d.fromDegrees(180)))))); + + rightDriveController.sendButtonNamesToNT(); + leftDriveController.sendButtonNamesToNT(); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/util/LazyPathPlannerTrajectory.java b/src/main/java/frc/robot/util/LazyPathPlannerTrajectory.java deleted file mode 100644 index cda5e80..0000000 --- a/src/main/java/frc/robot/util/LazyPathPlannerTrajectory.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.util; - -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; - -public class LazyPathPlannerTrajectory { - private final String trajectoryPath; - private final double maxVelocity; - private final double maxAcceleration; - - // private Optional trajectory = Optional.empty(); - - private PathPlannerTrajectory trajectory; - - public LazyPathPlannerTrajectory(String trajectoryPath, double maxVelocity, double maxAcceleration) { - this.trajectoryPath = trajectoryPath; - this.maxVelocity = maxVelocity; - this.maxAcceleration = maxAcceleration; - - this.trajectory = loadTrajectory(trajectoryPath, maxVelocity, maxAcceleration); - } - - public PathPlannerTrajectory getTrajectory() { - // Loads the trajectory only at the first request - // if (trajectory.isEmpty()) - // trajectory = Optional.of(loadTrajectory(trajectoryPath, maxVelocity, maxAcceleration)); - - // return trajectory.orElse(null); - - return trajectory; - } - - private PathPlannerTrajectory loadTrajectory(String path, double maxVel, double maxAccel) { - return PathPlanner.loadPath(path, maxVel, maxAccel); - } -} diff --git a/src/main/java/frc/robot/util/TrajectoryLoader.java b/src/main/java/frc/robot/util/TrajectoryLoader.java index a05c425..7b78ada 100644 --- a/src/main/java/frc/robot/util/TrajectoryLoader.java +++ b/src/main/java/frc/robot/util/TrajectoryLoader.java @@ -1,5 +1,6 @@ package frc.robot.util; +import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; public class TrajectoryLoader { @@ -18,91 +19,95 @@ public class TrajectoryLoader { private static final String TWO_BALL_STEAL_PATH = "twoball far steal"; private static final String ONE_BALL_STEAL_PATH = "oneball far steal"; - private LazyPathPlannerTrajectory threeBall; - private LazyPathPlannerTrajectory threeBall2; - private LazyPathPlannerTrajectory twoBall; - private LazyPathPlannerTrajectory twoBallFar; - private LazyPathPlannerTrajectory twoBallFarManualGyro; - private LazyPathPlannerTrajectory fourBall; - private LazyPathPlannerTrajectory fourBall2; - private LazyPathPlannerTrajectory fourBall3; - private LazyPathPlannerTrajectory fiveBall; - private LazyPathPlannerTrajectory fiveBall1; - private LazyPathPlannerTrajectory fiveBallSweep; - private LazyPathPlannerTrajectory fiveBall2; - private LazyPathPlannerTrajectory twoBallSteal; - private LazyPathPlannerTrajectory oneBallSteal; + private PathPlannerTrajectory threeBall; + private PathPlannerTrajectory threeBall2; + private PathPlannerTrajectory twoBall; + private PathPlannerTrajectory twoBallFar; + private PathPlannerTrajectory twoBallFarManualGyro; + private PathPlannerTrajectory fourBall; + private PathPlannerTrajectory fourBall2; + private PathPlannerTrajectory fourBall3; + private PathPlannerTrajectory fiveBall; + private PathPlannerTrajectory fiveBall1; + private PathPlannerTrajectory fiveBallSweep; + private PathPlannerTrajectory fiveBall2; + private PathPlannerTrajectory twoBallSteal; + private PathPlannerTrajectory oneBallSteal; public TrajectoryLoader() { - threeBall = new LazyPathPlannerTrajectory(THREE_BALL_PATH, 5, 6.0); - threeBall2 = new LazyPathPlannerTrajectory(THREE_BALL_2_PATH, 5, 2.5); - // twoBall = new LazyPathPlannerTrajectory(TWO_BALL_PATH, 5, 2.0); - twoBallFar = new LazyPathPlannerTrajectory(THREE_BALL_PATH, 5, 4.0); - // twoBallFarManualGyro = new LazyPathPlannerTrajectory(TWO_BALL_FAR_MANUAL_GYRO_PATH, 5, 2.5); - // fiveBall = new LazyPathPlannerTrajectory(FIVE_BALL_PATH, 5, 2.5); - fourBall = new LazyPathPlannerTrajectory(FOUR_BALL_PATH, 5, 2.5); - fourBall2 = new LazyPathPlannerTrajectory(FOUR_BALL_2_PATH, 5, 6.0); - fourBall3 = new LazyPathPlannerTrajectory(FOUR_BALL_3_PATH, 5, 6.0); - fiveBall1 = new LazyPathPlannerTrajectory(FIVE_BALL_1_PATH, 5, 5.5); - fiveBallSweep = new LazyPathPlannerTrajectory(FIVE_BALL_SWEEP_PATH, 2.5, 3.5); - fiveBall2 = new LazyPathPlannerTrajectory(FIVE_BALL_2_PATH, 5, 6.0); - // twoBallSteal = new LazyPathPlannerTrajectory(TWO_BALL_STEAL_PATH, 5, 2.5); - // oneBallSteal = new LazyPathPlannerTrajectory(ONE_BALL_STEAL_PATH, 5, 2.5); + threeBall = loadTrajectory(THREE_BALL_PATH, 5, 6.0); + threeBall2 = loadTrajectory(THREE_BALL_2_PATH, 5, 2.5); + // twoBall = loadTrajectory(TWO_BALL_PATH, 5, 2.0); + twoBallFar = loadTrajectory(THREE_BALL_PATH, 5, 4.0); + // twoBallFarManualGyro = loadTrajectory(TWO_BALL_FAR_MANUAL_GYRO_PATH, 5, 2.5); + // fiveBall = loadTrajectory(FIVE_BALL_PATH, 5, 2.5); + fourBall = loadTrajectory(FOUR_BALL_PATH, 5, 2.5); + fourBall2 = loadTrajectory(FOUR_BALL_2_PATH, 5, 6.0); + fourBall3 = loadTrajectory(FOUR_BALL_3_PATH, 5, 6.0); + fiveBall1 = loadTrajectory(FIVE_BALL_1_PATH, 5, 5.5); + fiveBallSweep = loadTrajectory(FIVE_BALL_SWEEP_PATH, 2.5, 3.5); + fiveBall2 = loadTrajectory(FIVE_BALL_2_PATH, 5, 6.0); + // twoBallSteal = loadTrajectory(TWO_BALL_STEAL_PATH, 5, 2.5); + // oneBallSteal = loadTrajectory(ONE_BALL_STEAL_PATH, 5, 2.5); + } + + private PathPlannerTrajectory loadTrajectory(String path, double maxVel, double maxAccel) { + return PathPlanner.loadPath(path, maxVel, maxAccel); } public PathPlannerTrajectory getThreeBall() { - return threeBall.getTrajectory(); + return threeBall; } public PathPlannerTrajectory getThreeBall2() { - return threeBall2.getTrajectory(); + return threeBall2; } public PathPlannerTrajectory getTwoBall() { - return twoBall.getTrajectory(); + return twoBall; } public PathPlannerTrajectory getTwoBallFar() { - return twoBallFar.getTrajectory(); + return twoBallFar; } public PathPlannerTrajectory getTwoBallFarManualGyro() { - return twoBallFarManualGyro.getTrajectory(); + return twoBallFarManualGyro; } public PathPlannerTrajectory getFiveBall() { - return fiveBall.getTrajectory(); + return fiveBall; } public PathPlannerTrajectory getFiveBall1() { - return fiveBall1.getTrajectory(); + return fiveBall1; } public PathPlannerTrajectory getFiveBallSweep() { - return fiveBallSweep.getTrajectory(); + return fiveBallSweep; } public PathPlannerTrajectory getFiveBall2() { - return fiveBall2.getTrajectory(); + return fiveBall2; } public PathPlannerTrajectory getFourBall() { - return fourBall.getTrajectory(); + return fourBall; } public PathPlannerTrajectory getFourBall2() { - return fourBall2.getTrajectory(); + return fourBall2; } public PathPlannerTrajectory getFourBall3() { - return fourBall3.getTrajectory(); + return fourBall3; } public PathPlannerTrajectory getTwoBallSteal() { - return twoBallSteal.getTrajectory(); + return twoBallSteal; } public PathPlannerTrajectory getOneBallSteal() { - return oneBallSteal.getTrajectory(); + return oneBallSteal; } }