Initial commit

TeamCode/build.gradle

@@ -0,0 +1,28 @@
+//
+// build.gradle in TeamCode
+//
+// Most of the definitions for building your module reside in a common, shared
+// file 'build.common.gradle'. Being factored in this way makes it easier to
+// integrate updates to the FTC into your code. If you really need to customize
+// the build definitions, you can place those customizations in this file, but
+// please think carefully as to whether such customizations are really necessary
+// before doing so.
+
+
+// Custom definitions may go here
+
+// Include common definitions from above.
+apply from: '../build.common.gradle'
+apply from: '../build.dependencies.gradle'
+
+android {
+    namespace = 'org.firstinspires.ftc.teamcode'
+
+    packagingOptions {
+        jniLibs.useLegacyPackaging true
+    }
+}
+
+dependencies {
+    implementation project(':FtcRobotController')
+}

TeamCode/src/main/AndroidManifest.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<!-- Note: the actual manifest file used in your APK merges this file with contributions
+     from the modules on which your app depends (such as FtcRobotController, etc).
+     So it won't ultimately be as empty as it might here appear to be :-) -->
+
+<!-- The package name here determines the package for your R class and your BuildConfig class -->
+<manifest
+    xmlns:android="http://schemas.android.com/apk/res/android">
+    <application/>
+</manifest>

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java

