Ramsete command construction (aDrive is the auto drivetrain object) ----------------------------------------------------------------------------------- public RamseteCommand setAutoCode() { aDrive.resetEncoders(); TrajectoryConfig config = new TrajectoryConfig(1, 1); config.setKinematics(aDrive.getKinematics()); Trajectory trajectory = TrajectoryGenerator.generateTrajectory( Arrays.asList(new Pose2d(0, 0, new Rotation2d()), new Pose2d(1, 0, new Rotation2d())), config); ramseteCommand = new RamseteCommand( trajectory, aDrive::getPose, new RamseteController(2, .7), aDrive.getFeedForward(), aDrive.getKinematics(), aDrive::getSpeeds, aDrive.getLeftPIDController(), aDrive.getRightPIDController(), aDrive::setOutput, aDrive); AutoCodeSet = true; return ramseteCommand; } ------------------------------------------------------------------------------------------------------------------ ------------------------------------------------------------------------------------------------------------------ ------------------------------------------------------------------------------------------------------------------ What the robot.java runs every thread tick public void autonomous() { if (!autoSwitcher.AutoCodeSet) { autoSwitcher.setAutoCode().schedule(); System.out.println("Scheduled Auto Code"); } else { CommandScheduler.getInstance().run(); } }