Skip to content

Kenny #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
DDbrother9999 merged 7 commits into from
Jan 20, 2026
Merged

Kenny #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
7d700ef
Readded my Main Auto
p-rint Jan 12, 2026
c17b9e5
test
p-rint Jan 12, 2026
8039807
Cloned mainTeleOp
p-rint Jan 14, 2026
0dd3c69
d
p-rint Jan 14, 2026
2217deb
PLEaSE WORK
p-rint Jan 14, 2026
b81359f
Fixed MainAuto.java, removed some commented stuff frim My teleop
p-rint Jan 14, 2026
8219f6e
chore: resolve merge conflicts
DDbrother9999 Jan 20, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
266 changes: 183 additions & 83 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,117 +1,217 @@
package org.firstinspires.ftc.teamcode;

import static dev.nextftc.extensions.pedro.PedroComponent.follower;
package org.firstinspires.ftc.teamcode; // make sure this aligns with class location
//HIIII pls don't delete

import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierCurve;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.Path;
import com.pedropathing.paths.PathChain;
import com.pedropathing.util.Timer;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;

import org.firstinspires.ftc.teamcode.subsystems.Drive;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.subsystems.Intake;
import org.firstinspires.ftc.teamcode.subsystems.Outtake;
import org.firstinspires.ftc.teamcode.subsystems.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Storage;
import org.firstinspires.ftc.teamcode.subsystems.Transitions;

import dev.nextftc.core.commands.Command;
import dev.nextftc.core.commands.CommandManager;
import dev.nextftc.core.commands.delays.Delay;
import dev.nextftc.core.commands.groups.SequentialGroup;
import dev.nextftc.core.components.BindingsComponent;
import dev.nextftc.core.components.SubsystemComponent;
import dev.nextftc.extensions.pedro.FollowPath;
import dev.nextftc.ftc.NextFTCOpMode;
import dev.nextftc.ftc.components.BulkReadComponent;

@Autonomous
public class MainAuto extends NextFTCOpMode {
{
addComponents(
BulkReadComponent.INSTANCE, // TODO: make actual MANUAL mode bulkreading (we don't need to also read the expansion hub every loop)
BindingsComponent.INSTANCE,
CommandManager.INSTANCE,
new SubsystemComponent(
Storage.INSTANCE,
Robot.INSTANCE,
Drive.INSTANCE,
Intake.INSTANCE,
Outtake.INSTANCE,
Transitions.INSTANCE
)
);
}

public static Pose endPose;
@Autonomous(name = "Main Auto")
public class MainAuto extends OpMode {

private Follower follower;
private Timer pathTimer, actionTimer, opmodeTimer;

private int pathState;



/*
private final Pose poseA = new Pose(48, 12, Math.toRadians(180)); // Start Pose of our robot.
private final Pose poseB = new Pose(18, 84, Math.toRadians(135));
private final Pose poseC = new Pose(74, 77, Math.toRadians(50));
private final Pose poseD = new Pose(21, 60, Math.toRadians(50));
private final Pose poseE = new Pose(73, 80, Math.toRadians(50));
private final Pose poseF = new Pose(18, 36, Math.toRadians(50));
private final Pose poseG = new Pose(75, 81, Math.toRadians(50));

private Path startPosition;
private PathChain moveB, moveC, moveD, moveE, moveF;
*/

private final Pose startPose = new Pose(48, 12, Math.toRadians(90)); // Start Pose of our robot.
private final Pose pickUp1 = new Pose(18, 84, Math.toRadians(180));

public static final Pose startPoseFarBlue = new Pose(56, 8, Math.toRadians(270)); // Start Pose of our robot.
public static final Pose scorePoseCloseBlue = new Pose(20, 123, Math.toRadians(323)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle.
public static final Pose scorePoseBlue = new Pose(68, 76, Math.toRadians(315)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle.
private final Pose pickUp1C1 = new Pose(57, 90);
private final Pose pickUp1C2 = new Pose(57, 84);

public static final Pose intakeAlign1Blue = new Pose(68, 84, Math.toRadians(180)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle.
public static final Pose intake1Blue = new Pose(16, 84, Math.toRadians(180)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle.
private final Pose pickUp2 = new Pose(18, 60, Math.toRadians(180));

private final Pose pickUp2C1 = new Pose(75, 57);
private final Pose pickUp2C2 = new Pose(66, 60);

public static final Pose startPoseFarRed = new Pose(87, 8, Math.toRadians(270)); // Start Pose of our robot.
public static final Pose startPoseCloseRed = new Pose(124, 123, Math.toRadians(37)); // Start Pose of our robot.
public static final Pose scorePoseRed = new Pose(76, 76, Math.toRadians(225)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle.
private final Pose pickUp3 = new Pose(18, 36, Math.toRadians(180));

private final Pose pickUp3C1 = new Pose(72, 30);
private final Pose pickUp3C2 = new Pose(72, 36);

private final Pose startPose = startPoseFarBlue;
public static final Pose scorePose = scorePoseBlue;
//public static final Pose scorePosebutActually = new Pose(73, 70, Math.toRadians(135)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle.
private final Pose shootPose = new Pose(75, 87, Math.toRadians(330));


Path scorePreload = new Path(new BezierLine(startPose, scorePose));
Path intakeAlign1 = new Path(new BezierLine(scorePose, intakeAlign1Blue));
Path intake1 = new Path(new BezierLine(intakeAlign1Blue, intake1Blue));
Path intakeOut1 = new Path(new BezierLine(intake1Blue, intakeAlign1Blue));
Path score1 = new Path(new BezierLine(intakeAlign1Blue, scorePose));
private Path startPosition;
private PathChain shoot1, intake2, shoot2, intake3, shoot3, goToStart;
public void buildPaths() {
//startPosition = new Path(new BezierLine(startPose, pickUp1));
startPosition = new Path(new BezierCurve(startPose, pickUp1C1, pickUp1C2, pickUp1));
startPosition.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180));


shoot1 = follower.pathBuilder()
.addPath(new BezierLine(pickUp1, shootPose))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330))
.build();

intake2 = follower.pathBuilder()
//.addPath(new BezierLine(shootPose, pickUp2))
.addPath(new BezierCurve(shootPose, pickUp2C1, pickUp2C2 ,pickUp2))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180))
.build();

shoot2 = follower.pathBuilder()
.addPath(new BezierLine(pickUp2, shootPose))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330))
.build();