@@ -0,0 +1,83 @@
+package org.firstinspires.ftc.teamcode.lib;
+
+import android.os.SystemClock;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+// PANELS IMPORTS
+import com.bylazar.telemetry.JoinedTelemetry;
+
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+import org.firstinspires.ftc.teamcode.util.FPSCounter;
+
+import java.util.List;
+
+public abstract class BaseOpMode extends OpMode {
+    protected final SubsysManager manager = new SubsysManager();
+    protected final FPSCounter fps = new FPSCounter();
+    private final ElapsedTime telemetryTimer = new ElapsedTime();
+    private List<LynxModule> allHubs;
+
+    protected Telemetry unifiedTelemetry;
+
+    protected abstract void setupSubsystems();
+    protected abstract void stateMachineUpdate();
+    protected void onInit() {}
+
+    @Override
+    public final void init() {
+        unifiedTelemetry = new JoinedTelemetry(telemetry);
+
+        allHubs = hardwareMap.getAll(LynxModule.class);
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+
+        setupSubsystems();
+
+        manager.initAll(hardwareMap);
+
+        onInit();
+    }
+
+    @Override
+    public final void start() {
+        fps.reset();
+        fps.startLoop();
+        telemetryTimer.reset();
+    }
+
+    @Override
+    public final void loop() {
+        for (LynxModule hub : allHubs) {
+            hub.clearBulkCache();
+        }
+
+        fps.startLoop();
+        stateMachineUpdate();
+
+        manager.updateAll();
+
+        if (telemetryTimer.milliseconds() >= Constants.GLOBAL.TELEMETRY_DELAY_MS) {
+
+            manager.publishTelemetryAll(unifiedTelemetry);
+
+            if (Constants.GLOBAL.DEBUG_MODE) {
+                unifiedTelemetry.addData("Loop Time (ms)", fps.getCurrentLoopTime());
+                unifiedTelemetry.addData("Target FPS", Constants.GLOBAL.TARGET_FPS);
+                unifiedTelemetry.addData("Current FPS", fps.getAverageFps());
+            }
+
+            unifiedTelemetry.update();
+            telemetryTimer.reset();
+        }
+
+        long sleepTime = fps.getSyncTime(Constants.GLOBAL.TARGET_FPS);
+        if (sleepTime > 0) {
+            SystemClock.sleep(sleepTime);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java

@@ -0,0 +1,51 @@
+package org.firstinspires.ftc.teamcode.lib;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class PIDController {
+    private double kP, kI, kD;
+
+    private double lastError = 0;
+    private double integralSum = 0;
+    private final ElapsedTime timer = new ElapsedTime();
+    private boolean hasRunOnce = false;
+
+    public PIDController(double kP, double kI, double kD) {
+        setCoefficients(kP, kI, kD);
+    }
+
+    public void setCoefficients(double kP, double kI, double kD) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+    }
+
+    public double calculate(double target, double current) {
+        double error = target - current;
+
+        if (!hasRunOnce) {
+            timer.reset();
+            lastError = error;
+            hasRunOnce = true;
+            return kP * error;
+        }
+
+        double dt = timer.seconds();
+        timer.reset();
+
+        if (dt == 0.0) return 0.0;
+
+        double derivative = (error - lastError) / dt;
+        integralSum += (error * dt);
+        lastError = error;
+
+        return (kP * error) + (kI * integralSum) + (kD * derivative);
+    }
+
+    public void reset() {
+        integralSum = 0;
+        lastError = 0;
+        hasRunOnce = false;
+        timer.reset();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/SubsysManager.java

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.lib;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class SubsysManager {
+    private final List<Subsystem> subsystems = new ArrayList<>();
+
+    public void register(Subsystem... subs) {
+        subsystems.addAll(Arrays.asList(subs));
+    }
+
+    public void initAll(HardwareMap hwMap) {
+        for (Subsystem sub : subsystems) {
+            sub.init(hwMap);
+        }
+    }
+
+    public void updateAll() {
+        for (Subsystem sub : subsystems) {
+            sub.update();
+        }
+    }
+
+    public void publishTelemetryAll(Telemetry telemetry) {
+        for (Subsystem sub : subsystems) {
+            sub.publishTelemetry(telemetry);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Subsystem.java

@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.lib;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+public abstract class Subsystem {
+    public abstract void init(HardwareMap hwMap);
+
+    public abstract void update();
+
+    public void publishTelemetry(Telemetry telemetry) {}
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java

@@ -0,0 +1,19 @@
+package org.firstinspires.ftc.teamcode.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();
+    }
+}

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""

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java

@@ -0,0 +1,94 @@
+package org.firstinspires.ftc.teamcode.subsys;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+import org.firstinspires.ftc.teamcode.lib.Subsystem;
+
+public class Drivetrain extends Subsystem {
+
+    // Subsystem Micro-States
+    public enum DriveState {
+        NORMAL,
+        PRECISION,
+        LOCKED // Safety state to halt motors instantly
+    }
+
+    private DriveState currentState = DriveState.NORMAL;
+
+    private DcMotor frontLeft, frontRight, backLeft, backRight;
+
+    private double driveY = 0.0;
+    private double driveX = 0.0;
+    private double driveTurn = 0.0;
+
+    @Override
+    public void init(HardwareMap hwMap) {
+        frontLeft = hwMap.get(DcMotor.class, Constants.DRIVE.FL_NAME);
+        frontRight = hwMap.get(DcMotor.class, Constants.DRIVE.FR_NAME);
+        backLeft = hwMap.get(DcMotor.class, Constants.DRIVE.BL_NAME);
+        backRight = hwMap.get(DcMotor.class, Constants.DRIVE.BR_NAME);
+
+        frontRight.setDirection(DcMotor.Direction.REVERSE);
+        backRight.setDirection(DcMotor.Direction.REVERSE);
+
+        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    // Command Hooks for the OpMode to control this subsystem
+    public void setTargetState(DriveState newState) {
+        this.currentState = newState;
+    }
+
+    public void setDriveVectors(double y, double x, double turn) {
+        this.driveY = y;
+        this.driveX = x;
+        this.driveTurn = turn;
+    }
+
+    @Override
+    public void update() {
+        if (currentState == DriveState.LOCKED) {
+            frontLeft.setPower(0); frontRight.setPower(0);
+            backLeft.setPower(0); backRight.setPower(0);
+            return;
+        }
+
+        double speedMultiplier = (currentState == DriveState.PRECISION)
+                ? Constants.DRIVE.PRECISION_SPEED
+                : Constants.DRIVE.NORMAL_SPEED;
+
+        double flPower = (driveY + driveX + driveTurn) * speedMultiplier;
+        double frPower = (driveY - driveX - driveTurn) * speedMultiplier;
+        double blPower = (driveY - driveX + driveTurn) * speedMultiplier;
+        double brPower = (driveY + driveX - driveTurn) * speedMultiplier;
+
+        // Normalization (prevents math outputting numbers > 1.0)
+        double max = Math.max(Math.abs(flPower), Math.abs(frPower));
+        max = Math.max(max, Math.abs(blPower));
+        max = Math.max(max, Math.abs(brPower));
+
+        if (max > 1.0) {
+            flPower /= max; frPower /= max;
+            blPower /= max; brPower /= max;
+        }
+
+        frontLeft.setPower(flPower);
+        frontRight.setPower(frPower);
+        backLeft.setPower(blPower);
+        backRight.setPower(brPower);
+    }
+
+    @Override
+    public void publishTelemetry(Telemetry telemetry) {
+        if (Constants.GLOBAL.DEBUG_MODE) {
+            telemetry.addData("Drive State", currentState.toString());
+            telemetry.addData("Inputs (Y/X/T)", String.format("%.2f / %.2f / %.2f", driveY, driveX, driveTurn));
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java

@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.bylazar.configurables.annotations.Configurable;
+
+@Configurable
+public class Constants {
+
+    @Configurable
+    public static class GLOBAL {
+        public static double TELEMETRY_DELAY_MS = 250.0;
+        public static boolean DEBUG_MODE = true;
+        public static int TARGET_FPS = 0; // No target FPS (UNCAPPED)
+    }
+
+    @Configurable
+    public static class DRIVE {
+        // Hardware Names
+        public static final String FL_NAME = "fl";
+        public static final String FR_NAME = "fr";
+        public static final String BL_NAME = "bl";
+        public static final String BR_NAME = "br";
+
+        // Speed Constants
+        public static double NORMAL_SPEED = 0.6;
+        public static double PRECISION_SPEED = 0.35;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java

@@ -0,0 +1,56 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import org.firstinspires.ftc.teamcode.lib.BaseOpMode;
+import org.firstinspires.ftc.teamcode.subsys.Drivetrain;
+import org.firstinspires.ftc.teamcode.util.AutoTransfer;
+
+@TeleOp(name = "Competition TeleOp", group = "Main")
+public class teleOp extends BaseOpMode {
+
+    public enum GlobalState {
+        IDLE,
+        SCORING
+    }
+
+    private GlobalState robotState = GlobalState.IDLE;
+    private Drivetrain drive;
+
+    @Override
+    protected void setupSubsystems() {
+        drive = new Drivetrain();
+        manager.register(drive);
+    }
+
+    @Override
+    protected void onInit() {
+        if (AutoTransfer.hasValidData()) {
+            unifiedTelemetry.addData("Loaded Alliance", AutoTransfer.getAllianceString());
+            unifiedTelemetry.update();
+        }
+    }
+
+    @Override
+    protected void stateMachineUpdate() {
+        drive.setDriveVectors(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x);
+
+        switch (robotState) {
+
+            case IDLE:
+                drive.setTargetState(Drivetrain.DriveState.NORMAL);
+
+                if (gamepad1.aWasPressed()) {
+                    robotState = GlobalState.SCORING;
+                }
+                break;
+
+            case SCORING:
+                drive.setTargetState(Drivetrain.DriveState.PRECISION);
+
+                if (gamepad1.bWasPressed()) {
+                    robotState = GlobalState.IDLE;
+                }
+                break;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AutoTransfer.java

@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.pedropathing.geometry.Pose;
+
+public class AutoTransfer {
+
+    public static Pose transferPose = new Pose(0, 0, 0);
+
+    public static int alliance = 0;
+
+    public static int motifPattern = 0;
+
+    public static boolean isAutonRan = false;
+
+    public static void reset() {
+        transferPose = new Pose(0, 0, 0);
+        alliance = 0;
+        motifPattern = 0;
+        isAutonRan = false;
+    }
+
+    public static void initAuton(int allianceColor) {
+        reset();
+        alliance = allianceColor;
+        isAutonRan = true;
+    }
+
+    public static void setMotifPattern(int pattern) {
+        motifPattern = pattern;
+    }
+
+    public static void updatePose(Pose pose) {
+        transferPose = pose;
+    }
+
+    public static String getAllianceString() {
+        switch (alliance) {
+            case 1: return "BLUE";
+            case 2: return "RED";
+            default: return "UNKNOWN";
+        }
+    }
+    public static String getMotifString() {
+        switch (motifPattern) {
+            case 1: return "Pattern 1 (GPP)";
+            case 2: return "Pattern 2 (PGP)";
+            case 3: return "Pattern 3 (PPG)";
+            default: return "UNKNOWN";
+        }
+    }
+
+    public static boolean hasValidData() {
+        return isAutonRan && alliance != 0;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java

@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class FPSCounter {
+    private final ElapsedTime loopTimer = new ElapsedTime();
+    private double loopTimeSum = 0;
+    private int loopCount = 0;
+    private double currentLoopTime = 0;
+    private double lastTime = 0;
+
+    public FPSCounter() {
+        reset();
+    }
+
+    public void startLoop() {
+        double now = loopTimer.milliseconds();
+        if (loopCount > 0) {
+            currentLoopTime = now - lastTime;
+            loopTimeSum += currentLoopTime;
+        }
+        lastTime = now;
+        loopCount++;
+    }
+
+    public long getSyncTime(int targetFps) {
+        if (targetFps <= 0) return 0;
+        
+        double targetMs = 1000.0 / targetFps;
+        double now = loopTimer.milliseconds();
+        double elapsedWorkMs = now - lastTime;
+        double remainingMs = targetMs - elapsedWorkMs;
+
+        return (remainingMs > 0) ? (long) remainingMs : 0;
+    }
+
+    public double getCurrentFps() {
+        return (currentLoopTime > 0) ? 1000.0 / currentLoopTime : 0;
+    }
+
+    public double getAverageFps() {
+        if (loopCount <= 1) return 0;
+        return 1000.0 / (loopTimeSum / (loopCount - 1));
+    }
+
+    public double getCurrentLoopTime() {
+        return currentLoopTime;
+    }
+
+    public double getAverageLoopTime() {
+        if (loopCount <= 1) return 0;
+        return loopTimeSum / (loopCount - 1);
+    }
+
+    public void reset() {
+        loopTimer.reset();
+        lastTime = 0;
+        loopTimeSum = 0;
+        loopCount = 0;
+        currentLoopTime = 0;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OATDrawing.java

@@ -0,0 +1,58 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.bylazar.field.FieldManager;
+import com.bylazar.field.PanelsField;
+import com.bylazar.field.Style;
+import com.pedropathing.geometry.*;
+import com.pedropathing.math.*;
+import com.pedropathing.paths.*;
+
+public class OATDrawing {
+    public static final double ROBOT_RADIUS = 9;
+    private static final FieldManager panelsField = PanelsField.INSTANCE.getField();
+
+    private static final Style robotLook = new Style("", "#3F51B5", 0.75);
+
+    private static final Style goalLook = new Style("", "#F44336", 0.9);
+
+    private static final Style vectorLook = new Style("", "#4CAF50", 0.8);
+
+    private static final Style visionLook = new Style("", "#FFD700", 0.9);
+
+    public static void init() {
+        panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING());
+    }
+    public static void drawOATState(Pose robotPose, Pose goalPose, boolean hasVisionLock) {
+        if (robotPose == null || goalPose == null) return;
+
+        drawRobot(robotPose, robotLook);
+
+        panelsField.setStyle(goalLook);
+        panelsField.moveCursor(goalPose.getX(), goalPose.getY());
+        panelsField.circle(4);
+
+        panelsField.setStyle(hasVisionLock ? visionLook : vectorLook);
+        panelsField.moveCursor(robotPose.getX(), robotPose.getY());
+        panelsField.line(goalPose.getX(), goalPose.getY());
+
+        panelsField.update();
+    }
+
+    private 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 x2 = pose.getX() + v.getXComponent();
+        double y2 = pose.getY() + v.getYComponent();
+
+        panelsField.moveCursor(pose.getX(), pose.getY());
+        panelsField.line(x2, y2);
+    }
+}

TeamCode/src/main/res/raw/readme.md

@@ -0,0 +1 @@
+Place your sound files in this folder to use them as project resources.

TeamCode/src/main/res/values/strings.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<resources>
+
+</resources>

TeamCode/src/main/res/xml/teamwebcamcalibrations.xml

@@ -0,0 +1,161 @@
+<?xml version='1.0' encoding='UTF-8' standalone='yes' ?>
+<!-- 
+  This file can provide additional camera calibration settings beyond those built into the SDK itself.
+  Each calibration is for a particular camera (indicated by USB vid & pid pair) and a particular
+  capture resolution for the camera. Note: it is very important when capturing images used to calibrate
+  a camera that the image acquisition tool can actually control this capture resolution within the camera
+  itself and that you use this setting correctly. Many image acquistion tools do not in fact provide
+  this level of control.
+
+  Beyond simply providing additional, new camera calibrations, calibrations provided herein can
+  *replace/update* those that are builtin to the SDK. This matching is keyed, of course, by the
+  (vid, pid, size) triple. Further, if such a calibration has the 'remove' attribute with value 'true',
+  any existing calibration with that key is removed (and the calibration itself not added).
+
+  Calibrations are internally processed according to aspect ratio. If a format is requested in a size
+  that is not calibrated, but a calibration does exist for the same aspect ratio on the same camera,
+  then the latter will be scaled to accommodate the request. For example, if a 640x480 calibration
+  is requested but only a 800x600 calibration exists for that camera, then the 800x600 is scaled
+  down to service the 640x480 request.
+
+  Further, it is important to note that if *no* calibrations exist for a given camera, then Vuforia
+  is offered the entire range of capture resolutions that the hardware can support (and it does its
+  best to deal with the lack of calibration). However, if *any* calibrations are provided for a camera,
+  then capture resolutions in those aspect ratios supported by the camera for which any calibrations
+  are *not* provided are *not* offered. Thus, if you calibrate a camera but fail to calibrate all
+  the camera's supported aspect ratios, you limit the choices of capture resolutions that Vuforia can
+  select from.
+
+  One image acquisition program that supports control of camera capture resolution is YouCam 7:
+    https://www.cyberlink.com/products/youcam/features_en_US.html
+
+  Programs that can process acquired images to determine camera calibration settings include:
+    https://www.3dflow.net/3df-zephyr-free/ (see "Utilities/Images/Launch Camera Calibration" therein)
+    http://graphics.cs.msu.ru/en/node/909
+  Note that the type of images that must be acquired in order to calibrate is specific to the
+  calibration software used.
+
+  The required contents are illustrated here by example. Note that for the attribute names, both the
+  camelCase or the underscore_variations are supported; they are equivalent. The attributes for
+  each Calibration are as follows:
+
+    size (int pair): space separated camera resolution (width, height).
+    focalLength (float pair): space separated focal length value.
+    principalPoint (float pair): space separated principal point values (width, height).
+    distortionCoefficients (an 8-element float array): distortion coefficients in the following form
+        (r:radial, t:tangential): [r0, r1, t0, t1, r2, r3, r4, r5]
+        see https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
+
+  The examples here are commented out as the values are built-in to the FTC SDK. They serve instead
+  here as examples on how make your own.
+
+-->
+<Calibrations>
+
+    <!-- ======================================================================================= -->
+
+    <!-- Microsoft Lifecam HD 3000 v1, Calibrated by PTC using unknown tooling -->
+    <!-- <Camera vid="Microsoft" pid="0x0779">
+        <Calibration
+            size="640 480"
+            focalLength="678.154f, 678.17f"
+            principalPoint="318.135f, 228.374f"
+            distortionCoefficients="0.154576f, -1.19143f, 0f, 0f, 2.06105f, 0f, 0f, 0f"
+            />
+    </Camera> -->
+
+    <!-- ======================================================================================= -->
+
+    <!-- Microsoft Lifecam HD 3000 v2, Calibrated by PTC using unknown tooling -->
+    <!-- <Camera vid="Microsoft" pid="0x0810">
+        <Calibration
+            size="640 480"
+            focalLength="678.154f, 678.17f"
+            principalPoint="318.135f, 228.374f"
+            distortionCoefficients="0.154576f, -1.19143f, 0f, 0f, 2.06105f, 0f, 0f, 0f"
+            />
+    </Camera> -->
+
+    <!-- ======================================================================================= -->
+
+    <!-- Logitech HD Webcam C310, Calibrated by by Robert Atkinson, 2018.05.30 using 3DF Zephyr -->
+    <!-- <Camera vid="Logitech" pid="0x081B">
+        <Calibration
+            size="640 480"
+            focalLength="821.993f, 821.993f"
+            principalPoint="330.489f, 248.997f"
+            distortionCoefficients="-0.018522, 1.03979, 0, 0, -3.3171, 0, 0, 0"
+            />
+
+        <Calibration
+            size="640 360"
+            focalLength="715.307f, 715.307f"
+            principalPoint="319.759f, 188.917f"
+            distortionCoefficients="-0.0258948, 1.06258, 0, 0, -3.40245, 0, 0, 0"
+            />
+    </Camera> -->
+
+    <!-- ======================================================================================= -->
+
+    <!-- Logitech HD Pro Webcam C920, Calibrated by Robert Atkinson, 2018.05.30 using 3DF Zephyr -->
+    <!-- <Camera vid="Logitech" pid="0x082D">
+
+        <Calibration
+            size="640 480"
+            focalLength="622.001f, 622.001f"
+            principalPoint="319.803f, 241.251f"
+            distortionCoefficients="0.1208, -0.261599, 0, 0, 0.10308, 0, 0, 0"
+            />
+
+        <Calibration
+            size="800 600"
+            focalLength="775.79f, 775.79f"
+            principalPoint="400.898f, 300.79f"
+            distortionCoefficients="0.112507, -0.272067, 0, 0, 0.15775, 0, 0, 0"
+            />
+
+        <Calibration
+            size="640 360"
+            focalLength="463.566f, 463.566f"
+            principalPoint="316.402f, 176.412f"
+            distortionCoefficients="0.111626 , -0.255626, 0, 0, 0.107992, 0, 0, 0"
+            />
+
+        <Calibration
+            size="1920, 1080"
+            focalLength="1385.92f , 1385.92f"
+            principalPoint="951.982f , 534.084f"
+            distortionCoefficients="0.117627, -0.248549, 0, 0, 0.107441, 0, 0, 0"
+            />
+
+        <Calibration
+            size="800, 448"
+            focalLength="578.272f , 578.272f"
+            principalPoint="402.145f , 221.506f"
+            distortionCoefficients="0.12175, -0.251652 , 0, 0, 0.112142, 0, 0, 0"
+            />
+
+        <Calibration
+            size="864, 480"
+            focalLength="626.909f , 626.909f"
+            principalPoint="426.007f , 236.834f"
+            distortionCoefficients="0.120988, -0.253336 , 0, 0, 0.102445, 0, 0, 0"
+            />
+
+    </Camera> -->
+
+    <!-- ======================================================================================= -->
+
+    <!-- Logitech HD Webcam C270, Calibrated by Noah Andrews, 2019.03.13 using 3DF Zephyr -->
+    <!--<Camera vid="Logitech" pid="0x0825">
+        <Calibration
+            size="640 480"
+            focalLength="822.317f, 822.317f"
+            principalPoint="319.495f, 242.502f"
+            distortionCoefficients="-0.0449369, 1.17277, 0, 0, -3.63244, 0, 0, 0"
+            />
+    </Camera> -->
+
+    <!-- ======================================================================================= -->
+
+</Calibrations>