From b0ebcc8c409044c61d392761f7d13472842a2293 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sun, 7 Sep 2025 18:30:46 -0500 Subject: [PATCH 01/39] changed readme.md for team setup --- .../org/firstinspires/ftc/teamcode/readme.md | 205 ++++++++++-------- 1 file changed, 112 insertions(+), 93 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md index 4d1da42..ee3146b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -1,131 +1,150 @@ -## TeamCode Module +# Team FTC Git Workflow Guide -Welcome! +This document explains how to set up the FTC project on your computer and the rules for working with branches. -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 +## 1. Cloning the Repository -The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own. +1. Open a terminal (or the terminal inside Android Studio). +2. Navigate to the folder where you want to keep the project. +3. Run: -Sample opmodes exist in the FtcRobotController module. -To locate these samples, find the FtcRobotController module in the "Project/Android" tab. + ```bash + git clone https://github.com/KeshavAnandCode/DecodeFTCMain.git + cd DecodeFTCMain + ``` -Expand the following tree elements: - FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples +4. Verify your remotes: -### Naming of Samples + ```bash + git remote -v + ``` -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. + You should see: + ``` + origin https://github.com/KeshavAnandCode/DecodeFTCMain.git (fetch) + origin https://github.com/KeshavAnandCode/DecodeFTCMain.git (push) + upstream https://github.com/FIRST-Tech-Challenge/FtcRobotController.git (fetch) + upstream https://github.com/FIRST-Tech-Challenge/FtcRobotController.git (push) + ``` -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: +## 2. Keeping `master` Clean -Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure - of a particular style of OpMode. These are bare bones examples. +- `master` should only contain clean, tested code. +- Nobody should ever code directly on `master`. +- To stay up to date: -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. + ```bash + git checkout master + git fetch upstream + git merge upstream/master + git push origin master + ``` -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. +## 3. Creating a Feature Branch -After the prefix, other conventions will apply: +Whenever you start a new task (feature, fix, experiment): -* 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 +1. Update `master` (see above). +2. Create a new branch from `master`: -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. + ```bash + git checkout master + git pull origin master + git checkout -b feature/short-description + ``` -This is done inside Android Studio directly, using the following steps: +### Branch Naming Standard - 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: +Branches **must** follow the format: ``` - @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. +Where `` is one of: +- `feature/` → new functionality +- `fix/` → bug fixes +- `experiment/` → prototypes or tests +- `docs/` → documentation updates +- `chore/` → maintenance or cleanup -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. +Examples: +- `feature/autonomous-path` +- `fix/motor-init` +- `experiment/vision-test` +- `docs/setup-instructions` +- `chore/gradle-update` +**Rules for names:** +- Use lowercase letters and hyphens (`-`) only. +- Keep it short but clear (3–5 words). +- One branch = one task. Never mix unrelated work. +--- -## ADVANCED Multi-Team App management: Cloning the TeamCode Module +## 4. Working on Your Branch -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. +- Make changes in Android Studio. +- Stage and commit your changes: -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). + ```bash + git add . + git commit -m "short message about what changed" + ``` -Selective Team phones can then be programmed by selecting the desired Module from the pulldown list -prior to clicking to the green Run arrow. +- Push your branch to GitHub: -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 :) + ```bash + git push origin feature/short-description + ``` -To clone TeamCode, do the following: +--- -Note: Some names start with "Team" and others start with "team". This is intentional. +## 5. Sharing Your Work -1) Using your operating system file management tools, copy the whole "TeamCode" - folder to a sibling folder with a corresponding new name, eg: "Team0417". +- Once your branch is ready: + 1. Open a Pull Request (PR) on GitHub to merge into `master`. + 2. At least one teammate should review before merging. -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". +## 6. Branching Rules -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" +**Do:** +- Always branch from `master`. +- Follow the naming standard exactly. +- Keep branches small and focused. +- Delete branches after they’re merged. -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 +**Don’t:** +- Don’t push commits directly to `master`. +- Don’t leave unfinished work on `master`. +- Don’t mix unrelated changes in one branch. + +--- + +## 7. Example Workflow + +```bash +# Get latest code +git checkout master +git fetch upstream +git merge upstream/master +git push origin master + +# Start a new feature +git checkout -b feature/teleop-improvements + +# Work on code, then commit +git add . +git commit -m "improved joystick scaling in TeleOp" + +# Push branch +git push origin feature/teleop-improvements +``` From 11219015ac2efebedb18e33e90f7921e61a2a784 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sun, 7 Sep 2025 18:50:44 -0500 Subject: [PATCH 02/39] Boilerplate for Robot.java --- .../firstinspires/ftc/teamcode/utils/Robot.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java new file mode 100644 index 0000000..ae78075 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class Robot { + + //Initialize Public Components + + public Robot (HardwareMap hardwareMap) { + + //Define components w/ hardware map + + } +} From ab5887a9c5afabcb4f39f462c66571e48c41c1d7 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sun, 7 Sep 2025 19:05:31 -0500 Subject: [PATCH 03/39] Setup Pedro --- FtcRobotController/build.gradle | 2 +- .../teamcode/libs/pedroPathing/Constants.java | 19 + .../teamcode/libs/pedroPathing/Tuning.java | 1325 +++++++++++++++++ build.dependencies.gradle | 13 + 4 files changed, 1358 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle index 9fa2169..8c796da 100644 --- a/FtcRobotController/build.gradle +++ b/FtcRobotController/build.gradle @@ -17,8 +17,8 @@ android { buildFeatures { buildConfig = true } + compileSdk 34 - compileSdkVersion 30 compileOptions { sourceCompatibility JavaVersion.VERSION_1_8 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java new file mode 100644 index 0000000..d596ce1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode.libs.pedroPathing; + +import com.pedropathing.follower.Follower; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.paths.PathConstraints; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class Constants { + public static FollowerConstants followerConstants = new FollowerConstants(); + + public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); + + public static Follower createFollower(HardwareMap hardwareMap) { + return new FollowerBuilder(followerConstants, hardwareMap) + .pathConstraints(pathConstraints) + .build(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java new file mode 100644 index 0000000..da2b0d6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java @@ -0,0 +1,1325 @@ +package org.firstinspires.ftc.teamcode.libs.pedroPathing; + +import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.changes; +import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.drawCurrent; +import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.drawCurrentAndHistory; +import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.follower; +import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.stopRobot; +import static org.firstinspires.ftc.teamcode.libs.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("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(); + } + + @Override + public void onLog(List lines) {} + + public static void drawCurrent() { + try { + Drawing.drawRobot(follower.getPose()); + Drawing.sendPacket(); + } catch (Exception e) { + throw new RuntimeException("Drawing failed " + e); + } + } + + public static void drawCurrentAndHistory() { + Drawing.drawPoseHistory(poseHistory); + drawCurrent(); + } + + /** 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() {} + + /** 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(); + drawCurrent(); + } + + @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); + + drawCurrentAndHistory(); + } +} + +/** + * 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.update(); + drawCurrent(); + } + + /** 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); + drawCurrent(); + } + + /** + * 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() / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); + telemetryM.update(telemetry); + + drawCurrentAndHistory(); + } +} + +/** + * 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.update(); + drawCurrent(); + } + + /** 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); + drawCurrent(); + } + + /** + * 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() / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); + telemetryM.update(telemetry); + + drawCurrentAndHistory(); + } +} + +/** + * 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.update(); + drawCurrent(); + } + + /** 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); + + drawCurrent(); + } + + /** + * 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); + + drawCurrentAndHistory(); + } +} + +/** + * 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() {} + + /** 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(); + drawCurrent(); + } + + /** 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(); + drawCurrentAndHistory(); + + + if (!end) { + if (Math.abs(follower.getPose().getX()) > DISTANCE) { + 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 right 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() {} + + /** + * 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 right."); + 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(); + drawCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run right 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(); + drawCurrentAndHistory(); + + if (!end) { + if (Math.abs(follower.getPose().getY()) > DISTANCE) { + 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() {} + + /** 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(); + drawCurrent(); + } + + /** 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(); + drawCurrentAndHistory(); + + 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 right 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() {} + + /** This initializes the drive motors as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run to the right 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(); + drawCurrent(); + } + + /** 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(); + drawCurrentAndHistory(); + + 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() {} + + /** 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(); + drawCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateTranslational(); + forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); + 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(); + drawCurrentAndHistory(); + + 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() {} + + /** + * 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(); + drawCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateHeading(); + forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); + 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(); + drawCurrentAndHistory(); + + 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() {} + + /** + * 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(); + drawCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateDrive(); + + forwards = follower.pathBuilder() + .setGlobalDeceleration() + .addPath(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))) + .setConstantHeadingInterpolation(0) + .build(); + + backwards = follower.pathBuilder() + .setGlobalDeceleration() + .addPath(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))) + .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(); + drawCurrentAndHistory(); + + 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() {} + + /** 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(); + drawCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); + 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(); + drawCurrentAndHistory(); + + 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() {} + + /** + * 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(); + drawCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + forwards = new Path(new BezierCurve(new Pose(), new Pose(Math.abs(DISTANCE),0), new Pose(Math.abs(DISTANCE),DISTANCE))); + backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE),DISTANCE), new Pose(Math.abs(DISTANCE),0), new Pose(0,0))); + + 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(); + drawCurrentAndHistory(); + 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(0, 0, Math.toRadians(0)); + private final Pose interPose = new Pose(24, -24, Math.toRadians(90)); + private final Pose endPose = new Pose(24, 24, 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(); + drawCurrentAndHistory(); + + if (follower.atParametricEnd()) { + follower.followPath(triangle, true); + } + } + + @Override + public void init() {} + + @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(); + drawCurrent(); + } + + /** 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(0, 0), new Pose(RADIUS, 0), new Pose(RADIUS, RADIUS))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) + .addPath(new BezierCurve(new Pose(RADIUS, RADIUS), new Pose(RADIUS, 2 * RADIUS), new Pose(0, 2 * RADIUS))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) + .addPath(new BezierCurve(new Pose(0, 2 * RADIUS), new Pose(-RADIUS, 2 * RADIUS), new Pose(-RADIUS, RADIUS))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) + .addPath(new BezierCurve(new Pose(-RADIUS, RADIUS), new Pose(-RADIUS, 0), new Pose(0, 0))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) + .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(); + drawCurrent(); + } + + @Override + public void init() {} + + /** + * 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(); + drawCurrentAndHistory(); + + 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.0 + ); + private static final Style historyLook = new Style( + "", "#4CAF50", 0.0 + ); + + /** + * 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(); + } +} \ No newline at end of file diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 1c07e8c..5356344 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -1,6 +1,8 @@ repositories { mavenCentral() google() // Needed for androidx + maven { url = 'https://maven.pedropathing.com' } //Pedro + maven { url = "https://mymaven.bylazar.com/releases" } //Panels } dependencies { @@ -13,5 +15,16 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:11.0.0' implementation 'org.firstinspires.ftc:Vision:11.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' + + implementation 'com.pedropathing:ftc:2.0.0' //PedroCore + implementation 'com.pedropathing:telemetry:0.0.6' //PedroTele + + implementation 'com.bylazar:fullpanels:1.0.2' //Panels + + + + + + } From a6312031894ae037bd9983a8efe272365d901f66 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sun, 7 Sep 2025 19:21:41 -0500 Subject: [PATCH 04/39] Setup RR --- .../ftc/teamcode/libs/RR/Drawing.java | 22 + .../ftc/teamcode/libs/RR/Localizer.java | 25 + .../ftc/teamcode/libs/RR/MecanumDrive.java | 496 +++++++++++++++++ .../ftc/teamcode/libs/RR/OTOSLocalizer.java | 68 +++ .../teamcode/libs/RR/PinpointLocalizer.java | 73 +++ .../ftc/teamcode/libs/RR/TankDrive.java | 509 ++++++++++++++++++ .../libs/RR/ThreeDeadWheelLocalizer.java | 113 ++++ .../libs/RR/TwoDeadWheelLocalizer.java | 143 +++++ .../libs/RR/messages/DriveCommandMessage.java | 24 + .../RR/messages/MecanumCommandMessage.java | 19 + .../MecanumLocalizerInputsMessage.java | 30 ++ .../libs/RR/messages/PoseMessage.java | 18 + .../libs/RR/messages/TankCommandMessage.java | 15 + .../messages/TankLocalizerInputsMessage.java | 17 + .../messages/ThreeDeadWheelInputsMessage.java | 17 + .../messages/TwoDeadWheelInputsMessage.java | 35 ++ .../libs/RR/tuning/LocalizationTest.java | 78 +++ .../libs/RR/tuning/ManualFeedbackTuner.java | 63 +++ .../teamcode/libs/RR/tuning/SplineTest.java | 39 ++ .../libs/RR/tuning/TuningOpModes.java | 321 +++++++++++ build.common.gradle | 2 +- build.dependencies.gradle | 6 + 22 files changed, 2132 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Drawing.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Localizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/OTOSLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TankDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/ThreeDeadWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/DriveCommandMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/MecanumCommandMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/MecanumLocalizerInputsMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/PoseMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TankCommandMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TankLocalizerInputsMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/ThreeDeadWheelInputsMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TwoDeadWheelInputsMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/LocalizationTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/ManualFeedbackTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/SplineTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/TuningOpModes.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Drawing.java new file mode 100644 index 0000000..764c4ee --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Drawing.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.libs.RR; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; + +public final class Drawing { + private Drawing() {} + + + public static void drawRobot(Canvas c, Pose2d t) { + final double ROBOT_RADIUS = 9; + + c.setStrokeWidth(1); + c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS); + + Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS); + Vector2d p1 = t.position.plus(halfv); + Vector2d p2 = p1.plus(halfv); + c.strokeLine(p1.x, p1.y, p2.x, p2.y); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Localizer.java new file mode 100644 index 0000000..970c400 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Localizer.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.libs.RR; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; + +/** + * Interface for localization methods. + */ +public interface Localizer { + void setPose(Pose2d pose); + + /** + * Returns the current pose estimate. + * NOTE: Does not update the pose estimate; + * you must call update() to update the pose estimate. + * @return the Localizer's current pose + */ + Pose2d getPose(); + + /** + * Updates the Localizer's pose estimate. + * @return the Localizer's current velocity estimate + */ + PoseVelocity2d update(); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java new file mode 100644 index 0000000..6cc00c9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java @@ -0,0 +1,496 @@ +package org.firstinspires.ftc.teamcode.libs.RR; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.*; +import com.acmerobotics.roadrunner.AngularVelConstraint; +import com.acmerobotics.roadrunner.DualNum; +import com.acmerobotics.roadrunner.HolonomicController; +import com.acmerobotics.roadrunner.MecanumKinematics; +import com.acmerobotics.roadrunner.MinVelConstraint; +import com.acmerobotics.roadrunner.MotorFeedforward; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Pose2dDual; +import com.acmerobotics.roadrunner.ProfileAccelConstraint; +import com.acmerobotics.roadrunner.Time; +import com.acmerobotics.roadrunner.TimeTrajectory; +import com.acmerobotics.roadrunner.TimeTurn; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TurnConstraints; +import com.acmerobotics.roadrunner.VelConstraint; +import com.acmerobotics.roadrunner.ftc.DownsampledWriter; +import com.acmerobotics.roadrunner.ftc.Encoder; +import com.acmerobotics.roadrunner.ftc.FlightRecorder; +import com.acmerobotics.roadrunner.ftc.LazyHardwareMapImu; +import com.acmerobotics.roadrunner.ftc.LazyImu; +import com.acmerobotics.roadrunner.ftc.LynxFirmware; +import com.acmerobotics.roadrunner.ftc.OverflowEncoder; +import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; +import com.acmerobotics.roadrunner.ftc.RawEncoder; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.libs.RR.messages.DriveCommandMessage; +import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumCommandMessage; +import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumLocalizerInputsMessage; +import org.firstinspires.ftc.teamcode.libs.RR.messages.PoseMessage; + +import java.lang.Math; +import java.util.Arrays; +import java.util.LinkedList; +import java.util.List; + +@Config +public final class MecanumDrive { + public static class Params { + // IMU orientation + // TODO: fill in these values based on + // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting + public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = + RevHubOrientationOnRobot.LogoFacingDirection.UP; + public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + + // drive model parameters + public double inPerTick = 1; + public double lateralInPerTick = inPerTick; + public double trackWidthTicks = 0; + + // feedforward parameters (in tick units) + public double kS = 0; + public double kV = 0; + public double kA = 0; + + // path profile parameters (in inches) + public double maxWheelVel = 50; + public double minProfileAccel = -30; + public double maxProfileAccel = 50; + + // turn profile parameters (in radians) + public double maxAngVel = Math.PI; // shared with path + public double maxAngAccel = Math.PI; + + // path controller gains + public double axialGain = 0.0; + public double lateralGain = 0.0; + public double headingGain = 0.0; // shared with turn + + public double axialVelGain = 0.0; + public double lateralVelGain = 0.0; + public double headingVelGain = 0.0; // shared with turn + } + + public static Params PARAMS = new Params(); + + public final MecanumKinematics kinematics = new MecanumKinematics( + PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick); + + public final TurnConstraints defaultTurnConstraints = new TurnConstraints( + PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel); + public final VelConstraint defaultVelConstraint = + new MinVelConstraint(Arrays.asList( + kinematics.new WheelVelConstraint(PARAMS.maxWheelVel), + new AngularVelConstraint(PARAMS.maxAngVel) + )); + public final AccelConstraint defaultAccelConstraint = + new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel); + + public final DcMotorEx leftFront, leftBack, rightBack, rightFront; + + public final VoltageSensor voltageSensor; + + public final LazyImu lazyImu; + + public final Localizer localizer; + private final LinkedList poseHistory = new LinkedList<>(); + + private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000); + private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000); + private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000); + private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000); + + public class DriveLocalizer implements Localizer { + public final Encoder leftFront, leftBack, rightBack, rightFront; + public final IMU imu; + + private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos; + private Rotation2d lastHeading; + private boolean initialized; + private Pose2d pose; + + public DriveLocalizer(Pose2d pose) { + leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront)); + leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack)); + rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); + rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); + + imu = lazyImu.get(); + + // TODO: reverse encoders if needed + // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + + this.pose = pose; + } + + @Override + public void setPose(Pose2d pose) { + this.pose = pose; + } + + @Override + public Pose2d getPose() { + return pose; + } + + @Override + public PoseVelocity2d update() { + PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity(); + PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity(); + PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity(); + PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity(); + + YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles(); + + FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage( + leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles)); + + Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS)); + + if (!initialized) { + initialized = true; + + lastLeftFrontPos = leftFrontPosVel.position; + lastLeftBackPos = leftBackPosVel.position; + lastRightBackPos = rightBackPosVel.position; + lastRightFrontPos = rightFrontPosVel.position; + + lastHeading = heading; + + return new PoseVelocity2d(new Vector2d(0.0, 0.0), 0.0); + } + + double headingDelta = heading.minus(lastHeading); + Twist2dDual