private Command autonomousRoutine() {
scorePreload.setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading());
intakeAlign1.setLinearHeadingInterpolation(scorePose.getHeading(), intakeAlign1Blue.getHeading());
intake1.setLinearHeadingInterpolation(intakeAlign1Blue.getHeading(), intake1Blue.getHeading());
score1.setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading());
intake3 = follower.pathBuilder()
//.addPath(new BezierLine(shootPose, pickUp3))
.addPath(new BezierCurve(shootPose, pickUp3C1, pickUp3C2 ,pickUp3))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180))
.build();

shoot3 = follower.pathBuilder()
.addPath(new BezierLine(pickUp3, shootPose))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330))
.build();

goToStart = follower.pathBuilder()
.addPath(new BezierLine(shootPose, startPose))
.setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(90))
.build();



}

int standardDelay = 1;

return new SequentialGroup(
new FollowPath(scorePreload),
new Delay(standardDelay),
Robot.outtakeAll,
new Delay(standardDelay),
new FollowPath(intakeAlign1),
new Delay(standardDelay),
// Start running intake procedure
new FollowPath(intake1),
new Delay(standardDelay),
new FollowPath(intakeOut1),
new Delay(standardDelay),
new FollowPath(score1),
Robot.outtakeAll
);
public void autonomousPathUpdate() {
switch (pathState) {
case 0:
follower.followPath(startPosition);
setPathState(1);
break;
case 1:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if(!follower.isBusy()) {
/* Score Preload */
/* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
follower.followPath(shoot1, 0.7, true);
setPathState(2);
}
break;
case 2:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if(!follower.isBusy()) {
/* Score Preload */
/* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
follower.followPath(intake2, .7,true);
setPathState(3);
}
break;
case 3:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if(!follower.isBusy()) {
/* Score Preload */
/* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
follower.followPath(shoot2, .7,true);
setPathState(4);
}
break;
case 4:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if(!follower.isBusy()) {
/* Score Preload */
/* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
follower.followPath(intake3, .7,true);
setPathState(5);
}
break;
case 5:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if(!follower.isBusy()) {
/* Score Preload */
/* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
follower.followPath(shoot3, .7,true);
setPathState(6);

}
break;
case 6:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if(!follower.isBusy()) {
/* Score Preload */
/* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
follower.followPath(goToStart, .7,true);
setPathState(-1);

}
}
}
/** These change the states of the paths and actions. It will also reset the timers of the individual switches **/
public void setPathState(int pState) {
pathState = pState;
pathTimer.resetTimer();
}


@Override
public void onStartButtonPressed() {
follower().setStartingPose(startPose);
autonomousRoutine().schedule();
follower().breakFollowing();
public void loop() {
// These loop the movements of the robot, these must be called continuously in order to work
follower.update();
autonomousPathUpdate();
// Feedback to Driver Hub for debugging
telemetry.addData("path state", pathState);
telemetry.addData("x", follower.getPose().getX());
telemetry.addData("y", follower.getPose().getY());
telemetry.addData("heading", follower.getPose().getHeading());
telemetry.update();
}

public void onUpdate(){
Drive.telemetryM.update();
follower().update();


@Override
public void init() {
pathTimer = new Timer();
opmodeTimer = new Timer();
opmodeTimer.resetTimer();
follower = Constants.createFollower(hardwareMap);
buildPaths();
follower.setStartingPose(startPose);
}

public void onStop(){
endPose = follower().getPose();
/** This method is called continuously after Init while waiting for "play". **/
@Override
public void init_loop() {}


/** This method is called once at the start of the OpMode.
* It runs all the setup actions, including building paths and starting the path system **/
@Override
public void start() {
opmodeTimer.resetTimer();
setPathState(0);
}
}
/** We do not use this because everything should automatically disable **/
@Override
public void stop() {}


}
Loading