diff --git a/TeamCode/TeamCode/BotTwoTeleopPreset.java b/TeamCode/TeamCode/BotTwoTeleopPreset.java new file mode 100644 index 000000000000..57563ed06947 --- /dev/null +++ b/TeamCode/TeamCode/BotTwoTeleopPreset.java @@ -0,0 +1,294 @@ +/* + * Copyright (c) 2025 FIRST + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior + * written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode.teamcode; + +import static com.qualcomm.robotcore.hardware.DcMotor.ZeroPowerBehavior.BRAKE; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; + +/* + * This file includes a teleop (driver-controlled) file for the goBILDA® StarterBot for the + * 2025-2026 FIRST® Tech Challenge season DECODE™. It leverages a differential/Skid-Steer + * system for robot mobility, one high-speed motor driving two "launcher wheels", and two servos + * which feed that launcher. + * + * Likely the most niche concept we'll use in this example is closed-loop motor velocity control. + * This control method reads the current speed as reported by the motor's encoder and applies a varying + * amount of power to reach, and then hold a target velocity. The FTC SDK calls this control method + * "RUN_USING_ENCODER". This contrasts to the default "RUN_WITHOUT_ENCODER" where you control the power + * applied to the motor directly. + * Since the dynamics of a launcher wheel system varies greatly from those of most other FTC mechanisms, + * we will also need to adjust the "PIDF" coefficients with some that are a better fit for our application. + */ + +@TeleOp(name = "BotTwoTeleopPreset", group = "StarterBot") +//@Disabled +public class BotTwoTeleopPreset extends OpMode { + + /* + * When we control our launcher motor, we are using encoders. These allow the control system + * to read the current speed of the motor and apply more or less power to keep it at a constant + * velocity. Here we are setting the target, and minimum velocity that the launcher should run + * at. The minimum velocity is a threshold for determining when to fire. + */ + final double LAUNCHER_TARGET_VELOCITY = 1125; + final double LAUNCHER_MIN_VELOCITY = 1075; + + // Declare OpMode members. + private DcMotor leftFrontDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor leftBackDrive = null; + private DcMotor rightBackDrive = null; + private DcMotor intake = null; + + + private DcMotor shooter = null; + + + private CRServo leftPusher = null; + + + private CRServo rightPusher = null; + + + private boolean shotTog = false; + + + + + /* + * TECH TIP: State Machines + * We use a "state machine" to control our launcher motor and feeder servos in this program. + * The first step of a state machine is creating an enum that captures the different "states" + * that our code can be in. + * The core advantage of a state machine is that it allows us to continue to loop through all + * of our code while only running specific code when it's necessary. We can continuously check + * what "State" our machine is in, run the associated code, and when we are done with that step + * move on to the next state. + * This enum is called the "LaunchState". It reflects the current condition of the shooter + * motor and we move through the enum when the user asks our code to fire a shot. + * It starts at idle, when the user requests a launch, we enter SPIN_UP where we get the + * motor up to speed, once it meets a minimum speed then it starts and then ends the launch process. + * We can use higher level code to cycle through these states. But this allows us to write + * functions and autonomous routines in a way that avoids loops within loops, and "waits". + */ + + + + // Setup a variable for each drive wheel to save power level for telemetry + double leftFrontPower; + double rightFrontPower; + double leftBackPower; + double rightBackPower; + + /* + * Code to run ONCE when the driver hits INIT + */ + @Override + public void init() { + + + /* + * Initialize the hardware variables. Note that the strings used here as parameters + * to 'get' must correspond to the names assigned during the robot configuration + * step. + */ + leftFrontDrive = hardwareMap.get(DcMotor.class, "leftFront"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "rightFront"); + leftBackDrive = hardwareMap.get(DcMotor.class, "leftBack"); + rightBackDrive = hardwareMap.get(DcMotor.class, "rightBack"); + intake = hardwareMap.get(DcMotor.class, "intake"); + shooter = hardwareMap.get(DcMotor.class, "shooter"); + leftPusher = hardwareMap.get(CRServo.class, "leftPusher"); + rightPusher = hardwareMap.get(CRServo.class, "rightPusher"); + + + /* + * To drive forward, most robots need the motor on one side to be reversed, + * because the axles point in opposite directions. Pushing the left stick forward + * MUST make robot go forward. So adjust these two lines based on your first test drive. + * Note: The settings here assume direct drive on left and right wheels. Gear + * Reduction or 90 Deg drives may require direction flips + */ + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + /* + * Here we set our launcher to the RUN_USING_ENCODER runmode. + * If you notice that you have no control over the velocity of the motor, it just jumps + * right to a number much higher than your set point, make sure that your encoders are plugged + * into the port right beside the motor itself. And that the motors polarity is consistent + * through any wiring. + */ + + /* + * Setting zeroPowerBehavior to BRAKE enables a "brake mode". This causes the motor to + * slow down much faster when it is coasting. This creates a much more controllable + * drivetrain. As the robot stops much quicker. + */ + leftFrontDrive.setZeroPowerBehavior(BRAKE); + rightFrontDrive.setZeroPowerBehavior(BRAKE); + leftBackDrive.setZeroPowerBehavior(BRAKE); + rightBackDrive.setZeroPowerBehavior(BRAKE); + + /* + * set Feeders to an initial value to initialize the servo controller + */ + + + + + /* + * Much like our drivetrain motors, we set the left feeder servo to reverse so that they + * both work to feed the ball into the robot. + */ + + + /* + * Tell the driver that initialization is complete. + */ + telemetry.addData("Status", "Initialized"); + } + + /* + * Code to run REPEATEDLY after the driver hits INIT, but before they hit START + */ + @Override + public void init_loop() { + } + + /* + * Code to run ONCE when the driver hits START + */ + @Override + public void start() { + } + + /* + * Code to run REPEATEDLY after the driver hits START but before they hit STOP + */ + @Override + public void loop() { + + + if(gamepad2.a) { + shotTog = true; + } + else if (gamepad2.b) { + shotTog = false; + } + + if (shotTog) { + shooter.setPower(-0.55); + } + else { + shooter.setPower(0); + } + //else if (gamepad2.y) { + // shooter.setPower(-.65); + // } + //else if(gamepad2.b) { + // shooter.setPower(-0.8); + // } + // else if(gamepad2.a) { + // shooter.setPower(-1); + //} + + + + + if (gamepad1.right_bumper){ + intake.setPower(-1); + } + else{ + intake.setPower(0); + } + + if (gamepad2.left_bumper){ + leftPusher.setPower(1); + } + else{ + leftPusher.setPower(0); + } + + if(gamepad2.right_bumper){ + rightPusher.setPower(-1); + } + else{ + rightPusher.setPower(0); + } + + + mecanumDrive(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + } + + /* + * Here we give the user control of the speed of the launcher motor without automatically + * queuing a shot. + */ + + + /* + * Code to run ONCE after the driver hits STOP + */ + @Override + public void stop() { + } + + void mecanumDrive(double forward, double strafe, double rotate){ + + /* the denominator is the largest motor power (absolute value) or 1 + * This ensures all the powers maintain the same ratio, + * but only if at least one is out of the range [-1, 1] + */ + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(rotate), 1); + + leftFrontPower = (forward + strafe + rotate) / denominator; + rightFrontPower = (forward - strafe - rotate) / denominator; + leftBackPower = (forward - strafe + rotate) / denominator; + rightBackPower = (forward + strafe - rotate) / denominator; + + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + + } + + +} diff --git a/TeamCode/TeamCode/TrialAutonomous.java b/TeamCode/TeamCode/TrialAutonomous.java new file mode 100644 index 000000000000..7150e7b8408e --- /dev/null +++ b/TeamCode/TeamCode/TrialAutonomous.java @@ -0,0 +1,473 @@ +package org.firstinspires.ftc.teamcode.teamcode; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +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 com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; + +@Autonomous +public class TrialAutonomous extends OpMode { + + DcMotorEx shooter; + DcMotor intake; + CRServo leftPusher; + CRServo rightPusher; + + Follower follower; + Timer pathTimer; + + boolean pathStarted; + boolean pathsBuilt = false; + + enum StateMachine { + DRIVE_STARPOS_SHOOT_POS, + SHOOT_TO_PREPICKUP, + INTAKE, + LAUNCH_PICKUP_1, + SHOOT_TO_PREPICKUP2, + INTAKE2, + LAUNCH_PICKUP_2, + SHOOT_TO_PREPICKUP3, + INTAKE3, + LAUNCH_PICKUP_3, + FINISH; + + } + + StateMachine pathState; + + private Pose startPose; + private Pose shootPose; + private Pose preloadPose; + private Pose intakePose; + private Pose launchPose1; + + private Pose preloadPose2; + private Pose intakePose2; + private Pose launchPose2; + + private Pose preloadPose3; + private Pose intakePose3; + private Pose launchPose3; + + private Pose finishPose; + + + + + private PathChain driveStartPosShootPos, shootToPrePickup, INTAKE, LAUNCH_PICKUP_1, SHOOT_TO_PREPICKUP2, + INTAKEPOS2, LAUNCHPOSE2, PRELOADPOSE3, INTAKEPOSE3, LAUNCHPOSE3, FINISH; + + @Override + public void init() { + + pathState = StateMachine.DRIVE_STARPOS_SHOOT_POS; + + pathTimer = new Timer(); + pathTimer.resetTimer(); + + follower = Constants.createFollower(hardwareMap); + + shooter = hardwareMap.get(DcMotorEx.class, "shooter"); + intake = hardwareMap.get(DcMotor.class, "intake"); + leftPusher = hardwareMap.get(CRServo.class, "leftPusher"); + rightPusher = hardwareMap.get(CRServo.class, "rightPusher"); + + pathStarted = false; + + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + } + + @Override + public void init_loop() { + if (!pathsBuilt) { + InitializePoseValues(); + } + } + + private void InitializePoseValues() { + + if (gamepad1.y) telemetry.addLine("Big blue selected"); + else if (gamepad1.b) telemetry.addLine("Big red selected"); + else if (gamepad1.a) telemetry.addLine("Small red selected"); + else if (gamepad1.x) telemetry.addLine("Small blue selected"); + else if (gamepad1.right_bumper && gamepad1.x) { + telemetry.addLine("Backup selected "); + + } else { + telemetry.addLine("Select auto mode by pressing: " + + "X(Small blue) / Y(Big blue) / B(Big red) / A(Small red)"); + telemetry.update(); + return; + } + + telemetry.update(); + + if (gamepad1.y) { + startPose = new Pose(21.350906095551892, 124.7841845140033, Math.toRadians(138)); + shootPose = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose = new Pose(47.44645799011532, 85.4036243822076, Math.toRadians(-180)); + intakePose = new Pose(26.332784184514, 91.57166392092257, Math.toRadians(-180)); + launchPose1 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose2 = new Pose(72.1203212890625, 65.63519836425781, Math.toRadians(-180)); + intakePose2 = new Pose(17.31795716639209, 61.680395387149915, Math.toRadians(-180)); + launchPose2 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + preloadPose3 = new Pose(61.205761230468745, 37.861437866210935, Math.toRadians(-180)); + intakePose3 = new Pose(19.453047775947276, 43.6507413509061, Math.toRadians(-180)); + launchPose3 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + finishPose = new Pose(35.110378912685334, 71.40691927512356, Math.toRadians(-180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.b) { + startPose = new Pose( + 122.64909390444811, 124.7841845140033, Math.toRadians(40)); + + shootPose = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); + + preloadPose = new Pose( + 96.55354200988468, 85.4036243822076, Math.toRadians(0)); + + intakePose = new Pose( + 138.16, 95.92, Math.toRadians(0)); + + launchPose1 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); + + preloadPose2 = new Pose( + 88.56, 62.64000000000001, Math.toRadians(0)); + + intakePose2 = new Pose( + 142.6, 68.599999999999994, Math.toRadians(0)); + + launchPose2 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); +//10 contrl z's remove a 8 in preload p 3 + preloadPose3 = new Pose( + 90.79423876953126, 37.861437866210935, Math.toRadians(0)); + + intakePose3 = new Pose( + 138.8, 39.6, Math.toRadians(0)); + + launchPose3 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); + + finishPose = new Pose( + 108.88962108731467, 71.40691927512356, Math.toRadians(0)); + + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.a) { + startPose = new Pose(79.90752160644531, 8.715837280273437, Math.toRadians(90)); + + shootPose = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48 )); + + preloadPose = new Pose( + 96.55354200988468, 85.4036243822076, Math.toRadians(0)); + + intakePose = new Pose( + 128.16, 85.92, Math.toRadians(0)); + + launchPose1 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose2 = new Pose( + 88.56, 62.64000000000001, Math.toRadians(0)); + + intakePose2 = new Pose( + 129.6, 63.599999999999994, Math.toRadians(0)); + + launchPose2 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose3 = new Pose( + 82.79423876953126, 37.861437866210935, Math.toRadians(0)); + + intakePose3 = new Pose( + 130.8, 39.6, Math.toRadians(0)); + + launchPose3 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + finishPose = new Pose( + 108.88962108731467, 71.40691927512356, Math.toRadians(0)); + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.x) { + startPose = new Pose(63.87744128417969, 7.987197265625001, Math.toRadians(90)); + shootPose = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose = new Pose(47.44645799011532, 85.4036243822076, Math.toRadians(-180)); + intakePose = new Pose(26.332784184514, 91.57166392092257, Math.toRadians(-180)); + launchPose1 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose2 = new Pose(64.1203212890625, 62.63519836425781, Math.toRadians(-180)); + intakePose2 = new Pose(23.960461285008236, 63.57825370675453, Math.toRadians(-180)); + launchPose2 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + preloadPose3 = new Pose(61.205761230468745, 37.861437866210935, Math.toRadians(-180)); + intakePose3 = new Pose(20.401976935749587, 41.75288303130149, Math.toRadians(-180)); + launchPose3 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + finishPose = new Pose(35.110378912685334, 71.40691927512356, Math.toRadians(-180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + } + + private void buildPaths() { + + driveStartPosShootPos = follower.pathBuilder() + .addPath(new BezierLine(startPose, shootPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), shootPose.getHeading()) + .build(); + + shootToPrePickup = follower.pathBuilder() + .addPath(new BezierLine(shootPose, preloadPose)) + .setLinearHeadingInterpolation(shootPose.getHeading(), preloadPose.getHeading()) + .build(); + + INTAKE = follower.pathBuilder() + .addPath(new BezierLine(preloadPose, intakePose)) + .setLinearHeadingInterpolation(preloadPose.getHeading(), intakePose.getHeading()) + .build(); + + LAUNCH_PICKUP_1 = follower.pathBuilder() + .addPath(new BezierLine(intakePose, launchPose1)) + .setLinearHeadingInterpolation(intakePose.getHeading(), launchPose1.getHeading()) + .build(); + + SHOOT_TO_PREPICKUP2 = follower.pathBuilder() + .addPath(new BezierLine(launchPose1, preloadPose2)) + .setLinearHeadingInterpolation(launchPose1.getHeading(), preloadPose2.getHeading()) + .build(); + + INTAKEPOS2 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose2, intakePose2)) + .setLinearHeadingInterpolation(preloadPose2.getHeading(), intakePose2.getHeading()) + .build(); + + LAUNCHPOSE2 = follower.pathBuilder() + .addPath(new BezierLine(intakePose2, launchPose2)) + .setLinearHeadingInterpolation(intakePose2.getHeading(), launchPose2.getHeading()) + .build(); + + PRELOADPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(launchPose2, preloadPose3)) + .setLinearHeadingInterpolation(launchPose2.getHeading(), preloadPose3.getHeading()) + .build(); + + INTAKEPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose3, intakePose3)) + .setLinearHeadingInterpolation(preloadPose3.getHeading(), intakePose3.getHeading()) + .build(); + + LAUNCHPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(intakePose3, launchPose3)) + .setLinearHeadingInterpolation(intakePose3.getHeading(), launchPose3.getHeading()) + .build(); + + FINISH = follower.pathBuilder() + .addPath(new BezierLine(launchPose3, finishPose)) + .setLinearHeadingInterpolation(launchPose3.getHeading(), finishPose.getHeading()) + .build(); + } + + private void statePathUpdate() { + if (pathState == null) + return; + + double shooterVelocity = -1150; + intake.setPower(-1); + + + switch (pathState) { + + case DRIVE_STARPOS_SHOOT_POS: + startPath(driveStartPosShootPos); + leftPusher.setPower(0); + rightPusher.setPower(0); + + if (!follower.isBusy()) { + shooter.setVelocity(shooterVelocity); + intake.setPower(-1); + if (shooter.getVelocity() <= -25) { + if (pathTimer.getElapsedTimeSeconds() >= 1) { + leftPusher.setPower(1); + } + if (pathTimer.getElapsedTimeSeconds() >= 1) { /// ///////////////////////////////////////////////////// + shooter.setVelocity(shooterVelocity); + rightPusher.setPower(-1); + } + } + if (pathTimer.getElapsedTimeSeconds() >= 8) { + setPathState(StateMachine.SHOOT_TO_PREPICKUP); + } + } + break; + + case SHOOT_TO_PREPICKUP: + startPath(shootToPrePickup); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE); + break; + + case INTAKE: + startPath(INTAKE); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_1); + break; + + case LAUNCH_PICKUP_1: + startPath(LAUNCH_PICKUP_1); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.5) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP2); + } + } + break; + + case SHOOT_TO_PREPICKUP2: + startPath(SHOOT_TO_PREPICKUP2); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE2); + break; + + case INTAKE2: + startPath(INTAKEPOS2); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_2); + break; + + case LAUNCH_PICKUP_2: + startPath(LAUNCHPOSE2); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.2) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP3); + } + } + break; + + case SHOOT_TO_PREPICKUP3: + startPath(PRELOADPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE3); + break; + + case INTAKE3: + startPath(INTAKEPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_3); + break; + + case LAUNCH_PICKUP_3: + startPath(LAUNCHPOSE3); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.85) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.25) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.FINISH); + } + } + break; + + case FINISH: + if (!pathStarted) follower.followPath(FINISH, true); + pathStarted = true; + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + break; + } + } + + private void startPath(PathChain path) { + if (!pathStarted) { + follower.followPath(path, true); + pathStarted = true; + pathTimer.resetTimer(); + } + } + + private void runIntakeWhileBusy() { + intake.setPower(-1); + } + + private void runShooterPusherWait(StateMachine nextState) { + if (pathTimer.getElapsedTimeSeconds() >= 5) { + setPathState(nextState); + } + } + + private void setPathState(StateMachine newState) { + pathState = newState; + pathStarted = false; + pathTimer.resetTimer(); + } + + @Override + public void loop() { + follower.update(); + statePathUpdate(); + + telemetry.addData("STATE", pathState); + + Pose pose = follower.getPose(); + if (pose != null) { + telemetry.addData("x", pose.getX()); + telemetry.addData("y", pose.getY()); + telemetry.addData("heading", pose.getHeading()); + } + + telemetry.addData("followerBusy", follower.isBusy()); + telemetry.addData("pathTimer", pathTimer.getElapsedTimeSeconds()); + telemetry.update(); + } +} diff --git a/TeamCode/TeamCode/TrialAutonomous_DBG.java b/TeamCode/TeamCode/TrialAutonomous_DBG.java new file mode 100644 index 000000000000..27b6802085e2 --- /dev/null +++ b/TeamCode/TeamCode/TrialAutonomous_DBG.java @@ -0,0 +1,479 @@ + +package org.firstinspires.ftc.teamcode.teamcode; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +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 com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; + +@Autonomous +public class TrialAutonomous_DBG extends OpMode { + + DcMotorEx shooter; + DcMotor intake; + CRServo leftPusher; + CRServo rightPusher; + + Follower follower; + Timer pathTimer; + Timer timer; + + boolean pathStarted; + boolean pathsBuilt = false; + + enum StateMachine { + DRIVE_STARPOS_SHOOT_POS, + SHOOT_TO_PREPICKUP, + INTAKE, + LAUNCH_PICKUP_1, + SHOOT_TO_PREPICKUP2, + INTAKE2, + LAUNCH_PICKUP_2, + SHOOT_TO_PREPICKUP3, + INTAKE3, + LAUNCH_PICKUP_3, + FINISH; + + } + + StateMachine pathState; + + private Pose startPose; + private Pose shootPose; + private Pose preloadPose; + private Pose intakePose; + private Pose launchPose1; + + private Pose preloadPose2; + private Pose intakePose2; + private Pose launchPose2; + + private Pose preloadPose3; + private Pose intakePose3; + private Pose launchPose3; + + private Pose finishPose; + boolean a; + boolean x; + + + + + private PathChain driveStartPosShootPos, shootToPrePickup, INTAKE, LAUNCH_PICKUP_1, SHOOT_TO_PREPICKUP2, + INTAKEPOS2, LAUNCHPOSE2, PRELOADPOSE3, INTAKEPOSE3, LAUNCHPOSE3, FINISH; + + @Override + public void init() { + + pathState = StateMachine.DRIVE_STARPOS_SHOOT_POS; + + pathTimer = new Timer(); + timer = new Timer(); + pathTimer.resetTimer(); + + + + follower = Constants.createFollower(hardwareMap); + + shooter = hardwareMap.get(DcMotorEx.class, "shooter"); + intake = hardwareMap.get(DcMotor .class, "intake"); + leftPusher = hardwareMap.get(CRServo.class, "leftPusher"); + rightPusher = hardwareMap.get(CRServo.class, "rightPusher"); + + pathStarted = false; + + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + } + public void start () { + timer.resetTimer(); + } + + @Override + public void init_loop() { + if (!pathsBuilt) { + InitializePoseValues(); + } + } + + private void InitializePoseValues() { + + if (gamepad1.y) { + telemetry.addLine("Big blue selected"); + } + else if (gamepad1.b) { + telemetry.addLine("Big red selected"); + } + else if (gamepad1.a) { + telemetry.addLine("Small red selected"); + } + else if (gamepad1.x) { + telemetry.addLine("Small blue selected"); + } + else { + telemetry.addLine("Select auto mode by pressing: " + + "X(Small blue) / Y(Big blue) / B(Big red) / A(Small red)"); + telemetry.update(); + return; + } + + telemetry.update(); + + if (gamepad1.y) { + startPose = new Pose(21.350906095551892, 124.7841845140033, Math.toRadians(138)); + shootPose = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(138)); + preloadPose = new Pose(47.44645799011532, 85.4036243822076, Math.toRadians(-180)); + intakePose = new Pose(26.332784184514, 91.57166392092257, Math.toRadians(-180)); + launchPose1 = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(138)); + preloadPose2 = new Pose(64.1203212890625, 62.63519836425781, Math.toRadians(-180)); + intakePose2 = new Pose(17.31795716639209, 61.680395387149915, Math.toRadians(-180)); + launchPose2 = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(132)); + preloadPose3 = new Pose(61.205761230468745, 37.861437866210935, Math.toRadians(-180)); + intakePose3 = new Pose(19.453047775947276, 43.6507413509061, Math.toRadians(-180)); + launchPose3 = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(132)); + finishPose = new Pose(35.110378912685334, 71.40691927512356, Math.toRadians(-180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.b) { + startPose = new Pose( + 122.64909390444811, 124.7841845140033, Math.toRadians(48)); + + shootPose = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose = new Pose( + 96.35555555555555, 95.11111111111111, Math.toRadians(0)); + + intakePose = new Pose( + 136.88888888888889, 95.46666666666667, Math.toRadians(0)); + + launchPose1 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose2 = new Pose( + 86.39999999999999, 71.82222222222222, Math.toRadians(0)); + + intakePose2 = new Pose( + 136.17777777777778, 72, Math.toRadians(0)); + + launchPose2 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose3 = new Pose( + 84.08888888888889, 47.28888888888889, Math.toRadians(0)); + + intakePose3 = new Pose( + 143.2888888888889, 47.28888888888889, Math.toRadians(0)); + + launchPose3 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + finishPose = new Pose( + 109.15555555555555, 84.08888888888889, Math.toRadians(0)); + + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.a) { + a = true; + startPose = new Pose( + 78.4, + 8.000000000000004, + Math.toRadians(0) + ); + + shootPose = new Pose( + 72.17777777777778, + 23.28888888888889, + Math.toRadians(70) + ); + + preloadPose = new Pose( + 102.93333333333334, + 10.422222222222222, + Math.toRadians(0) + ); + + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.x) { + x = true; + startPose = new Pose(65.6, 8.000000000000004, Math.toRadians(90)); + shootPose = new Pose(71.82222222222222, 23.28888888888889, Math.toRadians(110)); + preloadPose = new Pose(41.06666666666667, 9.422222222222222, Math.toRadians(180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + } + + private void buildPaths() { + + driveStartPosShootPos = follower.pathBuilder() + .addPath(new BezierLine(startPose, shootPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), shootPose.getHeading()) + .build(); + + shootToPrePickup = follower.pathBuilder() + .addPath(new BezierLine(shootPose, preloadPose)) + .setLinearHeadingInterpolation(shootPose.getHeading(), preloadPose.getHeading()) + .build(); + + INTAKE = follower.pathBuilder() + .addPath(new BezierLine(preloadPose, intakePose)) + .setLinearHeadingInterpolation(preloadPose.getHeading(), intakePose.getHeading()) + .build(); + + LAUNCH_PICKUP_1 = follower.pathBuilder() + .addPath(new BezierLine(intakePose, launchPose1)) + .setLinearHeadingInterpolation(intakePose.getHeading(), launchPose1.getHeading()) + .build(); + + SHOOT_TO_PREPICKUP2 = follower.pathBuilder() + .addPath(new BezierLine(launchPose1, preloadPose2)) + .setLinearHeadingInterpolation(launchPose1.getHeading(), preloadPose2.getHeading()) + .build(); + + INTAKEPOS2 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose2, intakePose2)) + .setLinearHeadingInterpolation(preloadPose2.getHeading(), intakePose2.getHeading()) + .build(); + + LAUNCHPOSE2 = follower.pathBuilder() + .addPath(new BezierLine(intakePose2, launchPose2)) + .setLinearHeadingInterpolation(intakePose2.getHeading(), launchPose2.getHeading()) + .build(); + + PRELOADPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(launchPose2, preloadPose3)) + .setLinearHeadingInterpolation(launchPose2.getHeading(), preloadPose3.getHeading()) + .build(); + + INTAKEPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose3, intakePose3)) + .setLinearHeadingInterpolation(preloadPose3.getHeading(), intakePose3.getHeading()) + .build(); + + LAUNCHPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(intakePose3, launchPose3)) + .setLinearHeadingInterpolation(intakePose3.getHeading(), launchPose3.getHeading()) + .build(); + + FINISH = follower.pathBuilder() + .addPath(new BezierLine(launchPose3, finishPose)) + .setLinearHeadingInterpolation(launchPose3.getHeading(), finishPose.getHeading()) + .build(); + } + + private void statePathUpdate() { + double shooterVelocity; + + if (pathState == null) + return; + + if (x ^ a) { + shooterVelocity = -1990; + } + else { + shooterVelocity = -1150; + } + + + intake.setPower(-1); + + + switch (pathState) { + + case DRIVE_STARPOS_SHOOT_POS: + startPath(driveStartPosShootPos); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(shooterVelocity); + + if (!follower.isBusy()) { + if (shooter.getVelocity() <= shooterVelocity) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 4) { + shooter.setVelocity(shooterVelocity); + leftPusher.setPower(1); + } + } + if (pathTimer.getElapsedTimeSeconds() >= 8.5) { + setPathState(StateMachine.SHOOT_TO_PREPICKUP); + } + + + break; + + case SHOOT_TO_PREPICKUP: + startPath(shootToPrePickup); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy() && !x ^ !a) { + setPathState(StateMachine.INTAKE); + } + break; + + case INTAKE: + startPath(INTAKE); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_1); + break; + + case LAUNCH_PICKUP_1: + startPath(LAUNCH_PICKUP_1); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.5) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP2); + } + + + + } + break; + + case SHOOT_TO_PREPICKUP2: + startPath(SHOOT_TO_PREPICKUP2); // ✅ ONLY FIX + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE2); + break; + + case INTAKE2: + startPath(INTAKEPOS2); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_2); + break; + + case LAUNCH_PICKUP_2: + startPath(LAUNCHPOSE2); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.2) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP3); + } + } + break; + + case SHOOT_TO_PREPICKUP3: + startPath(PRELOADPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE3); + break; + + case INTAKE3: + startPath(INTAKEPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_3); + break; + + case LAUNCH_PICKUP_3: + startPath(LAUNCHPOSE3); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.85) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.25) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.FINISH); + } + } + break; + + case FINISH: + if (!pathStarted) follower.followPath(FINISH, true); + pathStarted = true; + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + break; + } + } + + private void startPath(PathChain path) { + if (!pathStarted) { + follower.followPath(path, true); + pathStarted = true; + pathTimer.resetTimer(); + } + } + + private void runIntakeWhileBusy() { + intake.setPower(-1); + } + + private void runShooterPusherWait(StateMachine nextState) { + if (pathTimer.getElapsedTimeSeconds() >= 5) { + setPathState(nextState); + } + } + + private void setPathState(StateMachine newState) { + pathState = newState; + pathStarted = false; + pathTimer.resetTimer(); + } + + @Override + public void loop() { + follower.update(); + statePathUpdate(); + + telemetry.addData("STATE", pathState); + + Pose pose = follower.getPose(); + if (pose != null) { + telemetry.addData("x", pose.getX()); + telemetry.addData("y", pose.getY()); + telemetry.addData("heading", pose.getHeading()); + } + + telemetry.addData("followerBusy", follower.isBusy()); + telemetry.addData("pathTimer", pathTimer.getElapsedTimeSeconds()); + telemetry.addData("timer", timer.getElapsedTimeSeconds()); + telemetry.update(); + } +} \ No newline at end of file diff --git a/TeamCode/TeamCode/lib/OpModeAnnotationProcessor.jar b/TeamCode/TeamCode/lib/OpModeAnnotationProcessor.jar new file mode 100644 index 000000000000..4825cc36d960 Binary files /dev/null and b/TeamCode/TeamCode/lib/OpModeAnnotationProcessor.jar differ diff --git a/TeamCode/TeamCode/pedroPathing/Constants.java b/TeamCode/TeamCode/pedroPathing/Constants.java new file mode 100644 index 000000000000..84e8bd15ffad --- /dev/null +++ b/TeamCode/TeamCode/pedroPathing/Constants.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.teamcode.pedroPathing; + +import com.pedropathing.control.FilteredPIDFCoefficients; +import com.pedropathing.control.PIDFCoefficients; +import com.pedropathing.follower.Follower; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; +import com.pedropathing.ftc.localization.constants.PinpointConstants; +import com.pedropathing.paths.PathConstraints; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +public class Constants { + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(10.6) + .forwardZeroPowerAcceleration(-42.51297137628084) + .lateralZeroPowerAcceleration(-53.639708599274925) + .translationalPIDFCoefficients(new PIDFCoefficients(0.03, 0, 0.03, 0.035)) + .headingPIDFCoefficients(new PIDFCoefficients (1, 0, 0.022, 0.025)) + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.0075, 0, 0, 0.6, 0.025)) + .centripetalScaling(0.001); + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(1) + .rightFrontMotorName("rightFront") + .rightRearMotorName("rightBack") + .leftRearMotorName("leftBack") + .leftFrontMotorName("leftFront") + .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE) + .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) + .xVelocity(61.837623896561276) + .yVelocity(52.13752806656005); + + public static PinpointConstants localizerConstants = new PinpointConstants() + .forwardPodY(-6) + .strafePodX(3.5) + .distanceUnit(DistanceUnit.INCH) + .hardwareMapName("imu2") + .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) + .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED); + + + + public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); + + public static Follower createFollower(HardwareMap hardwareMap) { + return new FollowerBuilder(followerConstants, hardwareMap) + .pinpointLocalizer(localizerConstants) + .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) + .build(); + } +} diff --git a/TeamCode/TeamCode/pedroPathing/Tuning.java b/TeamCode/TeamCode/pedroPathing/Tuning.java new file mode 100644 index 000000000000..633d8d977ed4 --- /dev/null +++ b/TeamCode/TeamCode/pedroPathing/Tuning.java @@ -0,0 +1,1357 @@ +package org.firstinspires.ftc.teamcode.teamcode.pedroPathing; + +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.draw; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawOnlyCurrent; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM; + +import com.bylazar.configurables.PanelsConfigurables; +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.configurables.annotations.IgnoreConfigurable; +import com.bylazar.field.FieldManager; +import com.bylazar.field.PanelsField; +import com.bylazar.field.Style; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.math.Vector; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.pedropathing.telemetry.SelectableOpMode; +import com.pedropathing.util.PoseHistory; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; + +import java.util.ArrayList; +import java.util.List; + +/** + * This is the Tuning class. It contains a selection menu for various tuning OpModes. + * + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 6/26/2025 + */ +@Configurable +@TeleOp(name = "Tuning", group = "Pedro Pathing") +public class Tuning extends SelectableOpMode { + public static Follower follower; + + @IgnoreConfigurable + static PoseHistory poseHistory; + + @IgnoreConfigurable + static TelemetryManager telemetryM; + + @IgnoreConfigurable + static ArrayList changes = new ArrayList<>(); + + public Tuning() { + super("Select a Tuning OpMode", s -> { + s.folder("Localization", l -> { + l.add("Localization Test", LocalizationTest::new); + l.add("Forward Tuner", ForwardTuner::new); + l.add("Lateral Tuner", LateralTuner::new); + l.add("Turn Tuner", TurnTuner::new); + }); + s.folder("Automatic", a -> { + a.add("Forward Velocity Tuner", ForwardVelocityTuner::new); + a.add("Lateral Velocity Tuner", LateralVelocityTuner::new); + a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new); + a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new); + }); + s.folder("Manual", p -> { + p.add("Translational Tuner", TranslationalTuner::new); + p.add("Heading Tuner", HeadingTuner::new); + p.add("Drive Tuner", DriveTuner::new); + p.add("Line Tuner", Line::new); + p.add("Centripetal Tuner", CentripetalTuner::new); + }); + s.folder("Tests", p -> { + p.add("Line", Line::new); + p.add("Triangle", Triangle::new); + p.add("Circle", Circle::new); + }); + }); + } + + @Override + public void onSelect() { + if (follower == null) { + follower = org.firstinspires.ftc.teamcode.pedroPathing.Constants.createFollower(hardwareMap); + PanelsConfigurables.INSTANCE.refreshClass(this); + } else { + follower = Constants.createFollower(hardwareMap); + } + + follower.setStartingPose(new Pose()); + + poseHistory = follower.getPoseHistory(); + + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + + Drawing.init(); + } + + @Override + public void onLog(List lines) {} + + public static void drawOnlyCurrent() { + try { + Drawing.drawRobot(follower.getPose()); + Drawing.sendPacket(); + } catch (Exception e) { + throw new RuntimeException("Drawing failed " + e); + } + } + + public static void draw() { + Drawing.drawDebug(follower); + } + + /** This creates a full stop of the robot by setting the drive motors to run at 0 power. */ + public static void stopRobot() { + follower.startTeleopDrive(true); + follower.setTeleOpDrive(0,0,0,true); + } +} + +/** + * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a + * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot. + * You should use this to check the robot's localization. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class LocalizationTest extends OpMode { + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + } + + /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("This will print your robot's position to telemetry while " + + "allowing robot control through a basic mecanum drive on gamepad 1."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.startTeleopDrive(); + follower.update(); + } + + /** + * This updates the robot's pose estimate, the simple mecanum drive, and updates the + * Panels telemetry with the robot's position as well as draws the robot's position. + */ + @Override + public void loop() { + follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); + follower.update(); + + telemetryM.debug("x:" + follower.getPose().getX()); + telemetryM.debug("y:" + follower.getPose().getY()); + telemetryM.debug("heading:" + follower.getPose().getHeading()); + telemetryM.debug("total heading:" + follower.getTotalHeading()); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the forward ticks to inches in your + * localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class ForwardTuner extends OpMode { + public static double DISTANCE = 48; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Distance Moved: " + follower.getPose().getX()); + telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryM.debug("Multiplier: " + (DISTANCE / ((follower.getPose().getX() - 72) / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the strafe ticks to inches in your + * localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 2.0, 6/26/2025 + */ +class LateralTuner extends OpMode { + public static double DISTANCE = 48; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Distance Moved: " + follower.getPose().getY()); + telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryM.debug("Multiplier: " + (DISTANCE / ((follower.getPose().getY() - 72) / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current angle in ticks to the specified angle in radians. So, to use this, run the + * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground. + * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run + * multiple trials and average the results. Then, input the multiplier into the turning ticks to + * radians in your localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class TurnTuner extends OpMode { + public static double ANGLE = 2 * Math.PI; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Total Angle: " + follower.getTotalHeading()); + telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); + telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the ForwardVelocityTuner autonomous follower OpMode. This runs the robot forwards at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class ForwardVelocityTuner extends OpMode { + private final ArrayList velocities = new ArrayList<>(); + public static double DISTANCE = 48; + public static double RECORD_NUMBER = 10; + + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); + telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); + telemetryM.debug("Press B on game pad 1 to stop."); + telemetryM.debug("pose", follower.getPose()); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + follower.startTeleopDrive(true); + follower.update(); + end = false; + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run forward enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + + if (!end) { + if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) { + end = true; + stopRobot(); + } else { + follower.setTeleOpDrive(1,0,0,true); + //double currentVelocity = Math.abs(follower.getVelocity().getXComponent()); + double currentVelocity = Math.abs(follower.poseTracker.getLocalizer().getVelocity().getX()); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + stopRobot(); + double average = 0; + for (double velocity : velocities) { + average += velocity; + } + average /= velocities.size(); + telemetryM.debug("Forward Velocity: " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on)."); + + for (int i = 0; i < velocities.size(); i++) { + telemetry.addData(String.valueOf(i), velocities.get(i)); + } + + telemetryM.update(telemetry); + telemetry.update(); + + if (gamepad1.aWasPressed()) { + follower.setXVelocity(average); + String message = "XMovement: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the StrafeVelocityTuner autonomous follower OpMode. This runs the robot left at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class LateralVelocityTuner extends OpMode { + private final ArrayList velocities = new ArrayList<>(); + + public static double DISTANCE = 48; + public static double RECORD_NUMBER = 10; + + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the drive motors as well as the cache of velocities and the Panels + * telemetryM. + */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the left."); + telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); + telemetryM.debug("Press B on Gamepad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run left at full power. */ + @Override + public void start() { + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + follower.startTeleopDrive(true); + follower.update(); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run sideways enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + if (!end) { + if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) { + end = true; + stopRobot(); + } else { + follower.setTeleOpDrive(0,1,0,true); + double currentVelocity = Math.abs(follower.getVelocity().dot(new Vector(1, Math.PI / 2))); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + stopRobot(); + double average = 0; + for (double velocity : velocities) { + average += velocity; + } + average /= velocities.size(); + + telemetryM.debug("Strafe Velocity: " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.setYVelocity(average); + String message = "YMovement: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the ForwardZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot + * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +class ForwardZeroPowerAccelerationTuner extends OpMode { + private final ArrayList accelerations = new ArrayList<>(); + public static double VELOCITY = 30; + + private double previousVelocity; + private long previousTimeNano; + + private boolean stopping; + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the Panels telemetryM. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second."); + telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); + telemetryM.debug("Press B on Gamepad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + follower.startTeleopDrive(false); + follower.update(); + follower.setTeleOpDrive(1,0,0,true); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + Vector heading = new Vector(1.0, follower.getPose().getHeading()); + if (!end) { + if (!stopping) { + if (follower.getVelocity().dot(heading) > VELOCITY) { + previousVelocity = follower.getVelocity().dot(heading); + previousTimeNano = System.nanoTime(); + stopping = true; + follower.setTeleOpDrive(0,0,0,true); + } + } else { + double currentVelocity = follower.getVelocity().dot(heading); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { + end = true; + } + } + } else { + double average = 0; + for (double acceleration : accelerations) { + average += acceleration; + } + average /= accelerations.size(); + + telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.getConstants().setForwardZeroPowerAcceleration(average); + String message = "Forward Zero Power Acceleration: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the LateralZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot + * to the left until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class LateralZeroPowerAccelerationTuner extends OpMode { + private final ArrayList accelerations = new ArrayList<>(); + public static double VELOCITY = 30; + private double previousVelocity; + private long previousTimeNano; + private boolean stopping; + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run to the left until it reaches " + VELOCITY + " inches per second."); + telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); + telemetryM.debug("Press B on game pad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + follower.startTeleopDrive(false); + follower.update(); + follower.setTeleOpDrive(0,1,0,true); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + Vector heading = new Vector(1.0, follower.getPose().getHeading() - Math.PI / 2); + if (!end) { + if (!stopping) { + if (Math.abs(follower.getVelocity().dot(heading)) > VELOCITY) { + previousVelocity = Math.abs(follower.getVelocity().dot(heading)); + previousTimeNano = System.nanoTime(); + stopping = true; + follower.setTeleOpDrive(0,0,0,true); + } + } else { + double currentVelocity = Math.abs(follower.getVelocity().dot(heading)); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { + end = true; + } + } + } else { + double average = 0; + for (double acceleration : accelerations) { + average += acceleration; + } + average /= accelerations.size(); + + telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.getConstants().setLateralZeroPowerAcceleration(average); + String message = "Lateral Zero Power Acceleration: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the Translational PIDF Tuner OpMode. It will keep the robot in place. + * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class TranslationalTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the Follower and creates the forward and backward Paths. */ + @Override + public void init_loop() { + telemetryM.debug("This will activate the translational PIDF(s)"); + telemetryM.debug("The robot will try to stay in place while you push it laterally."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateTranslational(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); + telemetryM.update(telemetry); + } +} + +/** + * This is the Heading PIDF Tuner OpMode. It will keep the robot in place. + * The user should try to turn the robot to test the PIDF and adjust the PIDF values accordingly. + * It will try to keep the robot at a constant heading while the user tries to turn it. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class HeadingTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will activate the heading PIDF(s)."); + telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateHeading(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); + telemetryM.update(telemetry); + } +} + +/** + * This is the Drive PIDF Tuner OpMode. It will run the robot in a straight line going forward and back. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class DriveTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private PathChain forwards; + private PathChain backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward."); + telemetryM.debug("The robot will go forward and backward continuously along the path."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateDrive(); + + forwards = follower.pathBuilder() + .setGlobalDeceleration() + .addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))) + .setConstantHeadingInterpolation(0) + .build(); + + backwards = follower.pathBuilder() + .setGlobalDeceleration() + .addPath(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))) + .setConstantHeadingInterpolation(0) + .build(); + + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving forward?: " + forward); + telemetryM.update(telemetry); + } +} + +/** + * This is the Line Test Tuner OpMode. It will drive the robot forward and back + * The user should push the robot laterally and angular to test out the drive, heading, and translational PIDFs. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class Line extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the Follower and creates the forward and backward Paths. */ + @Override + public void init_loop() { + telemetryM.debug("This will activate all the PIDF(s)"); + telemetryM.debug("The robot will go forward and backward continuously along the path while correcting."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving Forward?: " + forward); + telemetryM.update(telemetry); + } +} + +/** + * This is the Centripetal Tuner OpMode. It runs the robot in a specified distance + * forward and to the left. On reaching the end of the forward Path, the robot runs the backward + * Path the same distance back to the start. Rinse and repeat! This is good for testing a variety + * of Vectors, like the drive Vector, the translational Vector, the heading Vector, and the + * centripetal Vector. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +class CentripetalTuner extends OpMode { + public static double DISTANCE = 20; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. + * Additionally, this initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); + telemetryM.debug("The robot will go continuously along the path."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + forwards = new Path(new BezierCurve(new Pose(72,72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72))); + backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(72,72))); + + backwards.setTangentHeadingInterpolation(); + backwards.reverseHeadingInterpolation(); + + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving away from the origin along the curve?: " + forward); + telemetryM.update(telemetry); + } +} + +/** + * This is the Triangle autonomous OpMode. + * It runs the robot in a triangle, with the starting point being the bottom-middle point. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Samarth Mahapatra - 1002 CircuitRunners Robotics Surge + * @version 1.0, 12/30/2024 + */ +class Triangle extends OpMode { + + private final Pose startPose = new Pose(72, 72, Math.toRadians(0)); + private final Pose interPose = new Pose(24 + 72, -24 + 72, Math.toRadians(90)); + private final Pose endPose = new Pose(24 + 72, 24 + 72, Math.toRadians(45)); + + private PathChain triangle; + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (follower.atParametricEnd()) { + follower.followPath(triangle, true); + } + } + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + @Override + public void init_loop() { + telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point."); + telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** Creates the PathChain for the "triangle".*/ + @Override + public void start() { + follower.setStartingPose(startPose); + + triangle = follower.pathBuilder() + .addPath(new BezierLine(startPose, interPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), interPose.getHeading()) + .addPath(new BezierLine(interPose, endPose)) + .setLinearHeadingInterpolation(interPose.getHeading(), endPose.getHeading()) + .addPath(new BezierLine(endPose, startPose)) + .setLinearHeadingInterpolation(endPose.getHeading(), startPose.getHeading()) + .build(); + + follower.followPath(triangle); + } +} + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bezier curves that have control points set essentially in a square. However, + * it turns enough to tune your centripetal force correction and some of your heading. Some lag in + * heading is to be expected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class Circle extends OpMode { + public static double RADIUS = 10; + private PathChain circle; + + public void start() { + circle = follower.pathBuilder() + .addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .build(); + follower.followPath(circle); + } + + @Override + public void init_loop() { + telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); + telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode."); + telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (follower.atParametricEnd()) { + follower.followPath(circle); + } + } +} + +/** + * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. + * + * @author Lazar - 19234 + * @version 1.1, 5/19/2025 + */ +class Drawing { + public static final double ROBOT_RADIUS = 9; // woah + private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); + + private static final Style robotLook = new Style( + "", "#3F51B5", 0.75 + ); + private static final Style historyLook = new Style( + "", "#4CAF50", 0.75 + ); + + /** + * This prepares Panels Field for using Pedro Offsets + */ + public static void init() { + panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING()); + } + + /** + * This draws everything that will be used in the Follower's telemetryDebug() method. This takes + * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * + * @param follower Pedro Follower instance. + */ + public static void drawDebug(Follower follower) { + if (follower.getCurrentPath() != null) { + drawPath(follower.getCurrentPath(), robotLook); + Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); + drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook); + } + drawPoseHistory(follower.getPoseHistory(), historyLook); + drawRobot(follower.getPose(), historyLook); + + sendPacket(); + } + + /** + * This draws a robot at a specified Pose with a specified + * look. The heading is represented as a line. + * + * @param pose the Pose to draw the robot at + * @param style the parameters used to draw the robot with + */ + public static void drawRobot(Pose pose, Style style) { + if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { + return; + } + + panelsField.setStyle(style); + panelsField.moveCursor(pose.getX(), pose.getY()); + panelsField.circle(ROBOT_RADIUS); + + Vector v = pose.getHeadingAsUnitVector(); + v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); + double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2; + double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent(); + + panelsField.setStyle(style); + panelsField.moveCursor(x1, y1); + panelsField.line(x2, y2); + } + + /** + * This draws a robot at a specified Pose. The heading is represented as a line. + * + * @param pose the Pose to draw the robot at + */ + public static void drawRobot(Pose pose) { + drawRobot(pose, robotLook); + } + + /** + * This draws a Path with a specified look. + * + * @param path the Path to draw + * @param style the parameters used to draw the Path with + */ + public static void drawPath(Path path, Style style) { + double[][] points = path.getPanelsDrawingPoints(); + + for (int i = 0; i < points[0].length; i++) { + for (int j = 0; j < points.length; j++) { + if (Double.isNaN(points[j][i])) { + points[j][i] = 0; + } + } + } + + panelsField.setStyle(style); + panelsField.moveCursor(points[0][0], points[0][1]); + panelsField.line(points[1][0], points[1][1]); + } + + /** + * This draws all the Paths in a PathChain with a + * specified look. + * + * @param pathChain the PathChain to draw + * @param style the parameters used to draw the PathChain with + */ + public static void drawPath(PathChain pathChain, Style style) { + for (int i = 0; i < pathChain.size(); i++) { + drawPath(pathChain.getPath(i), style); + } + } + + /** + * This draws the pose history of the robot. + * + * @param poseTracker the PoseHistory to get the pose history from + * @param style the parameters used to draw the pose history with + */ + public static void drawPoseHistory(PoseHistory poseTracker, Style style) { + panelsField.setStyle(style); + + int size = poseTracker.getXPositionsArray().length; + for (int i = 0; i < size - 1; i++) { + + panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]); + panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]); + } + } + + /** + * This draws the pose history of the robot. + * + * @param poseTracker the PoseHistory to get the pose history from + */ + public static void drawPoseHistory(PoseHistory poseTracker) { + drawPoseHistory(poseTracker, historyLook); + } + + /** + * This tries to send the current packet to FTControl Panels. + */ + public static void sendPacket() { + panelsField.update(); + } +} diff --git a/TeamCode/TeamCode/readme.md b/TeamCode/TeamCode/readme.md new file mode 100644 index 000000000000..4d1da42de0c0 --- /dev/null +++ b/TeamCode/TeamCode/readme.md @@ -0,0 +1,131 @@ +## TeamCode Module + +Welcome! + +This module, TeamCode, is the place where you will write/paste the code for your team's +robot controller App. This module is currently empty (a clean slate) but the +process for adding OpModes is straightforward. + +## Creating your own OpModes + +The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own. + +Sample opmodes exist in the FtcRobotController module. +To locate these samples, find the FtcRobotController module in the "Project/Android" tab. + +Expand the following tree elements: + FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples + +### Naming of Samples + +To gain a better understanding of how the samples are organized, and how to interpret the +naming system, it will help to understand the conventions that were used during their creation. + +These conventions are described (in detail) in the sample_conventions.md file in this folder. + +To summarize: A range of different samples classes will reside in the java/external/samples. +The class names will follow a naming convention which indicates the purpose of each class. +The prefix of the name will be one of the following: + +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure + of a particular style of OpMode. These are bare bones examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. + It is not intended to drive a functioning robot, it is simply showing the minimal code + required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. + It may be used to provide a common baseline driving OpMode, or + to demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function or concept. + These may be complex, but their operation should be explained clearly in the comments, + or the comments should reference an external doc, guide or tutorial. + Each OpMode should try to only demonstrate a single concept so they are easy to + locate based on their name. These OpModes may not produce a drivable robot. + +After the prefix, other conventions will apply: + +* Sensor class names are constructed as: Sensor - Company - Type +* Robot class names are constructed as: Robot - Mode - Action - OpModetype +* Concept class names are constructed as: Concept - Topic - OpModetype + +Once you are familiar with the range of samples available, you can choose one to be the +basis for your own robot. In all cases, the desired sample(s) needs to be copied into +your TeamCode module to be used. + +This is done inside Android Studio directly, using the following steps: + + 1) Locate the desired sample class in the Project/Android tree. + + 2) Right click on the sample class and select "Copy" + + 3) Expand the TeamCode/java folder + + 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" + + 5) You will be prompted for a class name for the copy. + Choose something meaningful based on the purpose of this class. + Start with a capital letter, and remember that there may be more similar classes later. + +Once your copy has been created, you should prepare it for use on your robot. +This is done by adjusting the OpMode's name, and enabling it to be displayed on the +Driver Station's OpMode list. + +Each OpMode sample class begins with several lines of code like the ones shown below: + +``` + @TeleOp(name="Template: Linear OpMode", group="Linear Opmode") + @Disabled +``` + +The name that will appear on the driver station's "opmode list" is defined by the code: + ``name="Template: Linear OpMode"`` +You can change what appears between the quotes to better describe your opmode. +The "group=" portion of the code can be used to help organize your list of OpModes. + +As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the + ``@Disabled`` annotation which has been included. +This line can simply be deleted , or commented out, to make the OpMode visible. + + + +## ADVANCED Multi-Team App management: Cloning the TeamCode Module + +In some situations, you have multiple teams in your club and you want them to all share +a common code organization, with each being able to *see* the others code but each having +their own team module with their own code that they maintain themselves. + +In this situation, you might wish to clone the TeamCode module, once for each of these teams. +Each of the clones would then appear along side each other in the Android Studio module list, +together with the FtcRobotController module (and the original TeamCode module). + +Selective Team phones can then be programmed by selecting the desired Module from the pulldown list +prior to clicking to the green Run arrow. + +Warning: This is not for the inexperienced Software developer. +You will need to be comfortable with File manipulations and managing Android Studio Modules. +These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this. + +Also.. Make a full project backup before you start this :) + +To clone TeamCode, do the following: + +Note: Some names start with "Team" and others start with "team". This is intentional. + +1) Using your operating system file management tools, copy the whole "TeamCode" + folder to a sibling folder with a corresponding new name, eg: "Team0417". + +2) In the new Team0417 folder, delete the TeamCode.iml file. + +3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder + to a matching name with a lowercase 'team' eg: "team0417". + +4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains + package="org.firstinspires.ftc.teamcode" + to be + package="org.firstinspires.ftc.team0417" + +5) Add: include ':Team0417' to the "/settings.gradle" file. + +6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" \ No newline at end of file diff --git a/TeamCode/TeamCode/src/main/AndroidManifest.xml b/TeamCode/TeamCode/src/main/AndroidManifest.xml new file mode 100644 index 000000000000..3705b3150b43 --- /dev/null +++ b/TeamCode/TeamCode/src/main/AndroidManifest.xml @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BotTwoTeleopPreset.java b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BotTwoTeleopPreset.java new file mode 100644 index 000000000000..8f99d4a72624 --- /dev/null +++ b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BotTwoTeleopPreset.java @@ -0,0 +1,295 @@ +/* + * Copyright (c) 2025 FIRST + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior + * written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import static com.qualcomm.robotcore.hardware.DcMotor.ZeroPowerBehavior.BRAKE; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +/* + * This file includes a teleop (driver-controlled) file for the goBILDA® StarterBot for the + * 2025-2026 FIRST® Tech Challenge season DECODE™. It leverages a differential/Skid-Steer + * system for robot mobility, one high-speed motor driving two "launcher wheels", and two servos + * which feed that launcher. + * + * Likely the most niche concept we'll use in this example is closed-loop motor velocity control. + * This control method reads the current speed as reported by the motor's encoder and applies a varying + * amount of power to reach, and then hold a target velocity. The FTC SDK calls this control method + * "RUN_USING_ENCODER". This contrasts to the default "RUN_WITHOUT_ENCODER" where you control the power + * applied to the motor directly. + * Since the dynamics of a launcher wheel system varies greatly from those of most other FTC mechanisms, + * we will also need to adjust the "PIDF" coefficients with some that are a better fit for our application. + */ + +@TeleOp(name = "BotTwoTeleopPreset", group = "StarterBot") +//@Disabled +public class BotTwoTeleopPreset extends OpMode { + + /* + * When we control our launcher motor, we are using encoders. These allow the control system + * to read the current speed of the motor and apply more or less power to keep it at a constant + * velocity. Here we are setting the target, and minimum velocity that the launcher should run + * at. The minimum velocity is a threshold for determining when to fire. + */ + final double LAUNCHER_TARGET_VELOCITY = 1125; + final double LAUNCHER_MIN_VELOCITY = 1075; + + // Declare OpMode members. + private DcMotor leftFrontDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor leftBackDrive = null; + private DcMotor rightBackDrive = null; + private DcMotor intake = null; + + + private DcMotor shooter = null; + + + private CRServo leftPusher = null; + + + private CRServo rightPusher = null; + + + private boolean shotTog = false; + + + + + /* + * TECH TIP: State Machines + * We use a "state machine" to control our launcher motor and feeder servos in this program. + * The first step of a state machine is creating an enum that captures the different "states" + * that our code can be in. + * The core advantage of a state machine is that it allows us to continue to loop through all + * of our code while only running specific code when it's necessary. We can continuously check + * what "State" our machine is in, run the associated code, and when we are done with that step + * move on to the next state. + * This enum is called the "LaunchState". It reflects the current condition of the shooter + * motor and we move through the enum when the user asks our code to fire a shot. + * It starts at idle, when the user requests a launch, we enter SPIN_UP where we get the + * motor up to speed, once it meets a minimum speed then it starts and then ends the launch process. + * We can use higher level code to cycle through these states. But this allows us to write + * functions and autonomous routines in a way that avoids loops within loops, and "waits". + */ + + + + // Setup a variable for each drive wheel to save power level for telemetry + double leftFrontPower; + double rightFrontPower; + double leftBackPower; + double rightBackPower; + + /* + * Code to run ONCE when the driver hits INIT + */ + @Override + public void init() { + + + /* + * Initialize the hardware variables. Note that the strings used here as parameters + * to 'get' must correspond to the names assigned during the robot configuration + * step. + */ + leftFrontDrive = hardwareMap.get(DcMotor.class, "leftFront"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "rightFront"); + leftBackDrive = hardwareMap.get(DcMotor.class, "leftBack"); + rightBackDrive = hardwareMap.get(DcMotor.class, "rightBack"); + intake = hardwareMap.get(DcMotor.class, "intake"); + shooter = hardwareMap.get(DcMotor.class, "shooter"); + leftPusher = hardwareMap.get(CRServo.class, "leftPusher"); + rightPusher = hardwareMap.get(CRServo.class, "rightPusher"); + + + /* + * To drive forward, most robots need the motor on one side to be reversed, + * because the axles point in opposite directions. Pushing the left stick forward + * MUST make robot go forward. So adjust these two lines based on your first test drive. + * Note: The settings here assume direct drive on left and right wheels. Gear + * Reduction or 90 Deg drives may require direction flips + */ + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + /* + * Here we set our launcher to the RUN_USING_ENCODER runmode. + * If you notice that you have no control over the velocity of the motor, it just jumps + * right to a number much higher than your set point, make sure that your encoders are plugged + * into the port right beside the motor itself. And that the motors polarity is consistent + * through any wiring. + */ + + /* + * Setting zeroPowerBehavior to BRAKE enables a "brake mode". This causes the motor to + * slow down much faster when it is coasting. This creates a much more controllable + * drivetrain. As the robot stops much quicker. + */ + leftFrontDrive.setZeroPowerBehavior(BRAKE); + rightFrontDrive.setZeroPowerBehavior(BRAKE); + leftBackDrive.setZeroPowerBehavior(BRAKE); + rightBackDrive.setZeroPowerBehavior(BRAKE); + + /* + * set Feeders to an initial value to initialize the servo controller + */ + + + + + /* + * Much like our drivetrain motors, we set the left feeder servo to reverse so that they + * both work to feed the ball into the robot. + */ + + + /* + * Tell the driver that initialization is complete. + */ + telemetry.addData("Status", "Initialized"); + } + + /* + * Code to run REPEATEDLY after the driver hits INIT, but before they hit START + */ + @Override + public void init_loop() { + } + + /* + * Code to run ONCE when the driver hits START + */ + @Override + public void start() { + } + + /* + * Code to run REPEATEDLY after the driver hits START but before they hit STOP + */ + @Override + public void loop() { + + + if(gamepad2.a) { + shotTog = true; + } + else if (gamepad2.b) { + shotTog = false; + } + + if (shotTog) { + shooter.setPower(-0.55); + } + else { + shooter.setPower(0); + } + //else if (gamepad2.y) { + // shooter.setPower(-.65); + // } + //else if(gamepad2.b) { + // shooter.setPower(-0.8); + // } + // else if(gamepad2.a) { + // shooter.setPower(-1); + //} + + + + + if (gamepad1.right_bumper){ + intake.setPower(-1); + } + else{ + intake.setPower(0); + } + + if (gamepad2.left_bumper){ + leftPusher.setPower(1); + } + else{ + leftPusher.setPower(0); + } + + if(gamepad2.right_bumper){ + rightPusher.setPower(-1); + } + else{ + rightPusher.setPower(0); + } + + + mecanumDrive(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + } + + /* + * Here we give the user control of the speed of the launcher motor without automatically + * queuing a shot. + */ + + + /* + * Code to run ONCE after the driver hits STOP + */ + @Override + public void stop() { + } + + void mecanumDrive(double forward, double strafe, double rotate){ + + /* the denominator is the largest motor power (absolute value) or 1 + * This ensures all the powers maintain the same ratio, + * but only if at least one is out of the range [-1, 1] + */ + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(rotate), 1); + + leftFrontPower = (forward + strafe + rotate) / denominator; + rightFrontPower = (forward - strafe - rotate) / denominator; + leftBackPower = (forward - strafe + rotate) / denominator; + rightBackPower = (forward + strafe - rotate) / denominator; + + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + + } + + +} diff --git a/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous.java b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous.java new file mode 100644 index 000000000000..b98c1f1e28c8 --- /dev/null +++ b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous.java @@ -0,0 +1,474 @@ +package org.firstinspires.ftc.teamcode; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.pedropathing.util.Timer; + +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; + +@Autonomous +public class TrialAutonomous extends OpMode { + + DcMotorEx shooter; + DcMotor intake; + CRServo leftPusher; + CRServo rightPusher; + + Follower follower; + Timer pathTimer; + + boolean pathStarted; + boolean pathsBuilt = false; + + enum StateMachine { + DRIVE_STARPOS_SHOOT_POS, + SHOOT_TO_PREPICKUP, + INTAKE, + LAUNCH_PICKUP_1, + SHOOT_TO_PREPICKUP2, + INTAKE2, + LAUNCH_PICKUP_2, + SHOOT_TO_PREPICKUP3, + INTAKE3, + LAUNCH_PICKUP_3, + FINISH; + + } + + StateMachine pathState; + + private Pose startPose; + private Pose shootPose; + private Pose preloadPose; + private Pose intakePose; + private Pose launchPose1; + + private Pose preloadPose2; + private Pose intakePose2; + private Pose launchPose2; + + private Pose preloadPose3; + private Pose intakePose3; + private Pose launchPose3; + + private Pose finishPose; + + + + + private PathChain driveStartPosShootPos, shootToPrePickup, INTAKE, LAUNCH_PICKUP_1, SHOOT_TO_PREPICKUP2, + INTAKEPOS2, LAUNCHPOSE2, PRELOADPOSE3, INTAKEPOSE3, LAUNCHPOSE3, FINISH; + + @Override + public void init() { + + pathState = StateMachine.DRIVE_STARPOS_SHOOT_POS; + + pathTimer = new Timer(); + pathTimer.resetTimer(); + + follower = Constants.createFollower(hardwareMap); + + shooter = hardwareMap.get(DcMotorEx.class, "shooter"); + intake = hardwareMap.get(DcMotor.class, "intake"); + leftPusher = hardwareMap.get(CRServo.class, "leftPusher"); + rightPusher = hardwareMap.get(CRServo.class, "rightPusher"); + + pathStarted = false; + + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + } + + @Override + public void init_loop() { + if (!pathsBuilt) { + InitializePoseValues(); + } + } + + private void InitializePoseValues() { + + if (gamepad1.y) telemetry.addLine("Big blue selected"); + else if (gamepad1.b) telemetry.addLine("Big red selected"); + else if (gamepad1.a) telemetry.addLine("Small red selected"); + else if (gamepad1.x) telemetry.addLine("Small blue selected"); + else if (gamepad1.right_bumper && gamepad1.x) { + telemetry.addLine("Backup selected "); + + } else { + telemetry.addLine("Select auto mode by pressing: " + + "X(Small blue) / Y(Big blue) / B(Big red) / A(Small red)"); + telemetry.update(); + return; + } + + telemetry.update(); + + if (gamepad1.y) { + startPose = new Pose(21.350906095551892, 124.7841845140033, Math.toRadians(138)); + shootPose = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose = new Pose(47.44645799011532, 85.4036243822076, Math.toRadians(-180)); + intakePose = new Pose(26.332784184514, 91.57166392092257, Math.toRadians(-180)); + launchPose1 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose2 = new Pose(72.1203212890625, 65.63519836425781, Math.toRadians(-180)); + intakePose2 = new Pose(17.31795716639209, 61.680395387149915, Math.toRadians(-180)); + launchPose2 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + preloadPose3 = new Pose(61.205761230468745, 37.861437866210935, Math.toRadians(-180)); + intakePose3 = new Pose(19.453047775947276, 43.6507413509061, Math.toRadians(-180)); + launchPose3 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + finishPose = new Pose(35.110378912685334, 71.40691927512356, Math.toRadians(-180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.b) { + startPose = new Pose( + 122.64909390444811, 124.7841845140033, Math.toRadians(40)); + + shootPose = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); + + preloadPose = new Pose( + 96.55354200988468, 85.4036243822076, Math.toRadians(0)); + + intakePose = new Pose( + 138.16, 95.92, Math.toRadians(0)); + + launchPose1 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); + + preloadPose2 = new Pose( + 88.56, 62.64000000000001, Math.toRadians(0)); + + intakePose2 = new Pose( + 142.6, 68.599999999999994, Math.toRadians(0)); + + launchPose2 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); +//10 contrl z's remove a 8 in preload p 3 + preloadPose3 = new Pose( + 90.79423876953126, 37.861437866210935, Math.toRadians(0)); + + intakePose3 = new Pose( + 138.8, 39.6, Math.toRadians(0)); + + launchPose3 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); + + finishPose = new Pose( + 108.88962108731467, 71.40691927512356, Math.toRadians(0)); + + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.a) { + startPose = new Pose(79.90752160644531, 8.715837280273437, Math.toRadians(90)); + + shootPose = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48 )); + + preloadPose = new Pose( + 96.55354200988468, 85.4036243822076, Math.toRadians(0)); + + intakePose = new Pose( + 128.16, 85.92, Math.toRadians(0)); + + launchPose1 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose2 = new Pose( + 88.56, 62.64000000000001, Math.toRadians(0)); + + intakePose2 = new Pose( + 129.6, 63.599999999999994, Math.toRadians(0)); + + launchPose2 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose3 = new Pose( + 82.79423876953126, 37.861437866210935, Math.toRadians(0)); + + intakePose3 = new Pose( + 130.8, 39.6, Math.toRadians(0)); + + launchPose3 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + finishPose = new Pose( + 108.88962108731467, 71.40691927512356, Math.toRadians(0)); + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.x) { + startPose = new Pose(63.87744128417969, 7.987197265625001, Math.toRadians(90)); + shootPose = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose = new Pose(47.44645799011532, 85.4036243822076, Math.toRadians(-180)); + intakePose = new Pose(26.332784184514, 91.57166392092257, Math.toRadians(-180)); + launchPose1 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose2 = new Pose(64.1203212890625, 62.63519836425781, Math.toRadians(-180)); + intakePose2 = new Pose(23.960461285008236, 63.57825370675453, Math.toRadians(-180)); + launchPose2 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + preloadPose3 = new Pose(61.205761230468745, 37.861437866210935, Math.toRadians(-180)); + intakePose3 = new Pose(20.401976935749587, 41.75288303130149, Math.toRadians(-180)); + launchPose3 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + finishPose = new Pose(35.110378912685334, 71.40691927512356, Math.toRadians(-180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + } + + private void buildPaths() { + + driveStartPosShootPos = follower.pathBuilder() + .addPath(new BezierLine(startPose, shootPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), shootPose.getHeading()) + .build(); + + shootToPrePickup = follower.pathBuilder() + .addPath(new BezierLine(shootPose, preloadPose)) + .setLinearHeadingInterpolation(shootPose.getHeading(), preloadPose.getHeading()) + .build(); + + INTAKE = follower.pathBuilder() + .addPath(new BezierLine(preloadPose, intakePose)) + .setLinearHeadingInterpolation(preloadPose.getHeading(), intakePose.getHeading()) + .build(); + + LAUNCH_PICKUP_1 = follower.pathBuilder() + .addPath(new BezierLine(intakePose, launchPose1)) + .setLinearHeadingInterpolation(intakePose.getHeading(), launchPose1.getHeading()) + .build(); + + SHOOT_TO_PREPICKUP2 = follower.pathBuilder() + .addPath(new BezierLine(launchPose1, preloadPose2)) + .setLinearHeadingInterpolation(launchPose1.getHeading(), preloadPose2.getHeading()) + .build(); + + INTAKEPOS2 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose2, intakePose2)) + .setLinearHeadingInterpolation(preloadPose2.getHeading(), intakePose2.getHeading()) + .build(); + + LAUNCHPOSE2 = follower.pathBuilder() + .addPath(new BezierLine(intakePose2, launchPose2)) + .setLinearHeadingInterpolation(intakePose2.getHeading(), launchPose2.getHeading()) + .build(); + + PRELOADPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(launchPose2, preloadPose3)) + .setLinearHeadingInterpolation(launchPose2.getHeading(), preloadPose3.getHeading()) + .build(); + + INTAKEPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose3, intakePose3)) + .setLinearHeadingInterpolation(preloadPose3.getHeading(), intakePose3.getHeading()) + .build(); + + LAUNCHPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(intakePose3, launchPose3)) + .setLinearHeadingInterpolation(intakePose3.getHeading(), launchPose3.getHeading()) + .build(); + + FINISH = follower.pathBuilder() + .addPath(new BezierLine(launchPose3, finishPose)) + .setLinearHeadingInterpolation(launchPose3.getHeading(), finishPose.getHeading()) + .build(); + } + + private void statePathUpdate() { + if (pathState == null) + return; + + double shooterVelocity = -1150; + intake.setPower(-1); + + + switch (pathState) { + + case DRIVE_STARPOS_SHOOT_POS: + startPath(driveStartPosShootPos); + leftPusher.setPower(0); + rightPusher.setPower(0); + + if (!follower.isBusy()) { + shooter.setVelocity(shooterVelocity); + intake.setPower(-1); + if (shooter.getVelocity() <= -25) { + if (pathTimer.getElapsedTimeSeconds() >= 1) { + leftPusher.setPower(1); + } + if (pathTimer.getElapsedTimeSeconds() >= 1) { /// ///////////////////////////////////////////////////// + shooter.setVelocity(shooterVelocity); + rightPusher.setPower(-1); + } + } + if (pathTimer.getElapsedTimeSeconds() >= 8) { + setPathState(StateMachine.SHOOT_TO_PREPICKUP); + } + } + break; + + case SHOOT_TO_PREPICKUP: + startPath(shootToPrePickup); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE); + break; + + case INTAKE: + startPath(INTAKE); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_1); + break; + + case LAUNCH_PICKUP_1: + startPath(LAUNCH_PICKUP_1); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.5) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP2); + } + } + break; + + case SHOOT_TO_PREPICKUP2: + startPath(SHOOT_TO_PREPICKUP2); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE2); + break; + + case INTAKE2: + startPath(INTAKEPOS2); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_2); + break; + + case LAUNCH_PICKUP_2: + startPath(LAUNCHPOSE2); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.2) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP3); + } + } + break; + + case SHOOT_TO_PREPICKUP3: + startPath(PRELOADPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE3); + break; + + case INTAKE3: + startPath(INTAKEPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_3); + break; + + case LAUNCH_PICKUP_3: + startPath(LAUNCHPOSE3); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.85) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.25) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.FINISH); + } + } + break; + + case FINISH: + if (!pathStarted) follower.followPath(FINISH, true); + pathStarted = true; + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + break; + } + } + + private void startPath(PathChain path) { + if (!pathStarted) { + follower.followPath(path, true); + pathStarted = true; + pathTimer.resetTimer(); + } + } + + private void runIntakeWhileBusy() { + intake.setPower(-1); + } + + private void runShooterPusherWait(StateMachine nextState) { + if (pathTimer.getElapsedTimeSeconds() >= 5) { + setPathState(nextState); + } + } + + private void setPathState(StateMachine newState) { + pathState = newState; + pathStarted = false; + pathTimer.resetTimer(); + } + + @Override + public void loop() { + follower.update(); + statePathUpdate(); + + telemetry.addData("STATE", pathState); + + Pose pose = follower.getPose(); + if (pose != null) { + telemetry.addData("x", pose.getX()); + telemetry.addData("y", pose.getY()); + telemetry.addData("heading", pose.getHeading()); + } + + telemetry.addData("followerBusy", follower.isBusy()); + telemetry.addData("pathTimer", pathTimer.getElapsedTimeSeconds()); + telemetry.update(); + } +} diff --git a/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous_DBG.java b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous_DBG.java new file mode 100644 index 000000000000..49b96125937c --- /dev/null +++ b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous_DBG.java @@ -0,0 +1,480 @@ + +package org.firstinspires.ftc.teamcode; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.pedropathing.util.Timer; + +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; + +@Autonomous +public class TrialAutonomous_DBG extends OpMode { + + DcMotorEx shooter; + DcMotor intake; + CRServo leftPusher; + CRServo rightPusher; + + Follower follower; + Timer pathTimer; + Timer timer; + + boolean pathStarted; + boolean pathsBuilt = false; + + enum StateMachine { + DRIVE_STARPOS_SHOOT_POS, + SHOOT_TO_PREPICKUP, + INTAKE, + LAUNCH_PICKUP_1, + SHOOT_TO_PREPICKUP2, + INTAKE2, + LAUNCH_PICKUP_2, + SHOOT_TO_PREPICKUP3, + INTAKE3, + LAUNCH_PICKUP_3, + FINISH; + + } + + StateMachine pathState; + + private Pose startPose; + private Pose shootPose; + private Pose preloadPose; + private Pose intakePose; + private Pose launchPose1; + + private Pose preloadPose2; + private Pose intakePose2; + private Pose launchPose2; + + private Pose preloadPose3; + private Pose intakePose3; + private Pose launchPose3; + + private Pose finishPose; + boolean a; + boolean x; + + + + + private PathChain driveStartPosShootPos, shootToPrePickup, INTAKE, LAUNCH_PICKUP_1, SHOOT_TO_PREPICKUP2, + INTAKEPOS2, LAUNCHPOSE2, PRELOADPOSE3, INTAKEPOSE3, LAUNCHPOSE3, FINISH; + + @Override + public void init() { + + pathState = StateMachine.DRIVE_STARPOS_SHOOT_POS; + + pathTimer = new Timer(); + timer = new Timer(); + pathTimer.resetTimer(); + + + + follower = Constants.createFollower(hardwareMap); + + shooter = hardwareMap.get(DcMotorEx.class, "shooter"); + intake = hardwareMap.get(DcMotor .class, "intake"); + leftPusher = hardwareMap.get(CRServo.class, "leftPusher"); + rightPusher = hardwareMap.get(CRServo.class, "rightPusher"); + + pathStarted = false; + + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + } + public void start () { + timer.resetTimer(); + } + + @Override + public void init_loop() { + if (!pathsBuilt) { + InitializePoseValues(); + } + } + + private void InitializePoseValues() { + + if (gamepad1.y) { + telemetry.addLine("Big blue selected"); + } + else if (gamepad1.b) { + telemetry.addLine("Big red selected"); + } + else if (gamepad1.a) { + telemetry.addLine("Small red selected"); + } + else if (gamepad1.x) { + telemetry.addLine("Small blue selected"); + } + else { + telemetry.addLine("Select auto mode by pressing: " + + "X(Small blue) / Y(Big blue) / B(Big red) / A(Small red)"); + telemetry.update(); + return; + } + + telemetry.update(); + + if (gamepad1.y) { + startPose = new Pose(21.350906095551892, 124.7841845140033, Math.toRadians(138)); + shootPose = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(138)); + preloadPose = new Pose(47.44645799011532, 85.4036243822076, Math.toRadians(-180)); + intakePose = new Pose(26.332784184514, 91.57166392092257, Math.toRadians(-180)); + launchPose1 = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(138)); + preloadPose2 = new Pose(64.1203212890625, 62.63519836425781, Math.toRadians(-180)); + intakePose2 = new Pose(17.31795716639209, 61.680395387149915, Math.toRadians(-180)); + launchPose2 = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(132)); + preloadPose3 = new Pose(61.205761230468745, 37.861437866210935, Math.toRadians(-180)); + intakePose3 = new Pose(19.453047775947276, 43.6507413509061, Math.toRadians(-180)); + launchPose3 = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(132)); + finishPose = new Pose(35.110378912685334, 71.40691927512356, Math.toRadians(-180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.b) { + startPose = new Pose( + 122.64909390444811, 124.7841845140033, Math.toRadians(48)); + + shootPose = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose = new Pose( + 96.35555555555555, 95.11111111111111, Math.toRadians(0)); + + intakePose = new Pose( + 136.88888888888889, 95.46666666666667, Math.toRadians(0)); + + launchPose1 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose2 = new Pose( + 86.39999999999999, 71.82222222222222, Math.toRadians(0)); + + intakePose2 = new Pose( + 136.17777777777778, 72, Math.toRadians(0)); + + launchPose2 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose3 = new Pose( + 84.08888888888889, 47.28888888888889, Math.toRadians(0)); + + intakePose3 = new Pose( + 143.2888888888889, 47.28888888888889, Math.toRadians(0)); + + launchPose3 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + finishPose = new Pose( + 109.15555555555555, 84.08888888888889, Math.toRadians(0)); + + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.a) { + a = true; + startPose = new Pose( + 78.4, + 8.000000000000004, + Math.toRadians(0) + ); + + shootPose = new Pose( + 72.17777777777778, + 23.28888888888889, + Math.toRadians(70) + ); + + preloadPose = new Pose( + 102.93333333333334, + 10.422222222222222, + Math.toRadians(0) + ); + + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.x) { + x = true; + startPose = new Pose(65.6, 8.000000000000004, Math.toRadians(90)); + shootPose = new Pose(71.82222222222222, 23.28888888888889, Math.toRadians(110)); + preloadPose = new Pose(41.06666666666667, 9.422222222222222, Math.toRadians(180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + } + + private void buildPaths() { + + driveStartPosShootPos = follower.pathBuilder() + .addPath(new BezierLine(startPose, shootPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), shootPose.getHeading()) + .build(); + + shootToPrePickup = follower.pathBuilder() + .addPath(new BezierLine(shootPose, preloadPose)) + .setLinearHeadingInterpolation(shootPose.getHeading(), preloadPose.getHeading()) + .build(); + + INTAKE = follower.pathBuilder() + .addPath(new BezierLine(preloadPose, intakePose)) + .setLinearHeadingInterpolation(preloadPose.getHeading(), intakePose.getHeading()) + .build(); + + LAUNCH_PICKUP_1 = follower.pathBuilder() + .addPath(new BezierLine(intakePose, launchPose1)) + .setLinearHeadingInterpolation(intakePose.getHeading(), launchPose1.getHeading()) + .build(); + + SHOOT_TO_PREPICKUP2 = follower.pathBuilder() + .addPath(new BezierLine(launchPose1, preloadPose2)) + .setLinearHeadingInterpolation(launchPose1.getHeading(), preloadPose2.getHeading()) + .build(); + + INTAKEPOS2 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose2, intakePose2)) + .setLinearHeadingInterpolation(preloadPose2.getHeading(), intakePose2.getHeading()) + .build(); + + LAUNCHPOSE2 = follower.pathBuilder() + .addPath(new BezierLine(intakePose2, launchPose2)) + .setLinearHeadingInterpolation(intakePose2.getHeading(), launchPose2.getHeading()) + .build(); + + PRELOADPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(launchPose2, preloadPose3)) + .setLinearHeadingInterpolation(launchPose2.getHeading(), preloadPose3.getHeading()) + .build(); + + INTAKEPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose3, intakePose3)) + .setLinearHeadingInterpolation(preloadPose3.getHeading(), intakePose3.getHeading()) + .build(); + + LAUNCHPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(intakePose3, launchPose3)) + .setLinearHeadingInterpolation(intakePose3.getHeading(), launchPose3.getHeading()) + .build(); + + FINISH = follower.pathBuilder() + .addPath(new BezierLine(launchPose3, finishPose)) + .setLinearHeadingInterpolation(launchPose3.getHeading(), finishPose.getHeading()) + .build(); + } + + private void statePathUpdate() { + double shooterVelocity; + + if (pathState == null) + return; + + if (x ^ a) { + shooterVelocity = -1990; + } + else { + shooterVelocity = -1150; + } + + + intake.setPower(-1); + + + switch (pathState) { + + case DRIVE_STARPOS_SHOOT_POS: + startPath(driveStartPosShootPos); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(shooterVelocity); + + if (!follower.isBusy()) { + if (shooter.getVelocity() <= shooterVelocity) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 4) { + shooter.setVelocity(shooterVelocity); + leftPusher.setPower(1); + } + } + if (pathTimer.getElapsedTimeSeconds() >= 8.5) { + setPathState(StateMachine.SHOOT_TO_PREPICKUP); + } + + + break; + + case SHOOT_TO_PREPICKUP: + startPath(shootToPrePickup); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy() && !x ^ !a) { + setPathState(StateMachine.INTAKE); + } + break; + + case INTAKE: + startPath(INTAKE); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_1); + break; + + case LAUNCH_PICKUP_1: + startPath(LAUNCH_PICKUP_1); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.5) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP2); + } + + + + } + break; + + case SHOOT_TO_PREPICKUP2: + startPath(SHOOT_TO_PREPICKUP2); // ✅ ONLY FIX + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE2); + break; + + case INTAKE2: + startPath(INTAKEPOS2); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_2); + break; + + case LAUNCH_PICKUP_2: + startPath(LAUNCHPOSE2); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.2) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP3); + } + } + break; + + case SHOOT_TO_PREPICKUP3: + startPath(PRELOADPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE3); + break; + + case INTAKE3: + startPath(INTAKEPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_3); + break; + + case LAUNCH_PICKUP_3: + startPath(LAUNCHPOSE3); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.85) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.25) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.FINISH); + } + } + break; + + case FINISH: + if (!pathStarted) follower.followPath(FINISH, true); + pathStarted = true; + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + break; + } + } + + private void startPath(PathChain path) { + if (!pathStarted) { + follower.followPath(path, true); + pathStarted = true; + pathTimer.resetTimer(); + } + } + + private void runIntakeWhileBusy() { + intake.setPower(-1); + } + + private void runShooterPusherWait(StateMachine nextState) { + if (pathTimer.getElapsedTimeSeconds() >= 5) { + setPathState(nextState); + } + } + + private void setPathState(StateMachine newState) { + pathState = newState; + pathStarted = false; + pathTimer.resetTimer(); + } + + @Override + public void loop() { + follower.update(); + statePathUpdate(); + + telemetry.addData("STATE", pathState); + + Pose pose = follower.getPose(); + if (pose != null) { + telemetry.addData("x", pose.getX()); + telemetry.addData("y", pose.getY()); + telemetry.addData("heading", pose.getHeading()); + } + + telemetry.addData("followerBusy", follower.isBusy()); + telemetry.addData("pathTimer", pathTimer.getElapsedTimeSeconds()); + telemetry.addData("timer", timer.getElapsedTimeSeconds()); + telemetry.update(); + } +} \ No newline at end of file diff --git a/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java new file mode 100644 index 000000000000..e61cd9777ad7 --- /dev/null +++ b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import com.pedropathing.control.FilteredPIDFCoefficients; +import com.pedropathing.control.PIDFCoefficients; +import com.pedropathing.follower.Follower; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; +import com.pedropathing.ftc.localization.constants.PinpointConstants; +import com.pedropathing.paths.PathConstraints; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +public class Constants { + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(10.6) + .forwardZeroPowerAcceleration(-42.51297137628084) + .lateralZeroPowerAcceleration(-53.639708599274925) + .translationalPIDFCoefficients(new PIDFCoefficients(0.03, 0, 0.03, 0.035)) + .headingPIDFCoefficients(new PIDFCoefficients (1, 0, 0.022, 0.025)) + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.0075, 0, 0, 0.6, 0.025)) + .centripetalScaling(0.001); + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(1) + .rightFrontMotorName("rightFront") + .rightRearMotorName("rightBack") + .leftRearMotorName("leftBack") + .leftFrontMotorName("leftFront") + .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE) + .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) + .xVelocity(61.837623896561276) + .yVelocity(52.13752806656005); + + public static PinpointConstants localizerConstants = new PinpointConstants() + .forwardPodY(-6) + .strafePodX(3.5) + .distanceUnit(DistanceUnit.INCH) + .hardwareMapName("imu2") + .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) + .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED); + + + + public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); + + public static Follower createFollower(HardwareMap hardwareMap) { + return new FollowerBuilder(followerConstants, hardwareMap) + .pinpointLocalizer(localizerConstants) + .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) + .build(); + } +} diff --git a/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java new file mode 100644 index 000000000000..c58d7fed70f2 --- /dev/null +++ b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -0,0 +1,1351 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawOnlyCurrent; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.draw; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM; + +import com.bylazar.configurables.PanelsConfigurables; +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.configurables.annotations.IgnoreConfigurable; +import com.bylazar.field.FieldManager; +import com.bylazar.field.PanelsField; +import com.bylazar.field.Style; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.*; +import com.pedropathing.math.*; +import com.pedropathing.paths.*; +import com.pedropathing.telemetry.SelectableOpMode; +import com.pedropathing.util.*; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import java.util.ArrayList; +import java.util.List; + +/** + * This is the Tuning class. It contains a selection menu for various tuning OpModes. + * + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 6/26/2025 + */ +@Configurable +@TeleOp(name = "Tuning", group = "Pedro Pathing") +public class Tuning extends SelectableOpMode { + public static Follower follower; + + @IgnoreConfigurable + static PoseHistory poseHistory; + + @IgnoreConfigurable + static TelemetryManager telemetryM; + + @IgnoreConfigurable + static ArrayList changes = new ArrayList<>(); + + public Tuning() { + super("Select a Tuning OpMode", s -> { + s.folder("Localization", l -> { + l.add("Localization Test", LocalizationTest::new); + l.add("Forward Tuner", ForwardTuner::new); + l.add("Lateral Tuner", LateralTuner::new); + l.add("Turn Tuner", TurnTuner::new); + }); + s.folder("Automatic", a -> { + a.add("Forward Velocity Tuner", ForwardVelocityTuner::new); + a.add("Lateral Velocity Tuner", LateralVelocityTuner::new); + a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new); + a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new); + }); + s.folder("Manual", p -> { + p.add("Translational Tuner", TranslationalTuner::new); + p.add("Heading Tuner", HeadingTuner::new); + p.add("Drive Tuner", DriveTuner::new); + p.add("Line Tuner", Line::new); + p.add("Centripetal Tuner", CentripetalTuner::new); + }); + s.folder("Tests", p -> { + p.add("Line", Line::new); + p.add("Triangle", Triangle::new); + p.add("Circle", Circle::new); + }); + }); + } + + @Override + public void onSelect() { + if (follower == null) { + follower = Constants.createFollower(hardwareMap); + PanelsConfigurables.INSTANCE.refreshClass(this); + } else { + follower = Constants.createFollower(hardwareMap); + } + + follower.setStartingPose(new Pose()); + + poseHistory = follower.getPoseHistory(); + + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + + Drawing.init(); + } + + @Override + public void onLog(List lines) {} + + public static void drawOnlyCurrent() { + try { + Drawing.drawRobot(follower.getPose()); + Drawing.sendPacket(); + } catch (Exception e) { + throw new RuntimeException("Drawing failed " + e); + } + } + + public static void draw() { + Drawing.drawDebug(follower); + } + + /** This creates a full stop of the robot by setting the drive motors to run at 0 power. */ + public static void stopRobot() { + follower.startTeleopDrive(true); + follower.setTeleOpDrive(0,0,0,true); + } +} + +/** + * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a + * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot. + * You should use this to check the robot's localization. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class LocalizationTest extends OpMode { + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + } + + /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("This will print your robot's position to telemetry while " + + "allowing robot control through a basic mecanum drive on gamepad 1."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.startTeleopDrive(); + follower.update(); + } + + /** + * This updates the robot's pose estimate, the simple mecanum drive, and updates the + * Panels telemetry with the robot's position as well as draws the robot's position. + */ + @Override + public void loop() { + follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); + follower.update(); + + telemetryM.debug("x:" + follower.getPose().getX()); + telemetryM.debug("y:" + follower.getPose().getY()); + telemetryM.debug("heading:" + follower.getPose().getHeading()); + telemetryM.debug("total heading:" + follower.getTotalHeading()); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the forward ticks to inches in your + * localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class ForwardTuner extends OpMode { + public static double DISTANCE = 48; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Distance Moved: " + follower.getPose().getX()); + telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryM.debug("Multiplier: " + (DISTANCE / ((follower.getPose().getX() - 72) / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the strafe ticks to inches in your + * localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 2.0, 6/26/2025 + */ +class LateralTuner extends OpMode { + public static double DISTANCE = 48; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Distance Moved: " + follower.getPose().getY()); + telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryM.debug("Multiplier: " + (DISTANCE / ((follower.getPose().getY() - 72) / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current angle in ticks to the specified angle in radians. So, to use this, run the + * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground. + * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run + * multiple trials and average the results. Then, input the multiplier into the turning ticks to + * radians in your localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class TurnTuner extends OpMode { + public static double ANGLE = 2 * Math.PI; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Total Angle: " + follower.getTotalHeading()); + telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); + telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the ForwardVelocityTuner autonomous follower OpMode. This runs the robot forwards at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class ForwardVelocityTuner extends OpMode { + private final ArrayList velocities = new ArrayList<>(); + public static double DISTANCE = 48; + public static double RECORD_NUMBER = 10; + + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); + telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); + telemetryM.debug("Press B on game pad 1 to stop."); + telemetryM.debug("pose", follower.getPose()); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + follower.startTeleopDrive(true); + follower.update(); + end = false; + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run forward enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + + if (!end) { + if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) { + end = true; + stopRobot(); + } else { + follower.setTeleOpDrive(1,0,0,true); + //double currentVelocity = Math.abs(follower.getVelocity().getXComponent()); + double currentVelocity = Math.abs(follower.poseTracker.getLocalizer().getVelocity().getX()); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + stopRobot(); + double average = 0; + for (double velocity : velocities) { + average += velocity; + } + average /= velocities.size(); + telemetryM.debug("Forward Velocity: " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on)."); + + for (int i = 0; i < velocities.size(); i++) { + telemetry.addData(String.valueOf(i), velocities.get(i)); + } + + telemetryM.update(telemetry); + telemetry.update(); + + if (gamepad1.aWasPressed()) { + follower.setXVelocity(average); + String message = "XMovement: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the StrafeVelocityTuner autonomous follower OpMode. This runs the robot left at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class LateralVelocityTuner extends OpMode { + private final ArrayList velocities = new ArrayList<>(); + + public static double DISTANCE = 48; + public static double RECORD_NUMBER = 10; + + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the drive motors as well as the cache of velocities and the Panels + * telemetryM. + */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the left."); + telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); + telemetryM.debug("Press B on Gamepad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run left at full power. */ + @Override + public void start() { + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + follower.startTeleopDrive(true); + follower.update(); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run sideways enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + if (!end) { + if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) { + end = true; + stopRobot(); + } else { + follower.setTeleOpDrive(0,1,0,true); + double currentVelocity = Math.abs(follower.getVelocity().dot(new Vector(1, Math.PI / 2))); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + stopRobot(); + double average = 0; + for (double velocity : velocities) { + average += velocity; + } + average /= velocities.size(); + + telemetryM.debug("Strafe Velocity: " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.setYVelocity(average); + String message = "YMovement: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the ForwardZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot + * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +class ForwardZeroPowerAccelerationTuner extends OpMode { + private final ArrayList accelerations = new ArrayList<>(); + public static double VELOCITY = 30; + + private double previousVelocity; + private long previousTimeNano; + + private boolean stopping; + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the Panels telemetryM. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second."); + telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); + telemetryM.debug("Press B on Gamepad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + follower.startTeleopDrive(false); + follower.update(); + follower.setTeleOpDrive(1,0,0,true); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + Vector heading = new Vector(1.0, follower.getPose().getHeading()); + if (!end) { + if (!stopping) { + if (follower.getVelocity().dot(heading) > VELOCITY) { + previousVelocity = follower.getVelocity().dot(heading); + previousTimeNano = System.nanoTime(); + stopping = true; + follower.setTeleOpDrive(0,0,0,true); + } + } else { + double currentVelocity = follower.getVelocity().dot(heading); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { + end = true; + } + } + } else { + double average = 0; + for (double acceleration : accelerations) { + average += acceleration; + } + average /= accelerations.size(); + + telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.getConstants().setForwardZeroPowerAcceleration(average); + String message = "Forward Zero Power Acceleration: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the LateralZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot + * to the left until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class LateralZeroPowerAccelerationTuner extends OpMode { + private final ArrayList accelerations = new ArrayList<>(); + public static double VELOCITY = 30; + private double previousVelocity; + private long previousTimeNano; + private boolean stopping; + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run to the left until it reaches " + VELOCITY + " inches per second."); + telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); + telemetryM.debug("Press B on game pad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + follower.startTeleopDrive(false); + follower.update(); + follower.setTeleOpDrive(0,1,0,true); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + Vector heading = new Vector(1.0, follower.getPose().getHeading() - Math.PI / 2); + if (!end) { + if (!stopping) { + if (Math.abs(follower.getVelocity().dot(heading)) > VELOCITY) { + previousVelocity = Math.abs(follower.getVelocity().dot(heading)); + previousTimeNano = System.nanoTime(); + stopping = true; + follower.setTeleOpDrive(0,0,0,true); + } + } else { + double currentVelocity = Math.abs(follower.getVelocity().dot(heading)); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { + end = true; + } + } + } else { + double average = 0; + for (double acceleration : accelerations) { + average += acceleration; + } + average /= accelerations.size(); + + telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.getConstants().setLateralZeroPowerAcceleration(average); + String message = "Lateral Zero Power Acceleration: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the Translational PIDF Tuner OpMode. It will keep the robot in place. + * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class TranslationalTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the Follower and creates the forward and backward Paths. */ + @Override + public void init_loop() { + telemetryM.debug("This will activate the translational PIDF(s)"); + telemetryM.debug("The robot will try to stay in place while you push it laterally."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateTranslational(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); + telemetryM.update(telemetry); + } +} + +/** + * This is the Heading PIDF Tuner OpMode. It will keep the robot in place. + * The user should try to turn the robot to test the PIDF and adjust the PIDF values accordingly. + * It will try to keep the robot at a constant heading while the user tries to turn it. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class HeadingTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will activate the heading PIDF(s)."); + telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateHeading(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); + telemetryM.update(telemetry); + } +} + +/** + * This is the Drive PIDF Tuner OpMode. It will run the robot in a straight line going forward and back. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class DriveTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private PathChain forwards; + private PathChain backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward."); + telemetryM.debug("The robot will go forward and backward continuously along the path."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateDrive(); + + forwards = follower.pathBuilder() + .setGlobalDeceleration() + .addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))) + .setConstantHeadingInterpolation(0) + .build(); + + backwards = follower.pathBuilder() + .setGlobalDeceleration() + .addPath(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))) + .setConstantHeadingInterpolation(0) + .build(); + + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving forward?: " + forward); + telemetryM.update(telemetry); + } +} + +/** + * This is the Line Test Tuner OpMode. It will drive the robot forward and back + * The user should push the robot laterally and angular to test out the drive, heading, and translational PIDFs. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class Line extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the Follower and creates the forward and backward Paths. */ + @Override + public void init_loop() { + telemetryM.debug("This will activate all the PIDF(s)"); + telemetryM.debug("The robot will go forward and backward continuously along the path while correcting."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving Forward?: " + forward); + telemetryM.update(telemetry); + } +} + +/** + * This is the Centripetal Tuner OpMode. It runs the robot in a specified distance + * forward and to the left. On reaching the end of the forward Path, the robot runs the backward + * Path the same distance back to the start. Rinse and repeat! This is good for testing a variety + * of Vectors, like the drive Vector, the translational Vector, the heading Vector, and the + * centripetal Vector. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +class CentripetalTuner extends OpMode { + public static double DISTANCE = 20; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. + * Additionally, this initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); + telemetryM.debug("The robot will go continuously along the path."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + forwards = new Path(new BezierCurve(new Pose(72,72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72))); + backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(72,72))); + + backwards.setTangentHeadingInterpolation(); + backwards.reverseHeadingInterpolation(); + + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving away from the origin along the curve?: " + forward); + telemetryM.update(telemetry); + } +} + +/** + * This is the Triangle autonomous OpMode. + * It runs the robot in a triangle, with the starting point being the bottom-middle point. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Samarth Mahapatra - 1002 CircuitRunners Robotics Surge + * @version 1.0, 12/30/2024 + */ +class Triangle extends OpMode { + + private final Pose startPose = new Pose(72, 72, Math.toRadians(0)); + private final Pose interPose = new Pose(24 + 72, -24 + 72, Math.toRadians(90)); + private final Pose endPose = new Pose(24 + 72, 24 + 72, Math.toRadians(45)); + + private PathChain triangle; + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (follower.atParametricEnd()) { + follower.followPath(triangle, true); + } + } + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + @Override + public void init_loop() { + telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point."); + telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** Creates the PathChain for the "triangle".*/ + @Override + public void start() { + follower.setStartingPose(startPose); + + triangle = follower.pathBuilder() + .addPath(new BezierLine(startPose, interPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), interPose.getHeading()) + .addPath(new BezierLine(interPose, endPose)) + .setLinearHeadingInterpolation(interPose.getHeading(), endPose.getHeading()) + .addPath(new BezierLine(endPose, startPose)) + .setLinearHeadingInterpolation(endPose.getHeading(), startPose.getHeading()) + .build(); + + follower.followPath(triangle); + } +} + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bezier curves that have control points set essentially in a square. However, + * it turns enough to tune your centripetal force correction and some of your heading. Some lag in + * heading is to be expected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class Circle extends OpMode { + public static double RADIUS = 10; + private PathChain circle; + + public void start() { + circle = follower.pathBuilder() + .addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .build(); + follower.followPath(circle); + } + + @Override + public void init_loop() { + telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); + telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode."); + telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (follower.atParametricEnd()) { + follower.followPath(circle); + } + } +} + +/** + * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. + * + * @author Lazar - 19234 + * @version 1.1, 5/19/2025 + */ +class Drawing { + public static final double ROBOT_RADIUS = 9; // woah + private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); + + private static final Style robotLook = new Style( + "", "#3F51B5", 0.75 + ); + private static final Style historyLook = new Style( + "", "#4CAF50", 0.75 + ); + + /** + * This prepares Panels Field for using Pedro Offsets + */ + public static void init() { + panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING()); + } + + /** + * This draws everything that will be used in the Follower's telemetryDebug() method. This takes + * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * + * @param follower Pedro Follower instance. + */ + public static void drawDebug(Follower follower) { + if (follower.getCurrentPath() != null) { + drawPath(follower.getCurrentPath(), robotLook); + Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); + drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook); + } + drawPoseHistory(follower.getPoseHistory(), historyLook); + drawRobot(follower.getPose(), historyLook); + + sendPacket(); + } + + /** + * This draws a robot at a specified Pose with a specified + * look. The heading is represented as a line. + * + * @param pose the Pose to draw the robot at + * @param style the parameters used to draw the robot with + */ + public static void drawRobot(Pose pose, Style style) { + if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { + return; + } + + panelsField.setStyle(style); + panelsField.moveCursor(pose.getX(), pose.getY()); + panelsField.circle(ROBOT_RADIUS); + + Vector v = pose.getHeadingAsUnitVector(); + v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); + double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2; + double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent(); + + panelsField.setStyle(style); + panelsField.moveCursor(x1, y1); + panelsField.line(x2, y2); + } + + /** + * This draws a robot at a specified Pose. The heading is represented as a line. + * + * @param pose the Pose to draw the robot at + */ + public static void drawRobot(Pose pose) { + drawRobot(pose, robotLook); + } + + /** + * This draws a Path with a specified look. + * + * @param path the Path to draw + * @param style the parameters used to draw the Path with + */ + public static void drawPath(Path path, Style style) { + double[][] points = path.getPanelsDrawingPoints(); + + for (int i = 0; i < points[0].length; i++) { + for (int j = 0; j < points.length; j++) { + if (Double.isNaN(points[j][i])) { + points[j][i] = 0; + } + } + } + + panelsField.setStyle(style); + panelsField.moveCursor(points[0][0], points[0][1]); + panelsField.line(points[1][0], points[1][1]); + } + + /** + * This draws all the Paths in a PathChain with a + * specified look. + * + * @param pathChain the PathChain to draw + * @param style the parameters used to draw the PathChain with + */ + public static void drawPath(PathChain pathChain, Style style) { + for (int i = 0; i < pathChain.size(); i++) { + drawPath(pathChain.getPath(i), style); + } + } + + /** + * This draws the pose history of the robot. + * + * @param poseTracker the PoseHistory to get the pose history from + * @param style the parameters used to draw the pose history with + */ + public static void drawPoseHistory(PoseHistory poseTracker, Style style) { + panelsField.setStyle(style); + + int size = poseTracker.getXPositionsArray().length; + for (int i = 0; i < size - 1; i++) { + + panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]); + panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]); + } + } + + /** + * This draws the pose history of the robot. + * + * @param poseTracker the PoseHistory to get the pose history from + */ + public static void drawPoseHistory(PoseHistory poseTracker) { + drawPoseHistory(poseTracker, historyLook); + } + + /** + * This tries to send the current packet to FTControl Panels. + */ + public static void sendPacket() { + panelsField.update(); + } +} diff --git a/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md new file mode 100644 index 000000000000..4d1da42de0c0 --- /dev/null +++ b/TeamCode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -0,0 +1,131 @@ +## TeamCode Module + +Welcome! + +This module, TeamCode, is the place where you will write/paste the code for your team's +robot controller App. This module is currently empty (a clean slate) but the +process for adding OpModes is straightforward. + +## Creating your own OpModes + +The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own. + +Sample opmodes exist in the FtcRobotController module. +To locate these samples, find the FtcRobotController module in the "Project/Android" tab. + +Expand the following tree elements: + FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples + +### Naming of Samples + +To gain a better understanding of how the samples are organized, and how to interpret the +naming system, it will help to understand the conventions that were used during their creation. + +These conventions are described (in detail) in the sample_conventions.md file in this folder. + +To summarize: A range of different samples classes will reside in the java/external/samples. +The class names will follow a naming convention which indicates the purpose of each class. +The prefix of the name will be one of the following: + +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure + of a particular style of OpMode. These are bare bones examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. + It is not intended to drive a functioning robot, it is simply showing the minimal code + required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. + It may be used to provide a common baseline driving OpMode, or + to demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function or concept. + These may be complex, but their operation should be explained clearly in the comments, + or the comments should reference an external doc, guide or tutorial. + Each OpMode should try to only demonstrate a single concept so they are easy to + locate based on their name. These OpModes may not produce a drivable robot. + +After the prefix, other conventions will apply: + +* Sensor class names are constructed as: Sensor - Company - Type +* Robot class names are constructed as: Robot - Mode - Action - OpModetype +* Concept class names are constructed as: Concept - Topic - OpModetype + +Once you are familiar with the range of samples available, you can choose one to be the +basis for your own robot. In all cases, the desired sample(s) needs to be copied into +your TeamCode module to be used. + +This is done inside Android Studio directly, using the following steps: + + 1) Locate the desired sample class in the Project/Android tree. + + 2) Right click on the sample class and select "Copy" + + 3) Expand the TeamCode/java folder + + 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" + + 5) You will be prompted for a class name for the copy. + Choose something meaningful based on the purpose of this class. + Start with a capital letter, and remember that there may be more similar classes later. + +Once your copy has been created, you should prepare it for use on your robot. +This is done by adjusting the OpMode's name, and enabling it to be displayed on the +Driver Station's OpMode list. + +Each OpMode sample class begins with several lines of code like the ones shown below: + +``` + @TeleOp(name="Template: Linear OpMode", group="Linear Opmode") + @Disabled +``` + +The name that will appear on the driver station's "opmode list" is defined by the code: + ``name="Template: Linear OpMode"`` +You can change what appears between the quotes to better describe your opmode. +The "group=" portion of the code can be used to help organize your list of OpModes. + +As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the + ``@Disabled`` annotation which has been included. +This line can simply be deleted , or commented out, to make the OpMode visible. + + + +## ADVANCED Multi-Team App management: Cloning the TeamCode Module + +In some situations, you have multiple teams in your club and you want them to all share +a common code organization, with each being able to *see* the others code but each having +their own team module with their own code that they maintain themselves. + +In this situation, you might wish to clone the TeamCode module, once for each of these teams. +Each of the clones would then appear along side each other in the Android Studio module list, +together with the FtcRobotController module (and the original TeamCode module). + +Selective Team phones can then be programmed by selecting the desired Module from the pulldown list +prior to clicking to the green Run arrow. + +Warning: This is not for the inexperienced Software developer. +You will need to be comfortable with File manipulations and managing Android Studio Modules. +These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this. + +Also.. Make a full project backup before you start this :) + +To clone TeamCode, do the following: + +Note: Some names start with "Team" and others start with "team". This is intentional. + +1) Using your operating system file management tools, copy the whole "TeamCode" + folder to a sibling folder with a corresponding new name, eg: "Team0417". + +2) In the new Team0417 folder, delete the TeamCode.iml file. + +3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder + to a matching name with a lowercase 'team' eg: "team0417". + +4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains + package="org.firstinspires.ftc.teamcode" + to be + package="org.firstinspires.ftc.team0417" + +5) Add: include ':Team0417' to the "/settings.gradle" file. + +6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" \ No newline at end of file diff --git a/TeamCode/TeamCode/src/main/res/raw/readme.md b/TeamCode/TeamCode/src/main/res/raw/readme.md new file mode 100644 index 000000000000..355a3c444b34 --- /dev/null +++ b/TeamCode/TeamCode/src/main/res/raw/readme.md @@ -0,0 +1 @@ +Place your sound files in this folder to use them as project resources. \ No newline at end of file diff --git a/TeamCode/TeamCode/src/main/res/values/strings.xml b/TeamCode/TeamCode/src/main/res/values/strings.xml new file mode 100644 index 000000000000..d781ec5f1e5a --- /dev/null +++ b/TeamCode/TeamCode/src/main/res/values/strings.xml @@ -0,0 +1,4 @@ + + + + diff --git a/TeamCode/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml b/TeamCode/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml new file mode 100644 index 000000000000..22ae7a86ba33 --- /dev/null +++ b/TeamCode/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BotTwoTeleopPreset.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BotTwoTeleopPreset.java new file mode 100644 index 000000000000..6374552c33c6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BotTwoTeleopPreset.java @@ -0,0 +1,294 @@ +/* + * Copyright (c) 2025 FIRST + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior + * written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import static com.qualcomm.robotcore.hardware.DcMotor.ZeroPowerBehavior.BRAKE; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; + +/* + * This file includes a teleop (driver-controlled) file for the goBILDA® StarterBot for the + * 2025-2026 FIRST® Tech Challenge season DECODE™. It leverages a differential/Skid-Steer + * system for robot mobility, one high-speed motor driving two "launcher wheels", and two servos + * which feed that launcher. + * + * Likely the most niche concept we'll use in this example is closed-loop motor velocity control. + * This control method reads the current speed as reported by the motor's encoder and applies a varying + * amount of power to reach, and then hold a target velocity. The FTC SDK calls this control method + * "RUN_USING_ENCODER". This contrasts to the default "RUN_WITHOUT_ENCODER" where you control the power + * applied to the motor directly. + * Since the dynamics of a launcher wheel system varies greatly from those of most other FTC mechanisms, + * we will also need to adjust the "PIDF" coefficients with some that are a better fit for our application. + */ + +@TeleOp(name = "BotTwoTeleopPreset", group = "StarterBot") +//@Disabled +public class BotTwoTeleopPreset extends OpMode { + + /* + * When we control our launcher motor, we are using encoders. These allow the control system + * to read the current speed of the motor and apply more or less power to keep it at a constant + * velocity. Here we are setting the target, and minimum velocity that the launcher should run + * at. The minimum velocity is a threshold for determining when to fire. + */ + final double LAUNCHER_TARGET_VELOCITY = 1125; + final double LAUNCHER_MIN_VELOCITY = 1075; + + // Declare OpMode members. + private DcMotor leftFrontDrive = null; + private DcMotor rightFrontDrive = null; + private DcMotor leftBackDrive = null; + private DcMotor rightBackDrive = null; + private DcMotor intake = null; + + + private DcMotor shooter = null; + + + private CRServo leftPusher = null; + + + private CRServo rightPusher = null; + + + private boolean shotTog = false; + + + + + /* + * TECH TIP: State Machines + * We use a "state machine" to control our launcher motor and feeder servos in this program. + * The first step of a state machine is creating an enum that captures the different "states" + * that our code can be in. + * The core advantage of a state machine is that it allows us to continue to loop through all + * of our code while only running specific code when it's necessary. We can continuously check + * what "State" our machine is in, run the associated code, and when we are done with that step + * move on to the next state. + * This enum is called the "LaunchState". It reflects the current condition of the shooter + * motor and we move through the enum when the user asks our code to fire a shot. + * It starts at idle, when the user requests a launch, we enter SPIN_UP where we get the + * motor up to speed, once it meets a minimum speed then it starts and then ends the launch process. + * We can use higher level code to cycle through these states. But this allows us to write + * functions and autonomous routines in a way that avoids loops within loops, and "waits". + */ + + + + // Setup a variable for each drive wheel to save power level for telemetry + double leftFrontPower; + double rightFrontPower; + double leftBackPower; + double rightBackPower; + + /* + * Code to run ONCE when the driver hits INIT + */ + @Override + public void init() { + + + /* + * Initialize the hardware variables. Note that the strings used here as parameters + * to 'get' must correspond to the names assigned during the robot configuration + * step. + */ + leftFrontDrive = hardwareMap.get(DcMotor.class, "leftFront"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "rightFront"); + leftBackDrive = hardwareMap.get(DcMotor.class, "leftBack"); + rightBackDrive = hardwareMap.get(DcMotor.class, "rightBack"); + intake = hardwareMap.get(DcMotor.class, "intake"); + shooter = hardwareMap.get(DcMotor.class, "shooter"); + leftPusher = hardwareMap.get(CRServo.class, "leftPusher"); + rightPusher = hardwareMap.get(CRServo.class, "rightPusher"); + + + /* + * To drive forward, most robots need the motor on one side to be reversed, + * because the axles point in opposite directions. Pushing the left stick forward + * MUST make robot go forward. So adjust these two lines based on your first test drive. + * Note: The settings here assume direct drive on left and right wheels. Gear + * Reduction or 90 Deg drives may require direction flips + */ + leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); + rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); + leftBackDrive.setDirection(DcMotor.Direction.REVERSE); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + /* + * Here we set our launcher to the RUN_USING_ENCODER runmode. + * If you notice that you have no control over the velocity of the motor, it just jumps + * right to a number much higher than your set point, make sure that your encoders are plugged + * into the port right beside the motor itself. And that the motors polarity is consistent + * through any wiring. + */ + + /* + * Setting zeroPowerBehavior to BRAKE enables a "brake mode". This causes the motor to + * slow down much faster when it is coasting. This creates a much more controllable + * drivetrain. As the robot stops much quicker. + */ + leftFrontDrive.setZeroPowerBehavior(BRAKE); + rightFrontDrive.setZeroPowerBehavior(BRAKE); + leftBackDrive.setZeroPowerBehavior(BRAKE); + rightBackDrive.setZeroPowerBehavior(BRAKE); + + /* + * set Feeders to an initial value to initialize the servo controller + */ + + + + + /* + * Much like our drivetrain motors, we set the left feeder servo to reverse so that they + * both work to feed the ball into the robot. + */ + + + /* + * Tell the driver that initialization is complete. + */ + telemetry.addData("Status", "Initialized"); + } + + /* + * Code to run REPEATEDLY after the driver hits INIT, but before they hit START + */ + @Override + public void init_loop() { + } + + /* + * Code to run ONCE when the driver hits START + */ + @Override + public void start() { + } + + /* + * Code to run REPEATEDLY after the driver hits START but before they hit STOP + */ + @Override + public void loop() { + + + if(gamepad2.a) { + shotTog = true; + } + else if (gamepad2.b) { + shotTog = false; + } + + if (shotTog) { + shooter.setPower(-0.55); + } + else { + shooter.setPower(0); + } + //else if (gamepad2.y) { + // shooter.setPower(-.65); + // } + //else if(gamepad2.b) { + // shooter.setPower(-0.8); + // } + // else if(gamepad2.a) { + // shooter.setPower(-1); + //} + + + + + if (gamepad1.right_bumper){ + intake.setPower(-1); + } + else{ + intake.setPower(0); + } + + if (gamepad2.left_bumper){ + leftPusher.setPower(1); + } + else{ + leftPusher.setPower(0); + } + + if(gamepad2.right_bumper){ + rightPusher.setPower(-1); + } + else{ + rightPusher.setPower(0); + } + + + mecanumDrive(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + } + + /* + * Here we give the user control of the speed of the launcher motor without automatically + * queuing a shot. + */ + + + /* + * Code to run ONCE after the driver hits STOP + */ + @Override + public void stop() { + } + + void mecanumDrive(double forward, double strafe, double rotate){ + + /* the denominator is the largest motor power (absolute value) or 1 + * This ensures all the powers maintain the same ratio, + * but only if at least one is out of the range [-1, 1] + */ + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(rotate), 1); + + leftFrontPower = (forward + strafe + rotate) / denominator; + rightFrontPower = (forward - strafe - rotate) / denominator; + leftBackPower = (forward - strafe + rotate) / denominator; + rightBackPower = (forward + strafe - rotate) / denominator; + + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + + } + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous.java new file mode 100644 index 000000000000..702dd4edfe90 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous.java @@ -0,0 +1,473 @@ +package org.firstinspires.ftc.teamcode; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +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 com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; + +@Autonomous +public class TrialAutonomous extends OpMode { + + DcMotorEx shooter; + DcMotor intake; + CRServo leftPusher; + CRServo rightPusher; + + Follower follower; + Timer pathTimer; + + boolean pathStarted; + boolean pathsBuilt = false; + + enum StateMachine { + DRIVE_STARPOS_SHOOT_POS, + SHOOT_TO_PREPICKUP, + INTAKE, + LAUNCH_PICKUP_1, + SHOOT_TO_PREPICKUP2, + INTAKE2, + LAUNCH_PICKUP_2, + SHOOT_TO_PREPICKUP3, + INTAKE3, + LAUNCH_PICKUP_3, + FINISH; + + } + + StateMachine pathState; + + private Pose startPose; + private Pose shootPose; + private Pose preloadPose; + private Pose intakePose; + private Pose launchPose1; + + private Pose preloadPose2; + private Pose intakePose2; + private Pose launchPose2; + + private Pose preloadPose3; + private Pose intakePose3; + private Pose launchPose3; + + private Pose finishPose; + + + + + private PathChain driveStartPosShootPos, shootToPrePickup, INTAKE, LAUNCH_PICKUP_1, SHOOT_TO_PREPICKUP2, + INTAKEPOS2, LAUNCHPOSE2, PRELOADPOSE3, INTAKEPOSE3, LAUNCHPOSE3, FINISH; + + @Override + public void init() { + + pathState = StateMachine.DRIVE_STARPOS_SHOOT_POS; + + pathTimer = new Timer(); + pathTimer.resetTimer(); + + follower = Constants.createFollower(hardwareMap); + + shooter = hardwareMap.get(DcMotorEx.class, "shooter"); + intake = hardwareMap.get(DcMotor.class, "intake"); + leftPusher = hardwareMap.get(CRServo.class, "leftPusher"); + rightPusher = hardwareMap.get(CRServo.class, "rightPusher"); + + pathStarted = false; + + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + } + + @Override + public void init_loop() { + if (!pathsBuilt) { + InitializePoseValues(); + } + } + + private void InitializePoseValues() { + + if (gamepad1.y) telemetry.addLine("Big blue selected"); + else if (gamepad1.b) telemetry.addLine("Big red selected"); + else if (gamepad1.a) telemetry.addLine("Small red selected"); + else if (gamepad1.x) telemetry.addLine("Small blue selected"); + else if (gamepad1.right_bumper && gamepad1.x) { + telemetry.addLine("Backup selected "); + + } else { + telemetry.addLine("Select auto mode by pressing: " + + "X(Small blue) / Y(Big blue) / B(Big red) / A(Small red)"); + telemetry.update(); + return; + } + + telemetry.update(); + + if (gamepad1.y) { + startPose = new Pose(21.350906095551892, 124.7841845140033, Math.toRadians(138)); + shootPose = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose = new Pose(47.44645799011532, 85.4036243822076, Math.toRadians(-180)); + intakePose = new Pose(26.332784184514, 91.57166392092257, Math.toRadians(-180)); + launchPose1 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose2 = new Pose(72.1203212890625, 65.63519836425781, Math.toRadians(-180)); + intakePose2 = new Pose(17.31795716639209, 61.680395387149915, Math.toRadians(-180)); + launchPose2 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + preloadPose3 = new Pose(61.205761230468745, 37.861437866210935, Math.toRadians(-180)); + intakePose3 = new Pose(19.453047775947276, 43.6507413509061, Math.toRadians(-180)); + launchPose3 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + finishPose = new Pose(35.110378912685334, 71.40691927512356, Math.toRadians(-180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.b) { + startPose = new Pose( + 122.64909390444811, 124.7841845140033, Math.toRadians(40)); + + shootPose = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); + + preloadPose = new Pose( + 96.55354200988468, 85.4036243822076, Math.toRadians(0)); + + intakePose = new Pose( + 138.16, 95.92, Math.toRadians(0)); + + launchPose1 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); + + preloadPose2 = new Pose( + 88.56, 62.64000000000001, Math.toRadians(0)); + + intakePose2 = new Pose( + 142.6, 68.599999999999994, Math.toRadians(0)); + + launchPose2 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); +//10 contrl z's remove a 8 in preload p 3 + preloadPose3 = new Pose( + 90.79423876953126, 37.861437866210935, Math.toRadians(0)); + + intakePose3 = new Pose( + 138.8, 39.6, Math.toRadians(0)); + + launchPose3 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(40)); + + finishPose = new Pose( + 108.88962108731467, 71.40691927512356, Math.toRadians(0)); + + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.a) { + startPose = new Pose(79.90752160644531, 8.715837280273437, Math.toRadians(90)); + + shootPose = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48 )); + + preloadPose = new Pose( + 96.55354200988468, 85.4036243822076, Math.toRadians(0)); + + intakePose = new Pose( + 128.16, 85.92, Math.toRadians(0)); + + launchPose1 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose2 = new Pose( + 88.56, 62.64000000000001, Math.toRadians(0)); + + intakePose2 = new Pose( + 129.6, 63.599999999999994, Math.toRadians(0)); + + launchPose2 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose3 = new Pose( + 82.79423876953126, 37.861437866210935, Math.toRadians(0)); + + intakePose3 = new Pose( + 130.8, 39.6, Math.toRadians(0)); + + launchPose3 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + finishPose = new Pose( + 108.88962108731467, 71.40691927512356, Math.toRadians(0)); + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.x) { + startPose = new Pose(63.87744128417969, 7.987197265625001, Math.toRadians(90)); + shootPose = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose = new Pose(47.44645799011532, 85.4036243822076, Math.toRadians(-180)); + intakePose = new Pose(26.332784184514, 91.57166392092257, Math.toRadians(-180)); + launchPose1 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(138)); + preloadPose2 = new Pose(64.1203212890625, 62.63519836425781, Math.toRadians(-180)); + intakePose2 = new Pose(23.960461285008236, 63.57825370675453, Math.toRadians(-180)); + launchPose2 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + preloadPose3 = new Pose(61.205761230468745, 37.861437866210935, Math.toRadians(-180)); + intakePose3 = new Pose(20.401976935749587, 41.75288303130149, Math.toRadians(-180)); + launchPose3 = new Pose(47.44645799011532, 95.36738056013179, Math.toRadians(132)); + finishPose = new Pose(35.110378912685334, 71.40691927512356, Math.toRadians(-180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + } + + private void buildPaths() { + + driveStartPosShootPos = follower.pathBuilder() + .addPath(new BezierLine(startPose, shootPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), shootPose.getHeading()) + .build(); + + shootToPrePickup = follower.pathBuilder() + .addPath(new BezierLine(shootPose, preloadPose)) + .setLinearHeadingInterpolation(shootPose.getHeading(), preloadPose.getHeading()) + .build(); + + INTAKE = follower.pathBuilder() + .addPath(new BezierLine(preloadPose, intakePose)) + .setLinearHeadingInterpolation(preloadPose.getHeading(), intakePose.getHeading()) + .build(); + + LAUNCH_PICKUP_1 = follower.pathBuilder() + .addPath(new BezierLine(intakePose, launchPose1)) + .setLinearHeadingInterpolation(intakePose.getHeading(), launchPose1.getHeading()) + .build(); + + SHOOT_TO_PREPICKUP2 = follower.pathBuilder() + .addPath(new BezierLine(launchPose1, preloadPose2)) + .setLinearHeadingInterpolation(launchPose1.getHeading(), preloadPose2.getHeading()) + .build(); + + INTAKEPOS2 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose2, intakePose2)) + .setLinearHeadingInterpolation(preloadPose2.getHeading(), intakePose2.getHeading()) + .build(); + + LAUNCHPOSE2 = follower.pathBuilder() + .addPath(new BezierLine(intakePose2, launchPose2)) + .setLinearHeadingInterpolation(intakePose2.getHeading(), launchPose2.getHeading()) + .build(); + + PRELOADPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(launchPose2, preloadPose3)) + .setLinearHeadingInterpolation(launchPose2.getHeading(), preloadPose3.getHeading()) + .build(); + + INTAKEPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose3, intakePose3)) + .setLinearHeadingInterpolation(preloadPose3.getHeading(), intakePose3.getHeading()) + .build(); + + LAUNCHPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(intakePose3, launchPose3)) + .setLinearHeadingInterpolation(intakePose3.getHeading(), launchPose3.getHeading()) + .build(); + + FINISH = follower.pathBuilder() + .addPath(new BezierLine(launchPose3, finishPose)) + .setLinearHeadingInterpolation(launchPose3.getHeading(), finishPose.getHeading()) + .build(); + } + + private void statePathUpdate() { + if (pathState == null) + return; + + double shooterVelocity = -1150; + intake.setPower(-1); + + + switch (pathState) { + + case DRIVE_STARPOS_SHOOT_POS: + startPath(driveStartPosShootPos); + leftPusher.setPower(0); + rightPusher.setPower(0); + + if (!follower.isBusy()) { + shooter.setVelocity(shooterVelocity); + intake.setPower(-1); + if (shooter.getVelocity() <= -25) { + if (pathTimer.getElapsedTimeSeconds() >= 1) { + leftPusher.setPower(1); + } + if (pathTimer.getElapsedTimeSeconds() >= 1) { /// ///////////////////////////////////////////////////// + shooter.setVelocity(shooterVelocity); + rightPusher.setPower(-1); + } + } + if (pathTimer.getElapsedTimeSeconds() >= 8) { + setPathState(StateMachine.SHOOT_TO_PREPICKUP); + } + } + break; + + case SHOOT_TO_PREPICKUP: + startPath(shootToPrePickup); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE); + break; + + case INTAKE: + startPath(INTAKE); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_1); + break; + + case LAUNCH_PICKUP_1: + startPath(LAUNCH_PICKUP_1); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.5) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP2); + } + } + break; + + case SHOOT_TO_PREPICKUP2: + startPath(SHOOT_TO_PREPICKUP2); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE2); + break; + + case INTAKE2: + startPath(INTAKEPOS2); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_2); + break; + + case LAUNCH_PICKUP_2: + startPath(LAUNCHPOSE2); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.2) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP3); + } + } + break; + + case SHOOT_TO_PREPICKUP3: + startPath(PRELOADPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE3); + break; + + case INTAKE3: + startPath(INTAKEPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_3); + break; + + case LAUNCH_PICKUP_3: + startPath(LAUNCHPOSE3); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.85) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.25) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.FINISH); + } + } + break; + + case FINISH: + if (!pathStarted) follower.followPath(FINISH, true); + pathStarted = true; + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + break; + } + } + + private void startPath(PathChain path) { + if (!pathStarted) { + follower.followPath(path, true); + pathStarted = true; + pathTimer.resetTimer(); + } + } + + private void runIntakeWhileBusy() { + intake.setPower(-1); + } + + private void runShooterPusherWait(StateMachine nextState) { + if (pathTimer.getElapsedTimeSeconds() >= 5) { + setPathState(nextState); + } + } + + private void setPathState(StateMachine newState) { + pathState = newState; + pathStarted = false; + pathTimer.resetTimer(); + } + + @Override + public void loop() { + follower.update(); + statePathUpdate(); + + telemetry.addData("STATE", pathState); + + Pose pose = follower.getPose(); + if (pose != null) { + telemetry.addData("x", pose.getX()); + telemetry.addData("y", pose.getY()); + telemetry.addData("heading", pose.getHeading()); + } + + telemetry.addData("followerBusy", follower.isBusy()); + telemetry.addData("pathTimer", pathTimer.getElapsedTimeSeconds()); + telemetry.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous_DBG.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous_DBG.java new file mode 100644 index 000000000000..639c330e8f32 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TrialAutonomous_DBG.java @@ -0,0 +1,479 @@ + +package org.firstinspires.ftc.teamcode; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +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 com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; + +@Autonomous +public class TrialAutonomous_DBG extends OpMode { + + DcMotorEx shooter; + DcMotor intake; + CRServo leftPusher; + CRServo rightPusher; + + Follower follower; + Timer pathTimer; + Timer timer; + + boolean pathStarted; + boolean pathsBuilt = false; + + enum StateMachine { + DRIVE_STARPOS_SHOOT_POS, + SHOOT_TO_PREPICKUP, + INTAKE, + LAUNCH_PICKUP_1, + SHOOT_TO_PREPICKUP2, + INTAKE2, + LAUNCH_PICKUP_2, + SHOOT_TO_PREPICKUP3, + INTAKE3, + LAUNCH_PICKUP_3, + FINISH; + + } + + StateMachine pathState; + + private Pose startPose; + private Pose shootPose; + private Pose preloadPose; + private Pose intakePose; + private Pose launchPose1; + + private Pose preloadPose2; + private Pose intakePose2; + private Pose launchPose2; + + private Pose preloadPose3; + private Pose intakePose3; + private Pose launchPose3; + + private Pose finishPose; + boolean a; + boolean x; + + + + + private PathChain driveStartPosShootPos, shootToPrePickup, INTAKE, LAUNCH_PICKUP_1, SHOOT_TO_PREPICKUP2, + INTAKEPOS2, LAUNCHPOSE2, PRELOADPOSE3, INTAKEPOSE3, LAUNCHPOSE3, FINISH; + + @Override + public void init() { + + pathState = StateMachine.DRIVE_STARPOS_SHOOT_POS; + + pathTimer = new Timer(); + timer = new Timer(); + pathTimer.resetTimer(); + + + + follower = Constants.createFollower(hardwareMap); + + shooter = hardwareMap.get(DcMotorEx.class, "shooter"); + intake = hardwareMap.get(DcMotor .class, "intake"); + leftPusher = hardwareMap.get(CRServo.class, "leftPusher"); + rightPusher = hardwareMap.get(CRServo.class, "rightPusher"); + + pathStarted = false; + + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + } + public void start () { + timer.resetTimer(); + } + + @Override + public void init_loop() { + if (!pathsBuilt) { + InitializePoseValues(); + } + } + + private void InitializePoseValues() { + + if (gamepad1.y) { + telemetry.addLine("Big blue selected"); + } + else if (gamepad1.b) { + telemetry.addLine("Big red selected"); + } + else if (gamepad1.a) { + telemetry.addLine("Small red selected"); + } + else if (gamepad1.x) { + telemetry.addLine("Small blue selected"); + } + else { + telemetry.addLine("Select auto mode by pressing: " + + "X(Small blue) / Y(Big blue) / B(Big red) / A(Small red)"); + telemetry.update(); + return; + } + + telemetry.update(); + + if (gamepad1.y) { + startPose = new Pose(21.350906095551892, 124.7841845140033, Math.toRadians(138)); + shootPose = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(138)); + preloadPose = new Pose(47.44645799011532, 85.4036243822076, Math.toRadians(-180)); + intakePose = new Pose(26.332784184514, 91.57166392092257, Math.toRadians(-180)); + launchPose1 = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(138)); + preloadPose2 = new Pose(64.1203212890625, 62.63519836425781, Math.toRadians(-180)); + intakePose2 = new Pose(17.31795716639209, 61.680395387149915, Math.toRadians(-180)); + launchPose2 = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(132)); + preloadPose3 = new Pose(61.205761230468745, 37.861437866210935, Math.toRadians(-180)); + intakePose3 = new Pose(19.453047775947276, 43.6507413509061, Math.toRadians(-180)); + launchPose3 = new Pose(49.44645799011532, 97.36738056013179, Math.toRadians(132)); + finishPose = new Pose(35.110378912685334, 71.40691927512356, Math.toRadians(-180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.b) { + startPose = new Pose( + 122.64909390444811, 124.7841845140033, Math.toRadians(48)); + + shootPose = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose = new Pose( + 96.35555555555555, 95.11111111111111, Math.toRadians(0)); + + intakePose = new Pose( + 136.88888888888889, 95.46666666666667, Math.toRadians(0)); + + launchPose1 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose2 = new Pose( + 86.39999999999999, 71.82222222222222, Math.toRadians(0)); + + intakePose2 = new Pose( + 136.17777777777778, 72, Math.toRadians(0)); + + launchPose2 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + preloadPose3 = new Pose( + 84.08888888888889, 47.28888888888889, Math.toRadians(0)); + + intakePose3 = new Pose( + 143.2888888888889, 47.28888888888889, Math.toRadians(0)); + + launchPose3 = new Pose( + 96.55354200988468, 95.36738056013179, Math.toRadians(48)); + + finishPose = new Pose( + 109.15555555555555, 84.08888888888889, Math.toRadians(0)); + + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.a) { + a = true; + startPose = new Pose( + 78.4, + 8.000000000000004, + Math.toRadians(0) + ); + + shootPose = new Pose( + 72.17777777777778, + 23.28888888888889, + Math.toRadians(70) + ); + + preloadPose = new Pose( + 102.93333333333334, + 10.422222222222222, + Math.toRadians(0) + ); + + + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + + if (gamepad1.x) { + x = true; + startPose = new Pose(65.6, 8.000000000000004, Math.toRadians(90)); + shootPose = new Pose(71.82222222222222, 23.28888888888889, Math.toRadians(110)); + preloadPose = new Pose(41.06666666666667, 9.422222222222222, Math.toRadians(180)); + buildPaths(); + follower.setPose(startPose); + pathsBuilt = true; + + } + } + + private void buildPaths() { + + driveStartPosShootPos = follower.pathBuilder() + .addPath(new BezierLine(startPose, shootPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), shootPose.getHeading()) + .build(); + + shootToPrePickup = follower.pathBuilder() + .addPath(new BezierLine(shootPose, preloadPose)) + .setLinearHeadingInterpolation(shootPose.getHeading(), preloadPose.getHeading()) + .build(); + + INTAKE = follower.pathBuilder() + .addPath(new BezierLine(preloadPose, intakePose)) + .setLinearHeadingInterpolation(preloadPose.getHeading(), intakePose.getHeading()) + .build(); + + LAUNCH_PICKUP_1 = follower.pathBuilder() + .addPath(new BezierLine(intakePose, launchPose1)) + .setLinearHeadingInterpolation(intakePose.getHeading(), launchPose1.getHeading()) + .build(); + + SHOOT_TO_PREPICKUP2 = follower.pathBuilder() + .addPath(new BezierLine(launchPose1, preloadPose2)) + .setLinearHeadingInterpolation(launchPose1.getHeading(), preloadPose2.getHeading()) + .build(); + + INTAKEPOS2 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose2, intakePose2)) + .setLinearHeadingInterpolation(preloadPose2.getHeading(), intakePose2.getHeading()) + .build(); + + LAUNCHPOSE2 = follower.pathBuilder() + .addPath(new BezierLine(intakePose2, launchPose2)) + .setLinearHeadingInterpolation(intakePose2.getHeading(), launchPose2.getHeading()) + .build(); + + PRELOADPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(launchPose2, preloadPose3)) + .setLinearHeadingInterpolation(launchPose2.getHeading(), preloadPose3.getHeading()) + .build(); + + INTAKEPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(preloadPose3, intakePose3)) + .setLinearHeadingInterpolation(preloadPose3.getHeading(), intakePose3.getHeading()) + .build(); + + LAUNCHPOSE3 = follower.pathBuilder() + .addPath(new BezierLine(intakePose3, launchPose3)) + .setLinearHeadingInterpolation(intakePose3.getHeading(), launchPose3.getHeading()) + .build(); + + FINISH = follower.pathBuilder() + .addPath(new BezierLine(launchPose3, finishPose)) + .setLinearHeadingInterpolation(launchPose3.getHeading(), finishPose.getHeading()) + .build(); + } + + private void statePathUpdate() { + double shooterVelocity; + + if (pathState == null) + return; + + if (x ^ a) { + shooterVelocity = -1990; + } + else { + shooterVelocity = -1150; + } + + + intake.setPower(-1); + + + switch (pathState) { + + case DRIVE_STARPOS_SHOOT_POS: + startPath(driveStartPosShootPos); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(shooterVelocity); + + if (!follower.isBusy()) { + if (shooter.getVelocity() <= shooterVelocity) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 4) { + shooter.setVelocity(shooterVelocity); + leftPusher.setPower(1); + } + } + if (pathTimer.getElapsedTimeSeconds() >= 8.5) { + setPathState(StateMachine.SHOOT_TO_PREPICKUP); + } + + + break; + + case SHOOT_TO_PREPICKUP: + startPath(shootToPrePickup); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy() && !x ^ !a) { + setPathState(StateMachine.INTAKE); + } + break; + + case INTAKE: + startPath(INTAKE); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_1); + break; + + case LAUNCH_PICKUP_1: + startPath(LAUNCH_PICKUP_1); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.5) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP2); + } + + + + } + break; + + case SHOOT_TO_PREPICKUP2: + startPath(SHOOT_TO_PREPICKUP2); // ✅ ONLY FIX + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE2); + break; + + case INTAKE2: + startPath(INTAKEPOS2); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_2); + break; + + case LAUNCH_PICKUP_2: + startPath(LAUNCHPOSE2); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.75) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.2) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.SHOOT_TO_PREPICKUP3); + } + } + break; + + case SHOOT_TO_PREPICKUP3: + startPath(PRELOADPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.INTAKE3); + break; + + case INTAKE3: + startPath(INTAKEPOSE3); + leftPusher.setPower(0); + rightPusher.setPower(0); + runIntakeWhileBusy(); + if (!follower.isBusy()) setPathState(StateMachine.LAUNCH_PICKUP_3); + break; + + case LAUNCH_PICKUP_3: + startPath(LAUNCHPOSE3); + if (shooter.getVelocity() <= shooterVelocity && pathTimer.getElapsedTimeSeconds() >= 1.85) { + leftPusher.setPower(1); + if (pathTimer.getElapsedTimeSeconds() >= 2.25) { + rightPusher.setPower(-1); + } + if (pathTimer.getElapsedTimeSeconds() >= 5.75) { + runShooterPusherWait(StateMachine.FINISH); + } + } + break; + + case FINISH: + if (!pathStarted) follower.followPath(FINISH, true); + pathStarted = true; + intake.setPower(0); + leftPusher.setPower(0); + rightPusher.setPower(0); + shooter.setVelocity(0); + break; + } + } + + private void startPath(PathChain path) { + if (!pathStarted) { + follower.followPath(path, true); + pathStarted = true; + pathTimer.resetTimer(); + } + } + + private void runIntakeWhileBusy() { + intake.setPower(-1); + } + + private void runShooterPusherWait(StateMachine nextState) { + if (pathTimer.getElapsedTimeSeconds() >= 5) { + setPathState(nextState); + } + } + + private void setPathState(StateMachine newState) { + pathState = newState; + pathStarted = false; + pathTimer.resetTimer(); + } + + @Override + public void loop() { + follower.update(); + statePathUpdate(); + + telemetry.addData("STATE", pathState); + + Pose pose = follower.getPose(); + if (pose != null) { + telemetry.addData("x", pose.getX()); + telemetry.addData("y", pose.getY()); + telemetry.addData("heading", pose.getHeading()); + } + + telemetry.addData("followerBusy", follower.isBusy()); + telemetry.addData("pathTimer", pathTimer.getElapsedTimeSeconds()); + telemetry.addData("timer", timer.getElapsedTimeSeconds()); + telemetry.update(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index fa30420ac7cd..e61cd9777ad7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -1,19 +1,60 @@ package org.firstinspires.ftc.teamcode.pedroPathing; +import com.pedropathing.control.FilteredPIDFCoefficients; +import com.pedropathing.control.PIDFCoefficients; import com.pedropathing.follower.Follower; import com.pedropathing.follower.FollowerConstants; import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; +import com.pedropathing.ftc.localization.constants.PinpointConstants; import com.pedropathing.paths.PathConstraints; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + public class Constants { - public static FollowerConstants followerConstants = new FollowerConstants(); + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(10.6) + .forwardZeroPowerAcceleration(-42.51297137628084) + .lateralZeroPowerAcceleration(-53.639708599274925) + .translationalPIDFCoefficients(new PIDFCoefficients(0.03, 0, 0.03, 0.035)) + .headingPIDFCoefficients(new PIDFCoefficients (1, 0, 0.022, 0.025)) + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.0075, 0, 0, 0.6, 0.025)) + .centripetalScaling(0.001); + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(1) + .rightFrontMotorName("rightFront") + .rightRearMotorName("rightBack") + .leftRearMotorName("leftBack") + .leftFrontMotorName("leftFront") + .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE) + .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) + .xVelocity(61.837623896561276) + .yVelocity(52.13752806656005); + + public static PinpointConstants localizerConstants = new PinpointConstants() + .forwardPodY(-6) + .strafePodX(3.5) + .distanceUnit(DistanceUnit.INCH) + .hardwareMapName("imu2") + .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) + .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED); + + public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); public static Follower createFollower(HardwareMap hardwareMap) { return new FollowerBuilder(followerConstants, hardwareMap) + .pinpointLocalizer(localizerConstants) .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) .build(); } }