diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 436ce67..c5a0398 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -26,4 +26,6 @@ android { dependencies { implementation project(':FtcRobotController') annotationProcessor files('lib/OpModeAnnotationProcessor.jar') + implementation 'org.openftc:easyopencv:1.7.0' + implementation 'com.acmerobotics.roadrunner:core:0.5.5' } diff --git a/TeamCode/src/main/java/opmodes/Sandbox.java b/TeamCode/src/main/java/opmodes/Sandbox.java deleted file mode 100644 index 755a6a1..0000000 --- a/TeamCode/src/main/java/opmodes/Sandbox.java +++ /dev/null @@ -1,28 +0,0 @@ -package opmodes; - -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.hardware.Camera; -import org.firstinspires.ftc.teamcode.hardware.Robot; - -/* - * Demonstrates an empty iterative OpMode - */ -@TeleOp(name = "Sandbox", group = "Experimental") -public class Sandbox extends OpMode { - - private Camera camera; - - @Override - public void init() { - telemetry.addData("Status", "Initialized"); - this.camera = new Camera(this.hardwareMap); - this.camera.initTargetingCamera(); - } - - @Override - public void loop() { - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Button.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Button.java new file mode 100644 index 0000000..876a3af --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Button.java @@ -0,0 +1,30 @@ +package org.firstinspires.ftc.teamcode.controller; + +public class Button { + boolean currentState = false; + boolean lastState = false; + boolean justPressed = false; + boolean justReleased = false; + + public Button() {} + + public void update(boolean pressed) { + lastState = currentState; + currentState = pressed; + + justPressed = currentState && !lastState; + justReleased = !currentState && lastState; + } + + public boolean isPressed() { + return currentState; + } + + public boolean isJustPressed() { + return justPressed; + } + + public boolean isJustReleased() { + return justReleased; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Controller.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Controller.java new file mode 100644 index 0000000..75e6dea --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Controller.java @@ -0,0 +1,197 @@ +package org.firstinspires.ftc.teamcode.controller; + +import com.qualcomm.robotcore.hardware.Gamepad; + +public class Controller { + private final Gamepad gamepad; + + private final Joystick leftStick; + private final Joystick rightStick; + + private final Button leftStickButton; + private final Button rightStickButton; + + private final Button dLeft; + private final Button dRight; + private final Button dUp; + private final Button dDown; + + private final Button a; + private final Button b; + private final Button x; + private final Button y; + + private final Button leftBumper; + private final Button rightBumper; + + private final Button back; + private final Button start; + + private final Trigger leftTrigger; + private final Trigger rightTrigger; + + private final Button touchpad; + + private final Button allButtons; + + public Controller(Gamepad gamepad) { + this.gamepad = gamepad; + + leftStick = new Joystick(); + rightStick = new Joystick(); + + leftStickButton = new Button(); + rightStickButton = new Button(); + + dLeft = new Button(); + dRight = new Button(); + dUp = new Button(); + dDown = new Button(); + + a = new Button(); + b = new Button(); + x = new Button(); + y = new Button(); + + leftBumper = new Button(); + rightBumper = new Button(); + + back = new Button(); + start = new Button(); + + leftTrigger = new Trigger(); + rightTrigger = new Trigger(); + + touchpad = new Button(); + + allButtons = new Button(); + } + + + public void update() { + leftStick.update(gamepad.left_stick_x, -gamepad.left_stick_y); + rightStick.update(gamepad.right_stick_x, -gamepad.right_stick_y); + + leftStickButton.update(gamepad.left_stick_button); + rightStickButton.update(gamepad.right_stick_button); + + dLeft.update(gamepad.dpad_left); + dRight.update(gamepad.dpad_right); + dUp.update(gamepad.dpad_up); + dDown.update(gamepad.dpad_down); + + a.update(gamepad.a); + b.update(gamepad.b); + x.update(gamepad.x); + y.update(gamepad.y); + + leftBumper.update(gamepad.left_bumper); + rightBumper.update(gamepad.right_bumper); + + back.update(gamepad.back); + start.update(gamepad.start); + + leftTrigger.update(gamepad.left_trigger); + rightTrigger.update(gamepad.right_trigger); + + touchpad.update(gamepad.touchpad); + + allButtons.update(leftStickButton.isPressed() || rightStickButton.isPressed() || dLeft.isPressed() || dRight.isPressed() || dUp.isPressed() || dDown.isPressed() || a.isPressed() || b.isPressed() || x.isPressed() || y.isPressed() || leftBumper.isPressed() || rightBumper.isPressed() || back.isPressed() || start.isPressed() || leftTrigger.getValue() > 0 || rightTrigger.getValue() > 0 || touchpad.isPressed()); + } + + public Joystick getLeftStick() { + return leftStick; + } + public Joystick getRightStick() { + return rightStick; + } + + public Button getLeftStickButton() { + return leftStickButton; + } + public Button getRightStickButton() { + return rightStickButton; + } + + public Button getDLeft() { + return dLeft; + } + public Button getDRight() { + return dRight; + } + public Button getDUp() { + return dUp; + } + public Button getDDown() { + return dDown; + } + + public Button getA() { + return a; + } + public Button getB() { + return b; + } + public Button getX() { + return x; + } + public Button getY() { + return y; + } + + public Button getLeftBumper() { + return leftBumper; + } + public Button getRightBumper() { + return rightBumper; + } + + public Button getBack() { + return back; + } + public Button getStart() { + return start; + } + + public Trigger getLeftTrigger() { + return leftTrigger; + } + public Trigger getRightTrigger() { + return rightTrigger; + } + + public Button getTouchpad() { + return touchpad; + } + + public Button getAllButtons() { + return allButtons; + } + + public void rumble() { + gamepad.rumble(Gamepad.RUMBLE_DURATION_CONTINUOUS); + } + + public void rumble(int milliseconds) { + gamepad.rumble(milliseconds); + } + + public void rumbleBlips(int blips) { + gamepad.rumbleBlips(blips); + } + + public void stopRumble() { + gamepad.stopRumble(); + } + + public boolean isRumbling() { + return gamepad.isRumbling(); + } + + public void setColor(int r, int g, int b) { + gamepad.setLedColor(r, g, b, -1); + } + public void setColor(int r, int g, int b, int milliseconds) { + gamepad.setLedColor(r, g, b, milliseconds); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Gamepad2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Gamepad2.java new file mode 100644 index 0000000..201a84b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Gamepad2.java @@ -0,0 +1,913 @@ +package org.firstinspires.ftc.teamcode.controller;///* +// * Copyright (c) 2014, 2015 Qualcomm Technologies Inc +// * +// * All rights reserved. +// * +// * Redistribution and use in source and binary forms, with or without modification, are permitted +// * (subject to the limitations in the disclaimer below) provided that the following conditions are +// * met: +// * +// * Redistributions of source code must retain the above copyright notice, this list of conditions +// * and the following disclaimer. +// * +// * Redistributions in binary form must reproduce the above copyright notice, this list of conditions +// * and the following disclaimer in the documentation and/or other materials provided with the +// * distribution. +// * +// * Neither the name of Qualcomm Technologies Inc nor the names of its contributors may be used to +// * endorse or promote products derived from this software without specific prior written permission. +// * +// * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE. THIS +// * SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED +// * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, +// * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF +// * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// */ +// +//package com.qualcomm.robotcore.hardware; +// +//import android.os.SystemClock; +// +//import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl; +//import com.qualcomm.robotcore.exception.RobotCoreException; +//import com.qualcomm.robotcore.robocol.RobocolParsable; +//import com.qualcomm.robotcore.robocol.RobocolParsableBase; +//import com.qualcomm.robotcore.util.Range; +//import com.qualcomm.robotcore.util.RobotLog; +// +//import org.firstinspires.ftc.robotcore.internal.collections.EvictingBlockingQueue; +//import org.firstinspires.ftc.robotcore.internal.collections.SimpleGson; +//import org.firstinspires.ftc.robotcore.internal.network.SendOnceRunnable; +//import org.firstinspires.ftc.robotcore.internal.ui.GamepadUser; +// +//import java.nio.BufferOverflowException; +//import java.nio.ByteBuffer; +//import java.util.ArrayList; +//import java.util.concurrent.ArrayBlockingQueue; +// +///** +// * Monitor a hardware gamepad. +// *

+// * The buttons, analog sticks, and triggers are represented a public +// * member variables that can be read from or written to directly. +// *

+// * Analog sticks are represented as floats that range from -1.0 to +1.0. They will be 0.0 while at +// * rest. The horizontal axis is labeled x, and the vertical axis is labeled y. +// *

+// * Triggers are represented as floats that range from 0.0 to 1.0. They will be at 0.0 while at +// * rest. +// *

+// * Buttons are boolean values. They will be true if the button is pressed, otherwise they will be +// * false. +// *

+// * The codes KEYCODE_BUTTON_SELECT and KEYCODE_BACK are both be handled as a "back" button event. +// * Older Android devices (Kit Kat) map a Logitech F310 "back" button press to a KEYCODE_BUTTON_SELECT event. +// * Newer Android devices (Marshmallow or greater) map this "back" button press to a KEYCODE_BACK event. +// * Also, the REV Robotics Gamepad (REV-31-1159) has a "select" button instead of a "back" button on the gamepad. +// *

+// * The dpad is represented as 4 buttons, dpad_up, dpad_down, dpad_left, and dpad_right +// */ +//@SuppressWarnings("unused") +//public class Gamepad2 extends RobocolParsableBase { +// +// /** +// * A gamepad with an ID equal to ID_UNASSOCIATED has not been associated with any device. +// */ +// public static final int ID_UNASSOCIATED = -1; +// +// /** +// * A gamepad with a phantom id a synthetic one made up by the system +// */ +// public static final int ID_SYNTHETIC = -2; +// +// public enum Type { +// // Do NOT change the order/names of existing entries, +// // you will break backwards compatibility!! +// UNKNOWN(LegacyType.UNKNOWN), +// LOGITECH_F310(LegacyType.LOGITECH_F310), +// XBOX_360(LegacyType.XBOX_360), +// SONY_PS4(LegacyType.SONY_PS4), // This indicates a PS4-compatible controller that is being used through our compatibility mode +// SONY_PS4_SUPPORTED_BY_KERNEL(LegacyType.SONY_PS4); // This indicates a PS4-compatible controller that is being used through the DualShock 4 Linux kernel driver. +// +// private final LegacyType correspondingLegacyType; +// Type(LegacyType correspondingLegacyType) { +// this.correspondingLegacyType = correspondingLegacyType; +// } +// } +// +// // LegacyType is necessary because robocol gamepad version 3 was written in a way that was not +// // forwards-compatible, so we have to keep sending V3-compatible values. +// public enum LegacyType { +// // Do NOT change the order or names of existing entries, or add new entries. +// // You will break backwards compatibility!! +// UNKNOWN, +// LOGITECH_F310, +// XBOX_360, +// SONY_PS4; +// } +// +// @SuppressWarnings("UnusedAssignment") +// public volatile Type type = Type.UNKNOWN; // IntelliJ thinks this is redundant, but it is NOT. Must be a bug in the analyzer? +// +// /** +// * left analog stick horizontal axis +// */ +// public volatile float left_stick_x = 0f; +// +// /** +// * left analog stick vertical axis +// */ +// public volatile float left_stick_y = 0f; +// +// /** +// * right analog stick horizontal axis +// */ +// public volatile float right_stick_x = 0f; +// +// /** +// * right analog stick vertical axis +// */ +// public volatile float right_stick_y = 0f; +// +// /** +// * dpad up +// */ +// public volatile boolean dpad_up = false; +// +// /** +// * dpad down +// */ +// public volatile boolean dpad_down = false; +// +// /** +// * dpad left +// */ +// public volatile boolean dpad_left = false; +// +// /** +// * dpad right +// */ +// public volatile boolean dpad_right = false; +// +// /** +// * button a +// */ +// public volatile boolean a = false; +// +// /** +// * button b +// */ +// public volatile boolean b = false; +// +// /** +// * button x +// */ +// public volatile boolean x = false; +// +// /** +// * button y +// */ +// public volatile boolean y = false; +// +// /** +// * button guide - often the large button in the middle of the controller. The OS may +// * capture this button before it is sent to the app; in which case you'll never +// * receive it. +// */ +// public volatile boolean guide = false; +// +// /** +// * button start +// */ +// public volatile boolean start = false; +// +// /** +// * button back +// */ +// public volatile boolean back = false; +// +// /** +// * button left bumper +// */ +// public volatile boolean left_bumper = false; +// +// /** +// * button right bumper +// */ +// public volatile boolean right_bumper = false; +// +// /** +// * left stick button +// */ +// public volatile boolean left_stick_button = false; +// +// /** +// * right stick button +// */ +// public volatile boolean right_stick_button = false; +// +// /** +// * left trigger +// */ +// public volatile float left_trigger = 0f; +// +// /** +// * right trigger +// */ +// public volatile float right_trigger = 0f; +// +// /** +// * PS4 Support - Circle +// */ +// public volatile boolean circle = false; +// +// /** +// * PS4 Support - cross +// */ +// public volatile boolean cross = false; +// +// /** +// * PS4 Support - triangle +// */ +// public volatile boolean triangle = false; +// +// /** +// * PS4 Support - square +// */ +// public volatile boolean square = false; +// +// /** +// * PS4 Support - share +// */ +// public volatile boolean share = false; +// +// /** +// * PS4 Support - options +// */ +// public volatile boolean options = false; +// +// /** +// * PS4 Support - touchpad +// */ +// public volatile boolean touchpad = false; +// public volatile boolean touchpad_finger_1; +// public volatile boolean touchpad_finger_2; +// public volatile float touchpad_finger_1_x; +// public volatile float touchpad_finger_1_y; +// public volatile float touchpad_finger_2_x; +// public volatile float touchpad_finger_2_y; +// +// /** +// * PS4 Support - PS Button +// */ +// public volatile boolean ps = false; +// +// /** +// * Which user is this gamepad used by +// */ +// protected volatile byte user = ID_UNASSOCIATED; +// // +// public GamepadUser getUser() { +// return GamepadUser.from(user); +// } +// // +// public void setUser(GamepadUser user) { +// this.user = user.id; +// } +// +// /** +// * See {@link OpModeManagerImpl#runActiveOpMode(Gamepad[])} +// */ +// protected volatile byte userForEffects = ID_UNASSOCIATED; +// public void setUserForEffects(byte userForEffects) { +// this.userForEffects = userForEffects; +// } +// +// /** +// * ID assigned to this gamepad by the OS. This value can change each time the device is plugged in. +// */ +// public volatile int id = ID_UNASSOCIATED; // public only for historical reasons +// +// public void setGamepadId(int id) { +// this.id = id; +// } +// public int getGamepadId() { +// return this.id; +// } +// +// /** +// * Relative timestamp of the last time an event was detected +// */ +// public volatile long timestamp = 0; +// +// /** +// * Sets the time at which this Gamepad last changed its state, +// * in the {@link android.os.SystemClock#uptimeMillis} time base. +// */ +// public void setTimestamp(long timestamp) { +// this.timestamp = timestamp; +// } +// +// /** +// * Refreshes the Gamepad's timestamp to be the current time. +// */ +// public void refreshTimestamp() { +// setTimestamp(SystemClock.uptimeMillis()); +// } +// +// // private static values used for packaging the gamepad state into a byte array +// private static final short PAYLOAD_SIZE = 60; +// private static final short BUFFER_SIZE = PAYLOAD_SIZE + RobocolParsable.HEADER_LENGTH; +// +// private static final byte ROBOCOL_GAMEPAD_VERSION = 5; +// +// public Gamepad() { +// this.type = type(); +// } +// +// /** +// * Copy the state of a gamepad into this gamepad +// * @param gamepad state to be copied from +// * @throws RobotCoreException if the copy fails - gamepad will be in an unknown +// * state if this exception is thrown +// */ +// public void copy(Gamepad gamepad) throws RobotCoreException { +// // reuse the serialization code; since that reduces the chances of bugs +// fromByteArray(gamepad.toByteArray()); +// } +// +// /** +// * Reset this gamepad into its initial state +// */ +// public void reset() { +// try { +// copy(new Gamepad()); +// } catch (RobotCoreException e) { +// // we should never hit this +// RobotLog.e("Gamepad library in an invalid state"); +// throw new IllegalStateException("Gamepad library in an invalid state"); +// } +// } +// +// @Override +// public MsgType getRobocolMsgType() { +// return RobocolParsable.MsgType.GAMEPAD; +// } +// +// @Override +// public byte[] toByteArray() throws RobotCoreException { +// +// ByteBuffer buffer = getWriteBuffer(PAYLOAD_SIZE); +// +// try { +// int buttons = 0; +// +// buffer.put(ROBOCOL_GAMEPAD_VERSION); +// buffer.putInt(id); +// buffer.putLong(timestamp).array(); +// buffer.putFloat(left_stick_x).array(); +// buffer.putFloat(left_stick_y).array(); +// buffer.putFloat(right_stick_x).array(); +// buffer.putFloat(right_stick_y).array(); +// buffer.putFloat(left_trigger).array(); +// buffer.putFloat(right_trigger).array(); +// +// buttons = (buttons << 1) + (touchpad_finger_1 ? 1 : 0); +// buttons = (buttons << 1) + (touchpad_finger_2 ? 1 : 0); +// buttons = (buttons << 1) + (touchpad ? 1 : 0); +// buttons = (buttons << 1) + (left_stick_button ? 1 : 0); +// buttons = (buttons << 1) + (right_stick_button ? 1 : 0); +// buttons = (buttons << 1) + (dpad_up ? 1 : 0); +// buttons = (buttons << 1) + (dpad_down ? 1 : 0); +// buttons = (buttons << 1) + (dpad_left ? 1 : 0); +// buttons = (buttons << 1) + (dpad_right ? 1 : 0); +// buttons = (buttons << 1) + (a ? 1 : 0); +// buttons = (buttons << 1) + (b ? 1 : 0); +// buttons = (buttons << 1) + (x ? 1 : 0); +// buttons = (buttons << 1) + (y ? 1 : 0); +// buttons = (buttons << 1) + (guide ? 1 : 0); +// buttons = (buttons << 1) + (start ? 1 : 0); +// buttons = (buttons << 1) + (back ? 1 : 0); +// buttons = (buttons << 1) + (left_bumper ? 1 : 0); +// buttons = (buttons << 1) + (right_bumper ? 1 : 0); +// buffer.putInt(buttons); +// +// // Version 2 +// buffer.put(user); +// +// // Version 3 +// buffer.put((byte) legacyType().ordinal()); +// +// // Version 4 +// buffer.put((byte) type.ordinal()); +// +// // Version 5 +// buffer.putFloat(touchpad_finger_1_x); +// buffer.putFloat(touchpad_finger_1_y); +// buffer.putFloat(touchpad_finger_2_x); +// buffer.putFloat(touchpad_finger_2_y); +// } catch (BufferOverflowException e) { +// RobotLog.logStacktrace(e); +// } +// +// return buffer.array(); +// } +// +// @Override +// public void fromByteArray(byte[] byteArray) throws RobotCoreException { +// if (byteArray.length < BUFFER_SIZE) { +// throw new RobotCoreException("Expected buffer of at least " + BUFFER_SIZE + " bytes, received " + byteArray.length); +// } +// +// ByteBuffer byteBuffer = getReadBuffer(byteArray); +// +// int buttons = 0; +// +// byte version = byteBuffer.get(); +// +// // TODO(Noah): Reset version to 1 +// // extract version 1 values +// if (version >= 1) { +// id = byteBuffer.getInt(); +// timestamp = byteBuffer.getLong(); +// left_stick_x = byteBuffer.getFloat(); +// left_stick_y = byteBuffer.getFloat(); +// right_stick_x = byteBuffer.getFloat(); +// right_stick_y = byteBuffer.getFloat(); +// left_trigger = byteBuffer.getFloat(); +// right_trigger = byteBuffer.getFloat(); +// +// buttons = byteBuffer.getInt(); +// touchpad_finger_1 = (buttons & 0x20000) != 0; +// touchpad_finger_2 = (buttons & 0x10000) != 0; +// touchpad = (buttons & 0x08000) != 0; +// left_stick_button = (buttons & 0x04000) != 0; +// right_stick_button = (buttons & 0x02000) != 0; +// dpad_up = (buttons & 0x01000) != 0; +// dpad_down = (buttons & 0x00800) != 0; +// dpad_left = (buttons & 0x00400) != 0; +// dpad_right = (buttons & 0x00200) != 0; +// a = (buttons & 0x00100) != 0; +// b = (buttons & 0x00080) != 0; +// x = (buttons & 0x00040) != 0; +// y = (buttons & 0x00020) != 0; +// guide = (buttons & 0x00010) != 0; +// start = (buttons & 0x00008) != 0; +// back = (buttons & 0x00004) != 0; +// left_bumper = (buttons & 0x00002) != 0; +// right_bumper = (buttons & 0x00001) != 0; +// } +// +// // extract version 2 values +// if (version >= 2) { +// user = byteBuffer.get(); +// } +// +// // extract version 3 values +// if (version >= 3) { +// type = Type.values()[byteBuffer.get()]; +// } +// +// if (version >= 4) { +// byte v4TypeValue = byteBuffer.get(); +// if (v4TypeValue < Type.values().length) { +// // Yes, this will replace the version 3 value. That is a good thing, since the version 3 +// // value was not forwards-compatible. +// type = Type.values()[v4TypeValue]; +// } // Else, we don't know what the number means, so we just stick with the value we got from the v3 type field +// } +// +// if(version >= 5) { +// touchpad_finger_1_x = byteBuffer.getFloat(); +// touchpad_finger_1_y = byteBuffer.getFloat(); +// touchpad_finger_2_x = byteBuffer.getFloat(); +// touchpad_finger_2_y = byteBuffer.getFloat(); +// } +// +// updateButtonAliases(); +// } +// +// /** +// * Are all analog sticks and triggers in their rest position? +// * @return true if all analog sticks and triggers are at rest; otherwise false +// */ +// public boolean atRest() { +// return ( +// left_stick_x == 0f && left_stick_y == 0f && +// right_stick_x == 0f && right_stick_y == 0f && +// left_trigger == 0f && right_trigger == 0f); +// } +// +// /** +// * Get the type of gamepad as a {@link Type}. This method defaults to "UNKNOWN". +// * @return gamepad type +// */ +// public Type type() { +// return type; +// } +// +// /** +// * Get the type of gamepad as a {@link LegacyType}. This method defaults to "UNKNOWN". +// * @return gamepad type +// */ +// private LegacyType legacyType() { +// return type.correspondingLegacyType; +// } +// +// +// /** +// * Display a summary of this gamepad, including the state of all buttons, analog sticks, and triggers +// * @return a summary +// */ +// @Override +// public String toString() { +// +// switch (type) { +// case SONY_PS4: +// case SONY_PS4_SUPPORTED_BY_KERNEL: +// return ps4ToString(); +// +// case UNKNOWN: +// case LOGITECH_F310: +// case XBOX_360: +// default: +// return genericToString(); +// } +// } +// +// +// protected String ps4ToString() { +// String buttons = new String(); +// if (dpad_up) buttons += "dpad_up "; +// if (dpad_down) buttons += "dpad_down "; +// if (dpad_left) buttons += "dpad_left "; +// if (dpad_right) buttons += "dpad_right "; +// if (cross) buttons += "cross "; +// if (circle) buttons += "circle "; +// if (square) buttons += "square "; +// if (triangle) buttons += "triangle "; +// if (ps) buttons += "ps "; +// if (share) buttons += "share "; +// if (options) buttons += "options "; +// if (touchpad) buttons += "touchpad "; +// if (left_bumper) buttons += "left_bumper "; +// if (right_bumper) buttons += "right_bumper "; +// if (left_stick_button) buttons += "left stick button "; +// if (right_stick_button) buttons += "right stick button "; +// +// return String.format("ID: %2d user: %2d lx: % 1.2f ly: % 1.2f rx: % 1.2f ry: % 1.2f lt: %1.2f rt: %1.2f %s", +// id, user, left_stick_x, left_stick_y, +// right_stick_x, right_stick_y, left_trigger, right_trigger, buttons); +// } +// +// protected String genericToString() { +// String buttons = new String(); +// if (dpad_up) buttons += "dpad_up "; +// if (dpad_down) buttons += "dpad_down "; +// if (dpad_left) buttons += "dpad_left "; +// if (dpad_right) buttons += "dpad_right "; +// if (a) buttons += "a "; +// if (b) buttons += "b "; +// if (x) buttons += "x "; +// if (y) buttons += "y "; +// if (guide) buttons += "guide "; +// if (start) buttons += "start "; +// if (back) buttons += "back "; +// if (left_bumper) buttons += "left_bumper "; +// if (right_bumper) buttons += "right_bumper "; +// if (left_stick_button) buttons += "left stick button "; +// if (right_stick_button) buttons += "right stick button "; +// +// return String.format("ID: %2d user: %2d lx: % 1.2f ly: % 1.2f rx: % 1.2f ry: % 1.2f lt: %1.2f rt: %1.2f %s", +// id, user, left_stick_x, left_stick_y, +// right_stick_x, right_stick_y, left_trigger, right_trigger, buttons); +// } +// +// +// // To prevent blowing up the command queue if the user tries to send an LED command in a tight loop, +// // we have a 1-element evicting blocking queue for the outgoing LED effect and the event loop periodically +// // just grabs the effect out of it (if any) +// public EvictingBlockingQueue ledQueue = new EvictingBlockingQueue<>(new ArrayBlockingQueue(1)); +// +// public static final int LED_DURATION_CONTINUOUS = -1; +// +// public static class LedEffect { +// public static class Step { +// public int r; +// public int g; +// public int b; +// public int duration; +// } +// +// public final ArrayList steps; +// public final boolean repeating; +// public int user; +// +// private LedEffect(ArrayList steps, boolean repeating) { +// this.steps = steps; +// this.repeating = repeating; +// } +// +// public String serialize() { +// return SimpleGson.getInstance().toJson(this); +// } +// +// public static LedEffect deserialize(String serialized) { +// return SimpleGson.getInstance().fromJson(serialized, LedEffect.class); +// } +// +// public static class Builder { +// private ArrayList steps = new ArrayList<>(); +// private boolean repeating; +// +// /** +// * Add a "step" to this LED effect. A step basically just means to set +// * the LED to a certain color (r,g,b) for a certain duration. By creating a chain of +// * steps, you can create unique effects. +// * +// * @param r the red LED intensity (0.0 - 1.0) +// * @param g the green LED intensity (0.0 - 1.0) +// * @param b the blue LED intensity (0.0 - 1.0) +// * @return the builder object, to follow the builder pattern +// */ +// public Builder addStep(double r, double g, double b, int durationMs) { +// return addStepInternal(r, g, b, Math.max(durationMs, 0)); +// } +// +// /** +// * Set whether this LED effect should loop after finishing, +// * unless the LED is otherwise commanded differently. +// * @param repeating whether to loop +// * @return the builder object, to follow the builder pattern +// */ +// public Builder setRepeating(boolean repeating) { +// this.repeating = repeating; +// return this; +// } +// +// private Builder addStepInternal(double r, double g, double b, int duration) { +// +// r = Range.clip(r, 0, 1); +// g = Range.clip(g, 0, 1); +// b = Range.clip(b, 0, 1); +// +// Step step = new Step(); +// step.r = (int) Math.round(Range.scale(r, 0.0, 1.0, 0, 255)); +// step.g = (int) Math.round(Range.scale(g, 0.0, 1.0, 0, 255)); +// step.b = (int) Math.round(Range.scale(b, 0.0, 1.0, 0, 255)); +// step.duration = duration; +// steps.add(step); +// return this; +// } +// +// /** +// * After you've added your steps, call this to get an LedEffect object +// * that you can then pass to {@link #runLedEffect(LedEffect)} +// * @return an LedEffect object, built from previously added steps +// */ +// public LedEffect build() { +// return new LedEffect(steps, repeating); +// } +// } +// } +// +// public void setLedColor(double r, double g, double b, int durationMs) { +// +// if (durationMs != LED_DURATION_CONTINUOUS) { +// durationMs = Math.max(0, durationMs); +// } +// +// LedEffect effect = new LedEffect.Builder().addStepInternal(r,g,b, durationMs).build(); +// queueEffect(effect); +// } +// +// /** +// * Run an LED effect built using {@link LedEffect.Builder} +// * The LED effect will be run asynchronously; your OpMode will +// * not halt execution while the effect is running. +// * +// * Calling this will displace any currently running LED effect +// */ +// public void runLedEffect(LedEffect effect) { +// queueEffect(effect); +// } +// +// private void queueEffect(LedEffect effect) { +// // We need to make a new object here, because since the effect is queued and +// // not set immediately, if you called this function for two different gamepads +// // but passed in the same instance of an LED effect object, then it could happen +// // that both effect commands would be directed to the *same* gamepad. (Because of +// // the "user" field being modified before the queued command was serialized) +// LedEffect copy = new LedEffect(effect.steps, effect.repeating); +// copy.user = userForEffects; +// ledQueue.offer(copy); +// } +// +// // To prevent blowing up the command queue if the user tries to send a rumble command in a tight loop, +// // we have a 1-element evicting blocking queue for the outgoing rumble effect and the event loop periodically +// // just grabs the effect out of it (if any) +// public EvictingBlockingQueue rumbleQueue = new EvictingBlockingQueue<>(new ArrayBlockingQueue(1)); +// public long nextRumbleApproxFinishTime = RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING; +// +// public static final int RUMBLE_DURATION_CONTINUOUS = -1; +// +// public static class RumbleEffect { +// public static class Step { +// public int large; +// public int small; +// public int duration; +// } +// +// public int user; +// public final ArrayList steps; +// +// private RumbleEffect(ArrayList steps) { +// this.steps = steps; +// } +// +// public String serialize() { +// return SimpleGson.getInstance().toJson(this); +// } +// +// public static RumbleEffect deserialize(String serialized) { +// return SimpleGson.getInstance().fromJson(serialized, RumbleEffect.class); +// } +// +// public static class Builder { +// private ArrayList steps = new ArrayList<>(); +// +// /** +// * Add a "step" to this rumble effect. A step basically just means to rumble +// * at a certain power level for a certain duration. By creating a chain of +// * steps, you can create unique effects. See {@link #rumbleBlips(int)} for a +// * a simple example. +// * @param rumble1 rumble power for rumble motor 1 (0.0 - 1.0) +// * @param rumble2 rumble power for rumble motor 2 (0.0 - 1.0) +// * @param durationMs milliseconds this step lasts +// * @return the builder object, to follow the builder pattern +// */ +// public Builder addStep(double rumble1, double rumble2, int durationMs) { +// return addStepInternal(rumble1, rumble2, Math.max(durationMs, 0)); +// } +// +// private Builder addStepInternal(double rumble1, double rumble2, int durationMs) { +// +// rumble1 = Range.clip(rumble1, 0, 1); +// rumble2 = Range.clip(rumble2, 0, 1); +// +// Step step = new Step(); +// step.large = (int) Math.round(Range.scale(rumble1, 0.0, 1.0, 0, 255)); +// step.small = (int) Math.round(Range.scale(rumble2, 0.0, 1.0, 0, 255)); +// step.duration = durationMs; +// steps.add(step); +// +// return this; +// } +// +// /** +// * After you've added your steps, call this to get a RumbleEffect object +// * that you can then pass to {@link #runRumbleEffect(RumbleEffect)} +// * @return a RumbleEffect object, built from previously added steps +// */ +// public RumbleEffect build() { +// return new RumbleEffect(steps); +// } +// } +// } +// +// /** +// * Run a rumble effect built using {@link RumbleEffect.Builder} +// * The rumble effect will be run asynchronously; your OpMode will +// * not halt execution while the effect is running. +// * +// * Calling this will displace any currently running rumble effect +// */ +// public void runRumbleEffect(RumbleEffect effect) { +// queueEffect(effect); +// } +// +// /** +// * Rumble the gamepad's first rumble motor at maximum power for a certain duration. +// * Calling this will displace any currently running rumble effect. +// * @param durationMs milliseconds to rumble for, or {@link #RUMBLE_DURATION_CONTINUOUS} +// */ +// public void rumble(int durationMs) { +// +// if (durationMs != RUMBLE_DURATION_CONTINUOUS) { +// durationMs = Math.max(0, durationMs); +// } +// +// RumbleEffect effect = new RumbleEffect.Builder().addStepInternal(1.0, 0, durationMs).build(); +// queueEffect(effect); +// } +// +// /** +// * Rumble the gamepad at a fixed rumble power for a certain duration +// * Calling this will displace any currently running rumble effect +// * @param rumble1 rumble power for rumble motor 1 (0.0 - 1.0) +// * @param rumble2 rumble power for rumble motor 2 (0.0 - 1.0) +// * @param durationMs milliseconds to rumble for, or {@link #RUMBLE_DURATION_CONTINUOUS} +// */ +// public void rumble(double rumble1, double rumble2, int durationMs) { +// +// if (durationMs != RUMBLE_DURATION_CONTINUOUS) { +// durationMs = Math.max(0, durationMs); +// } +// +// RumbleEffect effect = new RumbleEffect.Builder().addStepInternal(rumble1, rumble2, durationMs).build(); +// queueEffect(effect); +// } +// +// /** +// * Cancel the currently running rumble effect, if any +// */ +// public void stopRumble() { +// rumble(0,0,RUMBLE_DURATION_CONTINUOUS); +// } +// +// /** +// * Rumble the gamepad for a certain number of "blips" using predetermined blip timing +// * This will displace any currently running rumble effect. +// * @param count the number of rumble blips to perform +// */ +// public void rumbleBlips(int count) { +// RumbleEffect.Builder builder = new RumbleEffect.Builder(); +// +// for(int i = 0; i < count; i++) { +// builder.addStep(1.0,0,50).addStep(0,0,50); +// } +// +// queueEffect(builder.build()); +// } +// +// private void queueEffect(RumbleEffect effect) { +// // We need to make a new object here, because since the effect is queued and +// // not set immediately, if you called this function for two different gamepads +// // but passed in the same instance of a rumble effect object, then it could happen +// // that both rumble commands would be directed to the *same* gamepad. (Because of +// // the "user" field being modified before the queued command was serialized) +// RumbleEffect copy = new RumbleEffect(effect.steps); +// copy.user = userForEffects; +// rumbleQueue.offer(copy); +// nextRumbleApproxFinishTime = calcApproxRumbleFinishTime(copy); +// } +// +// private static final long RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING = -1; +// private static final long RUMBLE_FINISH_TIME_FLAG_INFINITE = Long.MAX_VALUE; +// +// /** +// * Returns an educated guess about whether there is a rumble action ongoing on this gamepad +// * @return an educated guess about whether there is a rumble action ongoing on this gamepad +// */ +// public boolean isRumbling() { +// if(nextRumbleApproxFinishTime == RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING) { +// return false; +// } else if(nextRumbleApproxFinishTime == RUMBLE_FINISH_TIME_FLAG_INFINITE) { +// return true; +// } else { +// return System.currentTimeMillis() < nextRumbleApproxFinishTime; +// } +// } +// +// private long calcApproxRumbleFinishTime(RumbleEffect effect) { +// // If the effect is only 1 step long and has an infinite duration... +// if(effect.steps.size() == 1 && effect.steps.get(0).duration == RUMBLE_DURATION_CONTINUOUS) { +// // If the power is zero, then that means the gamepad is being commanded to cease rumble +// if (effect.steps.get(0).large == 0 && effect.steps.get(0).small == 0) { +// return RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING; +// } else { // But if not, that means it's an infinite (continuous) rumble command +// return RUMBLE_FINISH_TIME_FLAG_INFINITE; +// } +// } else { // If the effect has more than one step (or one step with non-infinite duration) we need to sum the step times +// long time = System.currentTimeMillis(); +// long overhead = 50 /* rumbleGamepadsInterval in FtcEventLoopHandler */ + +// SendOnceRunnable.MS_BATCH_TRANSMISSION_INTERVAL + +// 5 /* Slop */; +// for(RumbleEffect.Step step : effect.steps) { +// time += step.duration; +// } +// time += overhead; +// return time; +// } +// } +// +// /** +// * Alias buttons so that XBOX & PS4 native button labels can be used in use code. +// * Should allow a team to program with whatever controllers they prefer, but +// * be able to swap controllers easily without changing code. +// */ +// protected void updateButtonAliases(){ +// // There is no assignment for touchpad because there is no equivalent on XBOX controllers. +// circle = b; +// cross = a; +// triangle = y; +// square = x; +// share = back; +// options = start; +// ps = guide; +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Joystick.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Joystick.java new file mode 100644 index 0000000..770ee11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Joystick.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode.controller; + +public class Joystick { + public static double DEADZONE = 0.05; //small 5% deadzone for the new controlers + double x = 0; + double y = 0; + + public Joystick() {} + + public void update(double x, double y) { + this.x = x; + this.y = y; + } + + public double getX() { + if(x-DEADZONE) { + return 0; + } + else{ + return x; + } + } + + public double getY() { + if(y-DEADZONE) { + return 0; + } + else{ + return y; + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Trigger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Trigger.java new file mode 100644 index 0000000..4f43bc4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Trigger.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.controller; + +public class Trigger { + double value = 0; + double currentState; + double lastState; + boolean justPressed = false; + boolean justReleased = false; + + public Trigger() {} + + public void update(double value) { + lastState = currentState; + currentState = value; + + justPressed = currentState != 0 && lastState == 0; + justReleased = currentState == 0 && lastState != 0; + + this.value = value; + } + + public double getValue() { + return value; + } + + public boolean isJustPressed() { + return justPressed; + } + + public boolean isJustReleased() { + return justReleased; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java index a297fbd..04f617d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java @@ -9,17 +9,37 @@ import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_WIDTH; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.teamcode.util.CameraPosition; +import org.firstinspires.ftc.teamcode.vision.AprilTagDetectionPipeline; import org.firstinspires.ftc.teamcode.vision.Detection; import org.firstinspires.ftc.teamcode.vision.TargetingPipeline; +import org.openftc.apriltag.AprilTagDetection; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; +import java.util.ArrayList; + // Class for the camera public class Camera { + + private float decimation; + private boolean needToSetDecimation; + int numFramesWithoutDetection = 0; + private boolean signalWebcamInitialized; + private OpenCvCamera signalWebcam; private HardwareMap hardwareMap; private OpenCvCamera targetingCamera; private TargetingPipeline targetingPipeline; private boolean targetingCameraInitialized; + AprilTagDetectionPipeline aprilTagDetectionPipeline; + ArrayList detections; + static final double FEET_PER_METER = 3.28084; + public CameraPosition cameraPosition; + private final Object decimationSync = new Object(); + final float DECIMATION_HIGH = 3; + final float DECIMATION_LOW = 2; + final float THRESHOLD_HIGH_DECIMATION_RANGE_METERS = 1.0f; + final int THRESHOLD_NUM_FRAMES_NO_DETECTION_BEFORE_LOW_DECIMATION = 4; // Constructor public Camera(HardwareMap hardwareMap) { @@ -53,8 +73,57 @@ public class Camera { } } - // Get the Red Goal Detection + // detect colors public Detection getRed() { return (targetingCameraInitialized ? targetingPipeline.getRed() : INVALID_DETECTION); } + + public Detection getBlue() { + return (targetingCameraInitialized ? targetingPipeline.getBlue() : INVALID_DETECTION); + } + + public Detection getBlack() { + return (targetingCameraInitialized ? targetingPipeline.getBlack() : INVALID_DETECTION); + } + + //return frame rate + public int getFrameCount() { + if (signalWebcamInitialized) { + return signalWebcam.getFrameCount(); + } else { + return 0; + } + } + + public int getMarkerId() { + detections = aprilTagDetectionPipeline.getLatestDetections(); + + // If there's been a new frame... + if (detections != null) { + // If we don't see any tags + if (detections.size() == 0) { + numFramesWithoutDetection++; + + // If we haven't seen a tag for a few frames, lower the decimation + // so we can hopefully pick one up if we're e.g. far back + if (numFramesWithoutDetection >= THRESHOLD_NUM_FRAMES_NO_DETECTION_BEFORE_LOW_DECIMATION) { + aprilTagDetectionPipeline.setDecimation(DECIMATION_LOW); + } + } + // We do see tags! + else { + numFramesWithoutDetection = 0; + + // If the target is within 1 meter, turn on high decimation to + // increase the frame rate + if (detections.get(0).pose.z < THRESHOLD_HIGH_DECIMATION_RANGE_METERS) { + aprilTagDetectionPipeline.setDecimation(DECIMATION_HIGH); + } + + return detections.get(0).id; + + } + } + return -1; + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java index 50bd0ee..241b117 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java @@ -1,28 +1,101 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Gamepad; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class Drive { - - private DcMotor frontLeft; - private DcMotor frontRight; - private DcMotor backLeft; - private DcMotor backRight; - - public Drive(HardwareMap hardwareMap) { - this.init(hardwareMap); - } - - private void init(HardwareMap hardwareMap) { - this.frontLeft = hardwareMap.get(DcMotor.class, "frontLeft"); - this.frontRight = hardwareMap.get(DcMotor.class, "frontRight"); - this.backLeft = hardwareMap.get(DcMotor.class, "backLeft"); - this.backRight = hardwareMap.get(DcMotor.class, "backRight"); - } - - public void setInput(Gamepad gamepad1,Gamepad gamepad2) { - - } -} +//package org.firstinspires.ftc.teamcode.hardware; +// +//import com.acmerobotics.roadrunner.control.PIDCoefficients; +//import com.acmerobotics.roadrunner.drive.DriveSignal; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.acmerobotics.roadrunner.trajectory.BaseTrajectoryBuilder; +//import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +//import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; +//import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint; +//import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; +//import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; +//import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +//import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +//import com.qualcomm.robotcore.hardware.DcMotor; +//import com.qualcomm.robotcore.hardware.DcMotorSimple; +//import com.qualcomm.robotcore.hardware.Gamepad; +//import com.qualcomm.robotcore.hardware.HardwareMap; +// +//import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants; +//import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceRunner; +// +//import java.util.Arrays; +// +//public class Drive { +// +// DcMotor frontLeft; +// DcMotor frontRight; +// DcMotor backLeft; +// DcMotor backRight; +// public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(10, 0, 0); +// public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); +// +// private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(DriveConstants.MAX_VEL, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH); +// private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(DriveConstants.MAX_ACCEL); +// private TrajectorySequenceRunner trajectorySequenceRunner; +// +// public Drive(HardwareMap hardwareMap) { +// this.init(hardwareMap); +// } +// +// private void init(HardwareMap hardwareMap) { +// this.frontLeft = hardwareMap.get(DcMotor.class, "frontLeft"); +// frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); +// frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// this.frontRight = hardwareMap.get(DcMotor.class, "frontRight"); +// frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); +// frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// this.backLeft = hardwareMap.get(DcMotor.class, "backLeft"); +// backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); +// backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// this.backRight = hardwareMap.get(DcMotor.class, "backRight"); +// backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); +// backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// this.frontLeft.setDirection(DcMotorSimple.Direction.FORWARD); +// this.backRight.setDirection(DcMotorSimple.Direction.REVERSE); +// this.frontRight.setDirection(DcMotorSimple.Direction.REVERSE); +// this.backLeft.setDirection(DcMotorSimple.Direction.FORWARD); +// } +// +// public void setInput(Gamepad gamepad1,Gamepad gamepad2) { +// float x = -gamepad1.left_stick_x; +// float y = gamepad1.left_stick_y; +// float z = -gamepad1.right_stick_x; +// float flPower = x + y + z; +// float frPower = -x + y - z; +// float blPower = -x + y + z; +// float brPower = x + y - z; +// +// float max = Math.max(1, Math.max(Math.max(Math.abs(blPower), Math.abs(brPower)), Math.max(Math.abs(flPower),Math.abs(frPower)))); +// float scale = 1-((max-1) / max); +// flPower *= scale; +// frPower *= scale; +// blPower *= scale; +// brPower *= scale; +// +// frontLeft.setPower(flPower); +// backLeft.setPower(blPower); +// frontRight.setPower(frPower); +// backRight.setPower(brPower); +// } +// +// public void update() { +// DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity()); +// if (signal != null) setDriveSignal(signal); +// } +// +// public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { +// return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT); +// } +// +// public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { +// return new MinVelocityConstraint(Arrays.asList( +// new AngularVelocityConstraint(maxAngularVel), +// new MecanumVelocityConstraint(maxVel, trackWidth) +// )); +// } +// +// public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) { +// return new ProfileAccelerationConstraint(maxAccel); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java new file mode 100644 index 0000000..aaa90d7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java @@ -0,0 +1,74 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +@Config +public class Intake { // TODO done in theory, but need to get the actual servo positions &&&& do the Sensor stuff + private Servo rServo; + private Servo lServo; + private DcMotor dcMotor; + private Position pos = Position.UP; + + public enum Position { + STACK1, STACK2, STACK3, STACK4, STACK5, UP; + + public Position nextPosition() { + int nextOne = (this.ordinal() + 1) % Position.values().length; + return Position.values()[nextOne]; + } + + public Position previousPosition() { + int backOne = (this.ordinal() - 1) % Position.values().length; + return Position.values()[backOne]; + } + } + + //Position + public static double stack1 = 0.49; + public static double stack2 = 0.50; + public static double stack3 = 0.51; + public static double stack4 = 0.52; + public static double stack5 = 0.53; + public static double up = 0.551; + public static double motorPower = 0; + + public Intake(HardwareMap hardwareMap) { + lServo = hardwareMap.get(Servo.class, "Left Servo"); + lServo.setDirection(Servo.Direction.REVERSE); + rServo = hardwareMap.get(Servo.class, "Right Servo"); + dcMotor = hardwareMap.get(DcMotor.class, "Intake Motor"); + } + + public void setpos(Position stack) { + if (stack == Position.STACK1) { + lServo.setPosition(stack1); + rServo.setPosition(stack1); + } else if (stack == Position.STACK2) { + lServo.setPosition(stack2); + rServo.setPosition(stack2); + } else if (stack == Position.STACK3) { + lServo.setPosition(stack3); + rServo.setPosition(stack3); + } else if (stack == Position.STACK4) { + lServo.setPosition(stack4); + rServo.setPosition(stack4); + } else if (stack == Position.STACK5) { + lServo.setPosition(stack5); + rServo.setPosition(stack5); + } else if (stack == Position.UP) { + lServo.setPosition(up); + rServo.setPosition(up); + } + } + + public void setDcMotor(double pwr) { + dcMotor.setPower(pwr); + } + + public String getTelemetry() { + return "lServo: "+lServo.getPosition()+"rServo: "+rServo.getPosition()+"dcMotor: "+dcMotor.getPower(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index 0291831..02dd6c2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -1,20 +1,39 @@ package org.firstinspires.ftc.teamcode.hardware; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.HardwareMap; -public class Robot { +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder; - private Drive drive; +@Config +public class Robot { + //set to public Drive to revert + public SampleMecanumDrive drive; + public Camera camera; + public Intake intake; + private boolean camEnabled = true; public Robot(HardwareMap hardwareMap) { - this.init(hardwareMap); + //change this to new Drive to revert + drive = new SampleMecanumDrive(hardwareMap); + camera = new Camera(hardwareMap); + camera.initTargetingCamera(); + intake = new Intake(hardwareMap); + camEnabled = true; + } + + public void update(double runTime) { + drive.update(); } - private void init(HardwareMap hardwareMap) { - this.drive = new Drive(hardwareMap); + public String getTelemetry() { + Encoder slide = null; + return String.format("position: %s", slide.getCurrentPosition()); } - public Drive getDrive() { + //set to public Drive to revert + public SampleMecanumDrive getDrive() { return this.drive; } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java new file mode 100644 index 0000000..6d78026 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; + +import java.util.ArrayList; + +public abstract class AbstractAuto extends LinearOpMode { + + public Robot robot; + private int teamElementLocation = 2; + private double currentRuntime; + + @Override + public void runOpMode() { + + telemetry.addLine("Initializing Robot..."); + telemetry.update(); + robot = new Robot(hardwareMap); + makeTrajectories(); + + while (robot.camera.getFrameCount() < 1) { + idle(); + } + + while (!(isStarted() || isStopRequested())) { + currentRuntime = getRuntime(); + robot.update(currentRuntime); + + int newLocation = robot.camera.getMarkerId(); + if (newLocation != -1) { + teamElementLocation = newLocation; + } + + telemetry.addLine("Initialized"); + telemetry.addLine("Randomization: " + teamElementLocation); + telemetry.addLine(robot.getTelemetry()); + telemetry.update(); + } + } + + public abstract void setAlliance(); + + public abstract void makeTrajectories(); + + public abstract void setCameraPosition(); + + public abstract boolean useCamera(); +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java new file mode 100644 index 0000000..c70d97d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java @@ -0,0 +1,65 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.hardware.Camera; +import org.firstinspires.ftc.teamcode.hardware.Drive; +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.util.CameraPosition; + +@Autonomous(name = "Start From Left Wall", group = "Left Start", preselectTeleOp = "Khang Main") +public class StartFromLeftCenterSpike extends AbstractAuto { + + //set to public Drive to revert + public SampleMecanumDrive drive; + public Robot robot; + public Camera camera; + private boolean camEnabled = true; + public CameraPosition cameraPosition; + private int teamElementLocation = 2; + //Steps + public Trajectory start; + public Trajectory step1; + public Trajectory step2; + + @Override + public void setAlliance() {} + + @Override + public void setCameraPosition() { + cameraPosition = CameraPosition.CENTER; + } + + @Override + public boolean useCamera() { + return true; + } + + public void Robot(HardwareMap hardwareMap) { + //set to new Drive to revert + drive = new SampleMecanumDrive(hardwareMap); + camera = new Camera(hardwareMap); + camera.initTargetingCamera(); + camEnabled = true; + } + + @Override + public void makeTrajectories() { + + // positions + Pose2d start = new Pose2d(-65.125,-48,Math.toRadians(-90)); + Pose2d step1 = new Pose2d(-24,-48,Math.toRadians(0)); + Pose2d step2 = new Pose2d(-24,-48,Math.toRadians(0)); + + this.start = robot.drive.trajectoryBuilder(start) + .lineToSplineHeading(step1, + SampleMecanumDrive.getVelocityConstraint(60, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) + ) + .build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java new file mode 100644 index 0000000..d0dda3d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java @@ -0,0 +1,150 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop; + +import static java.lang.Math.PI; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.controller.Controller; +import org.firstinspires.ftc.teamcode.hardware.Robot; + +@Config +public abstract class AbstractTeleOp extends OpMode { + private Robot robot; + Controller driver1; + Controller driver2; + + public static double drivebaseThrottle = 0.6; + public static double drivebaseTurbo = 1.0; + public static int heightIncrement = 20; + + Pose2d robot_pos; + double robot_x, robot_y, robot_heading; + + // auto align variables + double headingPID; + public static double headingP = 0.01; + public static double headingI = 0.03; + public static double headingD = 0.0005; + + double strafePID; + public static double strafeP = 0.05; + public static double strafeI = 0; + public static double strafeD = 0.01; + + double robot_y_pos; + boolean fixed90Toggle = false; + boolean fixed0Toggle = false; + +// public static double robot_width = 12; +// public static double robot_length = 12; +// public static double robot_radius = 6; +// +// public static double groundJuncRadius = 6; +// public static double coneRadius = 4; +// +// public static double armWait = 0.2; + +// private double timeSinceOpened = 0; // for claw +// private double timeSinceClosed = 0; + +// private int delayState = 0; // for arm +// private double delayStart = 0; +// private boolean doArmDelay = false; // for arm + private boolean isAutoClose = true; + + @Override + public void init() { + robot = new Robot(hardwareMap); + driver1 = new Controller(gamepad1); + driver2 = new Controller(gamepad2); + } + + @Override + public void init_loop() { +// if (robot.camera.getFrameCount() < 1) { +// telemetry.addLine("Initializing Robot..."); +// } else { +// telemetry.addLine("Initialized"); +// } + telemetry.addLine("Initialized"); + telemetry.addLine(robot.getTelemetry()); + telemetry.update(); + } + + private int getQuadrant(double angle) { + if (0 < angle && angle < PI/2.0) { + return 1; + } else if (PI/2.0 < angle && angle < PI) { + return 2; + } else if (PI < angle && angle < 3*PI/2.0) { + return 3; + } else { + return 4; + } + } + + @Override + public void loop() { + // robot position update + //robot_pos = robot.drive.getPoseEstimate(); + //robot_x = robot_pos.getX(); + //robot_y = robot_pos.getY(); + //robot_heading = robot_pos.getHeading(); // in radians + + // driver 1 controls + //driver1.update(); + //driver2.update(); + double x = -driver1.getLeftStick().getY(); + double y = driver1.getLeftStick().getX(); + double z = -driver1.getRightStick().getX(); + +// figure out if a toggle is present + if (driver1.getRightBumper().isPressed()) { + if (!fixed90Toggle) { + //robot_y_pos = robot.drive.getPoseEstimate().getX(); + } + fixed90Toggle = true; + } else { + fixed90Toggle = false; + } + + if (!fixed90Toggle && driver1.getLeftBumper().isPressed()) { + if (!fixed0Toggle) { + //robot_y_pos = robot.drive.getPoseEstimate().getY(); + } + fixed0Toggle = true; + } else { + fixed0Toggle = false; + } + + telemetry.addLine("Wanted Position: "+robot_y_pos); + telemetry.addLine("Actual Position: "+robot_y); + telemetry.addLine("PID: "+strafePID); + + // turbo + if (driver1.getLeftBumper().isPressed() || driver1.getRightBumper().isPressed()) { + x *= drivebaseTurbo; + y *= drivebaseTurbo; + z *= drivebaseTurbo; + } else { + x *= drivebaseThrottle; + y *= drivebaseThrottle; + z *= drivebaseThrottle; + } + + // actually set power + if (fixed0Toggle || fixed90Toggle) { + //robot.drive.setWeightedDrivePower(new Pose2d(x, 0, headingPID)); + } else { + //robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); + } + + // update and telemetry + //robot.update(getRuntime()); + + telemetry.addLine(robot.getTelemetry()); + telemetry.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/KhangMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/KhangMain.java new file mode 100644 index 0000000..1e52d99 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/KhangMain.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.controller.Controller; +import org.firstinspires.ftc.teamcode.hardware.Camera; +import org.firstinspires.ftc.teamcode.hardware.Intake; +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +@TeleOp(name = "Khang Main", group = "OpModes") +public class KhangMain extends OpMode { + + private Intake.Position Currentpos = Intake.Position.UP; + private Robot robot; + private Controller controller2; + + @Override + public void init() { + this.robot = new Robot(hardwareMap); + controller2 = new Controller(gamepad2); + } + + @Override + public void loop() { +// this.robot.getDrive().setInput(gamepad1, gamepad2); + double x = gamepad1.left_stick_x; + double y = -gamepad1.left_stick_y; + double z = gamepad1.right_stick_x; + robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); + robot.intake.setDcMotor(gamepad2.right_trigger); + robot.intake.setpos(Currentpos); + controller2.update(); + + if (controller2.getLeftBumper().isJustPressed()) { + Currentpos = Currentpos.nextPosition(); + } + + if (controller2.getRightBumper().isJustPressed()) { + Currentpos = Currentpos.previousPosition(); + } + + + + // Read pose +// Pose2d poseEstimate = drive.getPoseEstimate(); +// +//// Create a vector from the gamepad x/y inputs +//// Then, rotate that vector by the inverse of that heading +// Vector2d input = new Vector2d( +// -gamepad1.left_stick_y, +// -gamepad1.left_stick_x +// ).rotated(-poseEstimate.getHeading()); +// +//// Pass in the rotated input + right stick value for rotation +//// Rotation is not part of the rotated input thus must be passed in separately +// drive.setWeightedDrivePower( +// new Pose2d( +// input.getX(), +// input.getY(), +// -gamepad1.right_stick_x +// ) +// ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java new file mode 100644 index 0000000..d2f6fb0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java @@ -0,0 +1,89 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; + +/* + * Constants shared between multiple drive types. + * + * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final + * fields may also be edited through the dashboard (connect to the robot's WiFi network and + * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you + * adjust them in the dashboard; **config variable changes don't persist between app restarts**. + * + * These are not the only parameters; some are located in the localizer classes, drive base classes, + * and op modes themselves. + */ +@Config +public class DriveConstants { + + /* + * These are motor constants that should be listed online for your motors. + */ + public static final double TICKS_PER_REV = 384.5; + public static final double MAX_RPM = 435; + + /* + * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. + * Set this flag to false if drive encoders are not present and an alternative localization + * method is in use (e.g., tracking wheels). + * + * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients + * from DriveVelocityPIDTuner. + */ + public static final boolean RUN_USING_ENCODER = true; + public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0, + getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); + + /* + * These are physical constants that can be determined from your robot (including the track + * width; it will be tune empirically later although a rough estimate is important). Users are + * free to chose whichever linear distance unit they would like so long as it is consistently + * used. The default values were selected with inches in mind. Road runner uses radians for + * angular distances although most angular parameters are wrapped in Math.toRadians() for + * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. + */ + public static double WHEEL_RADIUS = 1.8045; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 15.75; // in + + /* + * These are the feedforward parameters used to model the drive motor behavior. If you are using + * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive + * motor encoders or have elected not to use them for velocity control, these values should be + * empirically tuned. + */ +// public static double kV = 1.0 / rpmToVelocity(MAX_RPM); + + public static double kV = 0.013; + public static double kA = 0.003; + public static double kStatic = 0; + + /* + * These values are used to generate the trajectories for you robot. To ensure proper operation, + * the constraints should never exceed ~80% of the robot's actual capabilities. While Road + * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start + * small and gradually increase them later after everything is working. All distance units are + * inches. + */ + // old is 35 35 120 120 + // new is 70 45 90 90 + public static double MAX_VEL = 35; + public static double MAX_ACCEL = 35; + public static double MAX_ANG_VEL = Math.toRadians(90); + public static double MAX_ANG_ACCEL = Math.toRadians(90); + + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + public static double rpmToVelocity(double rpm) { + return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0; + } + + public static double getMotorVelocityF(double ticksPerSecond) { + // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx + return 32767 / ticksPerSecond; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/MouseOdometry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/MouseOdometry.java new file mode 100644 index 0000000..e93cb3f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/MouseOdometry.java @@ -0,0 +1,5 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive;//package org.firstinspires.ftc.teamcode.roadrunner.drive; +//import java.awt. +// +//public class MouseOdometry { +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java new file mode 100644 index 0000000..dee69ec --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java @@ -0,0 +1,326 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.drive.MecanumDrive; +import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower; +import com.acmerobotics.roadrunner.followers.TrajectoryFollower; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceRunner; +import org.firstinspires.ftc.teamcode.roadrunner.util.LynxModuleUtil; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +@Config +public class SampleMecanumDrive extends MecanumDrive { + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(10, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); + + public static double LATERAL_MULTIPLIER = 1; + + public static double VX_WEIGHT = 1; + public static double VY_WEIGHT = 1; + public static double OMEGA_WEIGHT = 1; + + private TrajectorySequenceRunner trajectorySequenceRunner; + + private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(DriveConstants.MAX_VEL, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH); + private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(DriveConstants.MAX_ACCEL); + + private TrajectoryFollower follower; + + private DcMotorEx leftFront, leftRear, rightRear, rightFront; + private List motors; + + private BNO055IMU imu; + private VoltageSensor batteryVoltageSensor; + + public SampleMecanumDrive(HardwareMap hardwareMap) { + super(DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic, DriveConstants.TRACK_WIDTH, DriveConstants.TRACK_WIDTH, LATERAL_MULTIPLIER); + + follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, + new Pose2d(0.5, 0.5, Math.toRadians(5)), 0.1); + + LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + // TODO: adjust the names of the following hardware devices to match your configuration + imu = hardwareMap.get(BNO055IMU.class, "imu"); + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; + imu.initialize(parameters); + + // TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does + // not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.) + // + // | +Z axis + // | + // | + // | + // _______|_____________ +Y axis + // / |_____________/|__________ + // / REV / EXPANSION // + // / / HUB // + // /_______/_____________// + // |_______/_____________|/ + // / + // / +X axis + // + // This diagram is derived from the axes in section 3.4 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf + // and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location + // + // For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y. + // BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y); + + leftFront = hardwareMap.get(DcMotorEx.class, "frontLeft"); + leftRear = hardwareMap.get(DcMotorEx.class, "backLeft"); + rightRear = hardwareMap.get(DcMotorEx.class, "backRight"); + rightFront = hardwareMap.get(DcMotorEx.class, "frontRight"); + + motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + + if (DriveConstants.RUN_USING_ENCODER) { + setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + if (DriveConstants.RUN_USING_ENCODER && DriveConstants.MOTOR_VELO_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID); + } + + // TODO: reverse any motors using DcMotor.setDirection() + rightFront.setDirection(DcMotorSimple.Direction.REVERSE); + rightRear.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + + // TODO: if desired, use setLocalizer() to change the localization method + // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); + + setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this)); +// setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); + + trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { + return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { + return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { + return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT); + } + + public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { + return new TrajectorySequenceBuilder( + startPose, + VEL_CONSTRAINT, ACCEL_CONSTRAINT, + DriveConstants.MAX_ANG_VEL, DriveConstants.MAX_ANG_ACCEL + ); + } + + public void turnAsync(double angle) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(getPoseEstimate()) + .turn(angle) + .build() + ); + } + + public void turn(double angle) { + turnAsync(angle); + waitForIdle(); + } + + public void followTrajectoryAsync(Trajectory trajectory) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(trajectory.start()) + .addTrajectory(trajectory) + .build() + ); + } + + public void followTrajectory(Trajectory trajectory) { + followTrajectoryAsync(trajectory); + waitForIdle(); + } + + public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) { + trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence); + } + + public void followTrajectorySequence(TrajectorySequence trajectorySequence) { + followTrajectorySequenceAsync(trajectorySequence); + waitForIdle(); + } + + public Pose2d getLastError() { + return trajectorySequenceRunner.getLastPoseError(); + } + + public void update() { + updatePoseEstimate(); + DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity()); + if (signal != null) setDriveSignal(signal); + } + + public void waitForIdle() { + while (!Thread.currentThread().isInterrupted() && isBusy()) + update(); + } + + public boolean isBusy() { + return trajectorySequenceRunner.isBusy(); + } + + public void setMode(DcMotor.RunMode runMode) { + for (DcMotorEx motor : motors) { + motor.setMode(runMode); + } + } + + public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(zeroPowerBehavior); + } + } + + public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) { + PIDFCoefficients compensatedCoefficients = new PIDFCoefficients( + coefficients.p, coefficients.i, coefficients.d, + coefficients.f * 12 / batteryVoltageSensor.getVoltage() + ); + + for (DcMotorEx motor : motors) { + motor.setPIDFCoefficients(runMode, compensatedCoefficients); + } + } + + public void setWeightedDrivePower(Pose2d drivePower) { + Pose2d vel = drivePower; + + if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY()) + + Math.abs(drivePower.getHeading()) > 1) { + // re-normalize the powers according to the weights + double denom = VX_WEIGHT * Math.abs(drivePower.getX()) + + VY_WEIGHT * Math.abs(drivePower.getY()) + + OMEGA_WEIGHT * Math.abs(drivePower.getHeading()); + + vel = new Pose2d( + VX_WEIGHT * drivePower.getX(), + VY_WEIGHT * drivePower.getY(), + OMEGA_WEIGHT * drivePower.getHeading() + ).div(denom); + } + + setDrivePower(vel); + } + + @NonNull + @Override + public List getWheelPositions() { + List wheelPositions = new ArrayList<>(); + for (DcMotorEx motor : motors) { + wheelPositions.add(DriveConstants.encoderTicksToInches(motor.getCurrentPosition())); + } + return wheelPositions; + } + + @Override + public List getWheelVelocities() { + List wheelVelocities = new ArrayList<>(); + for (DcMotorEx motor : motors) { + wheelVelocities.add(DriveConstants.encoderTicksToInches(motor.getVelocity())); + } + return wheelVelocities; + } + + @Override + public void setMotorPowers(double v, double v1, double v2, double v3) { + leftFront.setPower(v); + leftRear.setPower(v1); + rightRear.setPower(v2); + rightFront.setPower(v3); + } + + @Override + public double getRawExternalHeading() { + return imu.getAngularOrientation().firstAngle; + } + + @Override + public Double getExternalHeadingVelocity() { + // To work around an SDK bug, use -zRotationRate in place of xRotationRate + // and -xRotationRate in place of zRotationRate (yRotationRate behaves as + // expected). This bug does NOT affect orientation. + // + // See https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251 for details. + return (double) -imu.getAngularVelocity().zRotationRate; + } + + public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { + return new MinVelocityConstraint(Arrays.asList( + new AngularVelocityConstraint(maxAngularVel), + new MecanumVelocityConstraint(maxVel, trackWidth) + )); + } + + public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) { + return new ProfileAccelerationConstraint(maxAccel); + } + + public String getTelemetry() { + return "Drive: "+getPoseEstimate(); + } + + public void setInput(Gamepad gamepad1, Gamepad gamepad2) { + double x = gamepad1.left_stick_x; + double y = -gamepad1.left_stick_y; + double z = gamepad1.right_stick_x; + + setInput(gamepad1, gamepad2); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleTankDrive.java new file mode 100644 index 0000000..63852f8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleTankDrive.java @@ -0,0 +1,321 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive; + +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ANG_ACCEL; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ANG_VEL; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.TRACK_WIDTH; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.encoderTicksToInches; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kV; + +import androidx.annotation.NonNull; + +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.drive.TankDrive; +import com.acmerobotics.roadrunner.followers.TankPIDVAFollower; +import com.acmerobotics.roadrunner.followers.TrajectoryFollower; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceRunner; +import org.firstinspires.ftc.teamcode.roadrunner.util.LynxModuleUtil; + +import java.util.Arrays; +import java.util.List; + +/* + * Simple tank drive hardware implementation for REV hardware. + */ +public class SampleTankDrive extends TankDrive { + public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); + + public static double VX_WEIGHT = 1; + public static double OMEGA_WEIGHT = 1; + + private TrajectorySequenceRunner trajectorySequenceRunner; + + private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH); + private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL); + + private TrajectoryFollower follower; + + private List motors, leftMotors, rightMotors; + private BNO055IMU imu; + + private VoltageSensor batteryVoltageSensor; + + public SampleTankDrive(HardwareMap hardwareMap) { + super(kV, kA, kStatic, TRACK_WIDTH); + + follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID, + new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); + + LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + // TODO: adjust the names of the following hardware devices to match your configuration + imu = hardwareMap.get(BNO055IMU.class, "imu"); + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; + imu.initialize(parameters); + + // TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does + // not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.) + // + // | +Z axis + // | + // | + // | + // _______|_____________ +Y axis + // / |_____________/|__________ + // / REV / EXPANSION // + // / / HUB // + // /_______/_____________// + // |_______/_____________|/ + // / + // / +X axis + // + // This diagram is derived from the axes in section 3.4 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf + // and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location + // + // For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y. + // BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y); + + // add/remove motors depending on your robot (e.g., 6WD) + DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); + leftMotors = Arrays.asList(leftFront, leftRear); + rightMotors = Arrays.asList(rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + if (RUN_USING_ENCODER) { + setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + } + + // TODO: reverse any motors using DcMotor.setDirection() + + // TODO: if desired, use setLocalizer() to change the localization method + // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); + + trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { + return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { + return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { + return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint); + } + + public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { + return new TrajectorySequenceBuilder( + startPose, + VEL_CONSTRAINT, accelConstraint, + MAX_ANG_VEL, MAX_ANG_ACCEL + ); + } + + public void turnAsync(double angle) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(getPoseEstimate()) + .turn(angle) + .build() + ); + } + + public void turn(double angle) { + turnAsync(angle); + waitForIdle(); + } + + public void followTrajectoryAsync(Trajectory trajectory) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(trajectory.start()) + .addTrajectory(trajectory) + .build() + ); + } + + public void followTrajectory(Trajectory trajectory) { + followTrajectoryAsync(trajectory); + waitForIdle(); + } + + public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) { + trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence); + } + + public void followTrajectorySequence(TrajectorySequence trajectorySequence) { + followTrajectorySequenceAsync(trajectorySequence); + waitForIdle(); + } + + public Pose2d getLastError() { + return trajectorySequenceRunner.getLastPoseError(); + } + + + public void update() { + updatePoseEstimate(); + DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity()); + if (signal != null) setDriveSignal(signal); + } + + public void waitForIdle() { + while (!Thread.currentThread().isInterrupted() && isBusy()) + update(); + } + + public boolean isBusy() { + return trajectorySequenceRunner.isBusy(); + } + + public void setMode(DcMotor.RunMode runMode) { + for (DcMotorEx motor : motors) { + motor.setMode(runMode); + } + } + + public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(zeroPowerBehavior); + } + } + + public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) { + PIDFCoefficients compensatedCoefficients = new PIDFCoefficients( + coefficients.p, coefficients.i, coefficients.d, + coefficients.f * 12 / batteryVoltageSensor.getVoltage() + ); + for (DcMotorEx motor : motors) { + motor.setPIDFCoefficients(runMode, compensatedCoefficients); + } + } + + public void setWeightedDrivePower(Pose2d drivePower) { + Pose2d vel = drivePower; + + if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getHeading()) > 1) { + // re-normalize the powers according to the weights + double denom = VX_WEIGHT * Math.abs(drivePower.getX()) + + OMEGA_WEIGHT * Math.abs(drivePower.getHeading()); + + vel = new Pose2d( + VX_WEIGHT * drivePower.getX(), + 0, + OMEGA_WEIGHT * drivePower.getHeading() + ).div(denom); + } + + setDrivePower(vel); + } + + @NonNull + @Override + public List getWheelPositions() { + double leftSum = 0, rightSum = 0; + for (DcMotorEx leftMotor : leftMotors) { + leftSum += encoderTicksToInches(leftMotor.getCurrentPosition()); + } + for (DcMotorEx rightMotor : rightMotors) { + rightSum += encoderTicksToInches(rightMotor.getCurrentPosition()); + } + return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); + } + + public List getWheelVelocities() { + double leftSum = 0, rightSum = 0; + for (DcMotorEx leftMotor : leftMotors) { + leftSum += encoderTicksToInches(leftMotor.getVelocity()); + } + for (DcMotorEx rightMotor : rightMotors) { + rightSum += encoderTicksToInches(rightMotor.getVelocity()); + } + return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); + } + + @Override + public void setMotorPowers(double v, double v1) { + for (DcMotorEx leftMotor : leftMotors) { + leftMotor.setPower(v); + } + for (DcMotorEx rightMotor : rightMotors) { + rightMotor.setPower(v1); + } + } + + @Override + public double getRawExternalHeading() { + return imu.getAngularOrientation().firstAngle; + } + + @Override + public Double getExternalHeadingVelocity() { + // To work around an SDK bug, use -zRotationRate in place of xRotationRate + // and -xRotationRate in place of zRotationRate (yRotationRate behaves as + // expected). This bug does NOT affect orientation. + // + // See https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251 for details. + return (double) -imu.getAngularVelocity().xRotationRate; + } + + public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { + return new MinVelocityConstraint(Arrays.asList( + new AngularVelocityConstraint(maxAngularVel), + new TankVelocityConstraint(maxVel, trackWidth) + )); + } + + public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) { + return new ProfileAccelerationConstraint(maxAccel); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/StandardTrackingWheelLocalizer.java new file mode 100644 index 0000000..2f7ec2f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/StandardTrackingWheelLocalizer.java @@ -0,0 +1,82 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive; + +import androidx.annotation.NonNull; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | + * | | + * | | + * \--------------/ + * + */ +public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { + public static double TICKS_PER_REV = 8192; + public static double WHEEL_RADIUS = 0.6889764; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + + public static double LATERAL_DISTANCE = 11.686023622; // in; distance between the left and right wheels + public static double FORWARD_OFFSET = 4.89812992126; // in; offset of the lateral wheel + + private Encoder leftEncoder, rightEncoder, frontEncoder; + + public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) { + super(Arrays.asList( + new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left + new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right + new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front + )); + + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft")); + frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontRight")); + + // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + leftEncoder.setDirection(Encoder.Direction.REVERSE); +// rightEncoder.setDirection(Encoder.Direction.REVERSE); +// frontEncoder.setDirection(Encoder.Direction.REVERSE); + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @NonNull + @Override + public List getWheelPositions() { + return Arrays.asList( + encoderTicksToInches(leftEncoder.getCurrentPosition()), + encoderTicksToInches(rightEncoder.getCurrentPosition()), + encoderTicksToInches(frontEncoder.getCurrentPosition()) + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other + // competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a + // compensation method + + return Arrays.asList( + encoderTicksToInches(leftEncoder.getCorrectedVelocity()), + encoderTicksToInches(rightEncoder.getCorrectedVelocity()), + encoderTicksToInches(frontEncoder.getCorrectedVelocity()) + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java new file mode 100644 index 0000000..5ea5411 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java @@ -0,0 +1,108 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive; + +import androidx.annotation.NonNull; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.TwoTrackingWheelLocalizer; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * ^ + * | + * | ( x direction) + * | + * v + * <----( y direction )----> + * (forward) + * /--------------\ + * | ____ | + * | ---- | <- Perpendicular Wheel + * | || | + * | || | <- Parallel Wheel + * | | + * | | + * \--------------/ + * + */ +public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { + public static double TICKS_PER_REV = 8192; + public static double WHEEL_RADIUS = 0.6889764; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + + public static double PARALLEL_X = -4.48818897638; // X is the up and down direction + public static double PARALLEL_Y = 3.10679133858; // Y is the strafe direction + + public static double PERPENDICULAR_X = 2.21801181102; + public static double PERPENDICULAR_Y = 0; + + public static double MULTIPLIER_X = 1.4; + public static double MULTIPLIER_Y = 1.4; + + // Parallel/Perpendicular to the forward axis + // Parallel wheel is parallel to the forward axis + // Perpendicular is perpendicular to the forward axis + private Encoder parallelEncoder, perpendicularEncoder; + + private SampleMecanumDrive drive; + + public TwoWheelTrackingLocalizer(HardwareMap hardwareMap, SampleMecanumDrive drive) { + super(Arrays.asList( + new Pose2d(PARALLEL_X, PARALLEL_Y, 0), + new Pose2d(PERPENDICULAR_X, PERPENDICULAR_Y, Math.toRadians(90)) + )); + + this.drive = drive; + + parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "parallel")); +// parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight")); + perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "perpendicular")); + + // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + perpendicularEncoder.setDirection(Encoder.Direction.REVERSE); + parallelEncoder.setDirection(Encoder.Direction.REVERSE); + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @Override + public double getHeading() { + return drive.getRawExternalHeading(); + } + + @Override + public Double getHeadingVelocity() { + return drive.getExternalHeadingVelocity(); + } + + @NonNull + @Override + public List getWheelPositions() { + return Arrays.asList( + encoderTicksToInches(parallelEncoder.getCurrentPosition()), + encoderTicksToInches(perpendicularEncoder.getCurrentPosition()) + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other + // competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a + // compensation method + + return Arrays.asList( + encoderTicksToInches(parallelEncoder.getCorrectedVelocity()), + encoderTicksToInches(perpendicularEncoder.getCorrectedVelocity()) + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java new file mode 100644 index 0000000..e275220 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java @@ -0,0 +1,203 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_RPM; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.rpmToVelocity; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.roadrunner.util.LoggingUtil; + +import java.util.ArrayList; +import java.util.List; + +/* + * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an + * outline of the procedure: + * 1. Slowly ramp the motor power and record encoder values along the way. + * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV) + * and an optional intercept (kStatic). + * 3. Accelerate the robot (apply constant power) and record the encoder counts. + * 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear + * regression. + */ +//@Config +@Disabled +@Autonomous(group = "drive") +public class AutomaticFeedforwardTuner extends LinearOpMode { + public static double MAX_POWER = 0.7; + public static double DISTANCE = 100; // in + + @Override + public void runOpMode() throws InterruptedException { + if (RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " + + "when using the built-in drive motor velocity PID."); + } + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Press play to begin the feedforward tuning routine"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Would you like to fit kStatic?"); + telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no"); + telemetry.update(); + + boolean fitIntercept = false; + while (!isStopRequested()) { + if (gamepad1.y) { + fitIntercept = true; + while (!isStopRequested() && gamepad1.y) { + idle(); + } + break; + } else if (gamepad1.b) { + while (!isStopRequested() && gamepad1.b) { + idle(); + } + break; + } + idle(); + } + + telemetry.clearAll(); + telemetry.addLine(Misc.formatInvariant( + "Place your robot on the field with at least %.2f in of room in front", DISTANCE)); + telemetry.addLine("Press (Y/Δ) to begin"); + telemetry.update(); + + while (!isStopRequested() && !gamepad1.y) { + idle(); + } + while (!isStopRequested() && gamepad1.y) { + idle(); + } + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + double maxVel = rpmToVelocity(MAX_RPM); + double finalVel = MAX_POWER * maxVel; + double accel = (finalVel * finalVel) / (2.0 * DISTANCE); + double rampTime = Math.sqrt(2.0 * DISTANCE / accel); + + List timeSamples = new ArrayList<>(); + List positionSamples = new ArrayList<>(); + List powerSamples = new ArrayList<>(); + + drive.setPoseEstimate(new Pose2d()); + + double startTime = clock.seconds(); + while (!isStopRequested()) { + double elapsedTime = clock.seconds() - startTime; + if (elapsedTime > rampTime) { + break; + } + double vel = accel * elapsedTime; + double power = vel / maxVel; + + timeSamples.add(elapsedTime); + positionSamples.add(drive.getPoseEstimate().getX()); + powerSamples.add(power); + + drive.setDrivePower(new Pose2d(power, 0.0, 0.0)); + drive.updatePoseEstimate(); + } + drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0)); + + telemetry.clearAll(); + telemetry.addLine("Quasi-static ramp up test complete"); + telemetry.addLine("Would you like to fit kA?"); + telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no"); + telemetry.update(); + + boolean fitAccelFF = false; + while (!isStopRequested()) { + if (gamepad1.y) { + fitAccelFF = true; + while (!isStopRequested() && gamepad1.y) { + idle(); + } + break; + } else if (gamepad1.b) { + while (!isStopRequested() && gamepad1.b) { + idle(); + } + break; + } + idle(); + } + + if (fitAccelFF) { + telemetry.clearAll(); + telemetry.addLine("Place the robot back in its starting position"); + telemetry.addLine("Press (Y/Δ) to continue"); + telemetry.update(); + + while (!isStopRequested() && !gamepad1.y) { + idle(); + } + while (!isStopRequested() && gamepad1.y) { + idle(); + } + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + double maxPowerTime = DISTANCE / maxVel; + + timeSamples.clear(); + positionSamples.clear(); + powerSamples.clear(); + + drive.setPoseEstimate(new Pose2d()); + drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0)); + + startTime = clock.seconds(); + while (!isStopRequested()) { + double elapsedTime = clock.seconds() - startTime; + if (elapsedTime > maxPowerTime) { + break; + } + + timeSamples.add(elapsedTime); + positionSamples.add(drive.getPoseEstimate().getX()); + powerSamples.add(MAX_POWER); + + drive.updatePoseEstimate(); + } + drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0)); + + + telemetry.clearAll(); + telemetry.addLine("Constant power test complete"); + telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)", + telemetry.update())); + } + + while (!isStopRequested()) { + idle(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/BackAndForth.java new file mode 100644 index 0000000..faadebc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/BackAndForth.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin moving forward and + * backward. You should observe the target position (green) and your pose estimate (blue) and adjust + * your follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + * + * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It + * is recommended that you use the FollowerPIDTuner opmode for further fine tuning. + */ +//@Config +@Autonomous(group = "drive") +public class BackAndForth extends LinearOpMode { + + public static double DISTANCE = 50; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d()) + .forward(DISTANCE) + .build(); + + Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end()) + .back(DISTANCE) + .build(); + + waitForStart(); + + while (opModeIsActive() && !isStopRequested()) { + drive.followTrajectory(trajectoryForward); + drive.followTrajectory(trajectoryBackward); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/DriveVelocityPIDTuner.java new file mode 100644 index 0000000..24d4ea2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/DriveVelocityPIDTuner.java @@ -0,0 +1,171 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kV; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +import java.util.List; + +/* + * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed- + * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as + * important as the positional parameters. Like the other manual tuning routines, this op mode + * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's + * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC + * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully + * connected, start the program, and your robot will begin moving forward and backward according to + * a motion profile. Your job is to graph the velocity errors over time and adjust the PID + * coefficients (note: the tuning variable will not appear until the op mode finishes initializing). + * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the + * MOTOR_VELO_PID field. + * + * Recommended tuning process: + * + * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to + * mitigate oscillations. + * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached. + * 3. Back off kP and kD a little until the response is less oscillatory (but without lag). + * + * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the + * user to reset the position of the bot in the event that it drifts off the path. + * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. + */ +//@Config +@Disabled +@Autonomous(group = "drive") +public class DriveVelocityPIDTuner extends LinearOpMode { + public static double DISTANCE = 72; // in + + enum Mode { + DRIVER_MODE, + TUNING_MODE + } + + private static MotionProfile generateProfile(boolean movingForward) { + MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); + MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); + } + + @Override + public void runOpMode() { + if (!RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" + + "PID is not in use", getClass().getSimpleName()); + } + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Mode mode = Mode.TUNING_MODE; + + double lastKp = MOTOR_VELO_PID.p; + double lastKi = MOTOR_VELO_PID.i; + double lastKd = MOTOR_VELO_PID.d; + double lastKf = MOTOR_VELO_PID.f; + + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Ready!"); + telemetry.update(); + telemetry.clearAll(); + + waitForStart(); + + if (isStopRequested()) return; + + boolean movingForwards = true; + MotionProfile activeProfile = generateProfile(true); + double profileStart = clock.seconds(); + + + while (!isStopRequested()) { + telemetry.addData("mode", mode); + + switch (mode) { + case TUNING_MODE: + if (gamepad1.y) { + mode = Mode.DRIVER_MODE; + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + // calculate and set the motor power + double profileTime = clock.seconds() - profileStart; + + if (profileTime > activeProfile.duration()) { + // generate a new profile + movingForwards = !movingForwards; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + MotionState motionState = activeProfile.get(profileTime); + double targetPower = kV * motionState.getV(); + drive.setDrivePower(new Pose2d(targetPower, 0, 0)); + + List velocities = drive.getWheelVelocities(); + + // update telemetry + telemetry.addData("targetVelocity", motionState.getV()); + for (int i = 0; i < velocities.size(); i++) { + telemetry.addData("measuredVelocity" + i, velocities.get(i)); + telemetry.addData( + "error" + i, + motionState.getV() - velocities.get(i) + ); + } + break; + case DRIVER_MODE: + if (gamepad1.b) { + drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + mode = Mode.TUNING_MODE; + movingForwards = true; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + break; + } + + if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d + || lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) { + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + + lastKp = MOTOR_VELO_PID.p; + lastKi = MOTOR_VELO_PID.i; + lastKd = MOTOR_VELO_PID.d; + lastKf = MOTOR_VELO_PID.f; + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java new file mode 100644 index 0000000..e4fbfae --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java @@ -0,0 +1,56 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin driving in a square. + * You should observe the target position (green) and your pose estimate (blue) and adjust your + * follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + */ +//@Config +@Disabled +@Autonomous(group = "drive") +public class FollowerPIDTuner extends LinearOpMode { + public static double DISTANCE = 48; // in + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0); + + drive.setPoseEstimate(startPose); + + waitForStart(); + + if (isStopRequested()) return; + + while (!isStopRequested()) { + TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .build(); + drive.followTrajectorySequence(trajSeq); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/LocalizationTest.java new file mode 100644 index 0000000..674d9b1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/LocalizationTest.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +/** + * This is a simple teleop routine for testing localization. Drive the robot around like a normal + * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight + * errors are not out of the ordinary, especially with sudden drive motions). The goal of this + * exercise is to ascertain whether the localizer has been configured properly (note: the pure + * encoder localizer heading may be significantly off if the track width has not been tuned). + */ +@TeleOp(group = "drive") +public class LocalizationTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + waitForStart(); + + while (!isStopRequested()) { + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + + drive.update(); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("x", poseEstimate.getX()); + telemetry.addData("y", poseEstimate.getY()); + telemetry.addData("heading", poseEstimate.getHeading()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/ManualFeedforwardTuner.java new file mode 100644 index 0000000..7b2ba18 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/ManualFeedforwardTuner.java @@ -0,0 +1,143 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kV; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.kinematics.Kinematics; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +import java.util.Objects; + +/* + * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary, + * tuning these coefficients is just as important as the positional parameters. Like the other + * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if + * you are using the Control Hub. Once you've successfully connected, start the program, and your + * robot will begin moving forward and backward according to a motion profile. Your job is to graph + * the velocity errors over time and adjust the feedforward coefficients. Once you've found a + * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file. + * + * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the + * user to reset the position of the bot in the event that it drifts off the path. + * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. + */ +//@Config +@Autonomous(group = "drive") +public class ManualFeedforwardTuner extends LinearOpMode { + public static double DISTANCE = 72; // in + + private FtcDashboard dashboard = FtcDashboard.getInstance(); + + private SampleMecanumDrive drive; + + enum Mode { + DRIVER_MODE, + TUNING_MODE + } + + private Mode mode; + + private static MotionProfile generateProfile(boolean movingForward) { + MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); + MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); + } + + @Override + public void runOpMode() { +// if (RUN_USING_ENCODER) { +// RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " + +// "when using the built-in drive motor velocity PID."); +// } + + telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry()); + + drive = new SampleMecanumDrive(hardwareMap); + + mode = Mode.TUNING_MODE; + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Ready!"); + telemetry.update(); + telemetry.clearAll(); + + waitForStart(); + + if (isStopRequested()) return; + + boolean movingForwards = true; + MotionProfile activeProfile = generateProfile(true); + double profileStart = clock.seconds(); + + + while (!isStopRequested()) { + telemetry.addData("mode", mode); + + switch (mode) { + case TUNING_MODE: + if (gamepad1.y) { + mode = Mode.DRIVER_MODE; + } + + // calculate and set the motor power + double profileTime = clock.seconds() - profileStart; + + if (profileTime > activeProfile.duration()) { + // generate a new profile + movingForwards = !movingForwards; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + MotionState motionState = activeProfile.get(profileTime); + double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic); + + drive.setDrivePower(new Pose2d(targetPower, 0, 0)); + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + double currentVelo = poseVelo.getX(); + + // update telemetry + telemetry.addData("targetVelocity", motionState.getV()); + telemetry.addData("measuredVelocity", currentVelo); + telemetry.addData("error", motionState.getV() - currentVelo); + break; + case DRIVER_MODE: + if (gamepad1.b) { + mode = Mode.TUNING_MODE; + movingForwards = true; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + break; + } + + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxAngularVeloTuner.java new file mode 100644 index 0000000..04213a9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxAngularVeloTuner.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +import java.util.Objects; + +/** + * This routine is designed to calculate the maximum angular velocity your bot can achieve under load. + *

+ * Upon pressing start, your bot will turn at max power for RUNTIME seconds. + *

+ * Further fine tuning of MAX_ANG_VEL may be desired. + */ + +//@Config +@Disabled +@Autonomous(group = "drive") +public class MaxAngularVeloTuner extends LinearOpMode { + public static double RUNTIME = 4.0; + + private ElapsedTime timer; + private double maxAngVelocity = 0.0; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(0, 0, 1)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + + maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity); + } + + drive.setDrivePower(new Pose2d()); + + telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity); + telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity)); + telemetry.update(); + + while (!isStopRequested()) idle(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxVelocityTuner.java new file mode 100644 index 0000000..dbd8ba2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxVelocityTuner.java @@ -0,0 +1,83 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +import java.util.Objects; + +/** + * This routine is designed to calculate the maximum velocity your bot can achieve under load. It + * will also calculate the effective kF value for your velocity PID. + *

+ * Upon pressing start, your bot will run at max power for RUNTIME seconds. + *

+ * Further fine tuning of kF may be desired. + */ +//@Config +@Disabled +@Autonomous(group = "drive") +public class MaxVelocityTuner extends LinearOpMode { + public static double RUNTIME = 2.0; + + private ElapsedTime timer; + private double maxVelocity = 0.0; + + private VoltageSensor batteryVoltageSensor; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(1, 0, 0)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + + maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity); + } + + drive.setDrivePower(new Pose2d()); + + double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity)); + + telemetry.addData("Max Velocity", maxVelocity); + telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) idle(); + } + + private double veloInchesToTicks(double inchesPerSec) { + return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MotorDirectionDebugger.java new file mode 100644 index 0000000..d49b45b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MotorDirectionDebugger.java @@ -0,0 +1,90 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +/** + * This is a simple teleop routine for debugging your motor configuration. + * Pressing each of the buttons will power its respective motor. + * + * Button Mappings: + * + * Xbox/PS4 Button - Motor + * X / â–¢ - Front Left + * Y / Δ - Front Right + * B / O - Rear Right + * A / X - Rear Left + * The buttons are mapped to match the wheels spatially if you + * were to rotate the gamepad 45deg°. x/square is the front left + * ________ and each button corresponds to the wheel as you go clockwise + * / ______ \ + * ------------.-' _ '-..+ Front of Bot + * / _ ( Y ) _ \ ^ + * | ( X ) _ ( B ) | Front Left \ Front Right + * ___ '. ( A ) /| Wheel \ Wheel + * .' '. '-._____.-' .' (x/â–¢) \ (Y/Δ) + * | | | \ + * '.___.' '. | Rear Left \ Rear Right + * '. / Wheel \ Wheel + * \. .' (A/X) \ (B/O) + * \________/ + * + * Uncomment the @Disabled tag below to use this opmode. + */ +//@Config +@TeleOp(group = "drive") +public class MotorDirectionDebugger extends LinearOpMode { + public static double MOTOR_POWER = 0.7; + + @Override + public void runOpMode() throws InterruptedException { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + telemetry.addLine("Press play to begin the debugging opmode"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + while (!isStopRequested()) { + telemetry.addLine("Press each button to turn on its respective motor"); + telemetry.addLine(); + telemetry.addLine("Xbox/PS4 Button - Motor"); + telemetry.addLine("  X / â–¢         - Front Left"); + telemetry.addLine("  Y / Δ         - Front Right"); + telemetry.addLine("  B / O         - Rear  Right"); + telemetry.addLine("  A / X         - Rear  Left"); + telemetry.addLine(); + + if(gamepad1.x) { + drive.setMotorPowers(MOTOR_POWER, 0, 0, 0); + telemetry.addLine("Running Motor: Front Left"); + } else if(gamepad1.y) { + drive.setMotorPowers(0, 0, 0, MOTOR_POWER); + telemetry.addLine("Running Motor: Front Right"); + } else if(gamepad1.b) { + drive.setMotorPowers(0, 0, MOTOR_POWER, 0); + telemetry.addLine("Running Motor: Rear Right"); + } else if(gamepad1.a) { + drive.setMotorPowers(0, MOTOR_POWER, 0, 0); + telemetry.addLine("Running Motor: Rear Left"); + } else { + drive.setMotorPowers(0, 0, 0, 0); + telemetry.addLine("Running Motor: None"); + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/SplineTest.java new file mode 100644 index 0000000..29b6af6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/SplineTest.java @@ -0,0 +1,40 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +/* + * This is an example of a more complex path to really test the tuning. + */ +@Disabled +@Autonomous(group = "drive") +public class SplineTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + Trajectory traj = drive.trajectoryBuilder(new Pose2d()) + .splineTo(new Vector2d(30, 30), 0) + .build(); + + drive.followTrajectory(traj); + + sleep(2000); + + drive.followTrajectory( + drive.trajectoryBuilder(traj.end(), true) + .splineTo(new Vector2d(0, 0), Math.toRadians(180)) + .build() + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StrafeTest.java new file mode 100644 index 0000000..8a4ec02 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StrafeTest.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test translational drive capabilities. + */ +//@Config +@Disabled +@Autonomous(group = "drive") +public class StrafeTest extends LinearOpMode { + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .strafeRight(DISTANCE) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) ; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StraightTest.java new file mode 100644 index 0000000..da1d7fe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StraightTest.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test translational drive capabilities. + */ +//@Config +@Autonomous(group = "drive") +public class StraightTest extends LinearOpMode { + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .forward(DISTANCE) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) ; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackWidthTuner.java new file mode 100644 index 0000000..a49fbec --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackWidthTuner.java @@ -0,0 +1,88 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.MovingStatistics; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +/* + * This routine determines the effective track width. The procedure works by executing a point turn + * with a given angle and measuring the difference between that angle and the actual angle (as + * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient + * given angle / actual angle gives a multiplicative adjustment to the estimated track width + * (effective track width = estimated track width * given angle / actual angle). The routine repeats + * this procedure a few times and averages the values for additional accuracy. Note: a relatively + * accurate track width estimate is important or else the angular constraints will be thrown off. + */ +//@Config +@Disabled +@Autonomous(group = "drive") +public class TrackWidthTuner extends LinearOpMode { + public static double ANGLE = 180; // deg + public static int NUM_TRIALS = 5; + public static int DELAY = 1000; // ms + + @Override + public void runOpMode() throws InterruptedException { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + // TODO: if you haven't already, set the localizer to something that doesn't depend on + // drive encoders for computing the heading + + telemetry.addLine("Press play to begin the track width tuner routine"); + telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS); + for (int i = 0; i < NUM_TRIALS; i++) { + drive.setPoseEstimate(new Pose2d()); + + // it is important to handle heading wraparounds + double headingAccumulator = 0; + double lastHeading = 0; + + drive.turnAsync(Math.toRadians(ANGLE)); + + while (!isStopRequested() && drive.isBusy()) { + double heading = drive.getPoseEstimate().getHeading(); + headingAccumulator += Angle.normDelta(heading - lastHeading); + lastHeading = heading; + + drive.update(); + } + + double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator; + trackWidthStats.add(trackWidth); + + sleep(DELAY); + } + + telemetry.clearAll(); + telemetry.addLine("Tuning complete"); + telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)", + trackWidthStats.getMean(), + trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS))); + telemetry.update(); + + while (!isStopRequested()) { + idle(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java new file mode 100644 index 0000000..81dfe4d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java @@ -0,0 +1,104 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.MovingStatistics; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.roadrunner.drive.StandardTrackingWheelLocalizer; + +/** + * This routine determines the effective forward offset for the lateral tracking wheel. + * The procedure executes a point turn at a given angle for a certain number of trials, + * along with a specified delay in milliseconds. The purpose of this is to track the + * change in the y position during the turn. The offset, or distance, of the lateral tracking + * wheel from the center or rotation allows the wheel to spin during a point turn, leading + * to an incorrect measurement for the y position. This creates an arc around around + * the center of rotation with an arc length of change in y and a radius equal to the forward + * offset. We can compute this offset by calculating (change in y position) / (change in heading) + * which returns the radius if the angle (change in heading) is in radians. This is based + * on the arc length formula of length = theta * radius. + * + * To run this routine, simply adjust the desired angle and specify the number of trials + * and the desired delay. Then, run the procedure. Once it finishes, it will print the + * average of all the calculated forward offsets derived from the calculation. This calculated + * forward offset is then added onto the current forward offset to produce an overall estimate + * for the forward offset. You can run this procedure as many times as necessary until a + * satisfactory result is produced. + */ +//@Config +@Disabled +@Autonomous(group="drive") +public class TrackingWheelForwardOffsetTuner extends LinearOpMode { + public static double ANGLE = 180; // deg + public static int NUM_TRIALS = 5; + public static int DELAY = 1000; // ms + + @Override + public void runOpMode() throws InterruptedException { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { + RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " + + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" + + "(hardwareMap));\" is called in SampleMecanumDrive.java"); + } + + telemetry.addLine("Press play to begin the forward offset tuner"); + telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS); + for (int i = 0; i < NUM_TRIALS; i++) { + drive.setPoseEstimate(new Pose2d()); + + // it is important to handle heading wraparounds + double headingAccumulator = 0; + double lastHeading = 0; + + drive.turnAsync(Math.toRadians(ANGLE)); + + while (!isStopRequested() && drive.isBusy()) { + double heading = drive.getPoseEstimate().getHeading(); + headingAccumulator += Angle.norm(heading - lastHeading); + lastHeading = heading; + + drive.update(); + } + + double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET + + drive.getPoseEstimate().getY() / headingAccumulator; + forwardOffsetStats.add(forwardOffset); + + sleep(DELAY); + } + + telemetry.clearAll(); + telemetry.addLine("Tuning complete"); + telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)", + forwardOffsetStats.getMean(), + forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS))); + telemetry.update(); + + while (!isStopRequested()) { + idle(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java new file mode 100644 index 0000000..d14e016 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java @@ -0,0 +1,131 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.roadrunner.drive.StandardTrackingWheelLocalizer; + +/** + * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s + * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel + * wheels. + * + * Tuning Routine: + * + * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical + * measured value. This need only be an estimated value as you will be tuning it anyways. + * + * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an + * similar mark right below the indicator on your bot. This will be your reference point to + * ensure you've turned exactly 360°. + * + * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help + * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash + * if you are using the Control Hub. + * Ensure the field is showing (select the field view in top right of the page). + * + * 4. Press play to begin the tuning routine. + * + * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise. + * + * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns. + * + * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators + * on the bot and on the ground you created earlier should be lined up. + * + * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your + * StandardTrackingWheelLocalizer.java class. + * + * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value + * yourself. Read the heading output and follow the advice stated in the note below to manually + * nudge the values yourself. + * + * Note: + * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with + * a line from the circumference to the center should be present, representing the bot. The line + * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in + * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than + * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the + * actual bot, the LATERAL_DISTANCE should be increased. + * + * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the + * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the + * effective center of rotation. You can ignore this effect. The center of rotation will be offset + * slightly but your heading will still be fine. This does not affect your overall tracking + * precision. The heading should still line up. + */ +//@Config +@Disabled +@TeleOp(group = "drive") +public class TrackingWheelLateralDistanceTuner extends LinearOpMode { + public static int NUM_TURNS = 10; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { + RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " + + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" + + "(hardwareMap));\" is called in SampleMecanumDrive.java"); + } + + telemetry.addLine("Prior to beginning the routine, please read the directions " + + "located in the comments of the opmode file."); + telemetry.addLine("Press play to begin the tuning routine."); + telemetry.addLine(""); + telemetry.addLine("Press Y/â–³ to stop the routine."); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.update(); + + double headingAccumulator = 0; + double lastHeading = 0; + + boolean tuningFinished = false; + + while (!isStopRequested() && !tuningFinished) { + Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x); + drive.setDrivePower(vel); + + drive.update(); + + double heading = drive.getPoseEstimate().getHeading(); + double deltaHeading = heading - lastHeading; + + headingAccumulator += Angle.normDelta(deltaHeading); + lastHeading = heading; + + telemetry.clearAll(); + telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator)); + telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading)); + telemetry.addLine(); + telemetry.addLine("Press Y/â–³ to conclude routine"); + telemetry.update(); + + if (gamepad1.y) + tuningFinished = true; + } + + telemetry.clearAll(); + telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°"); + telemetry.addLine("Effective LATERAL_DISTANCE: " + + (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE); + + telemetry.update(); + + while (!isStopRequested()) idle(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TurnTest.java new file mode 100644 index 0000000..2d80f29 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TurnTest.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test turning capabilities. + */ +//@Config +@Disabled +@Autonomous(group = "drive") +public class TurnTest extends LinearOpMode { + public static double ANGLE = 90; // deg + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + drive.turn(Math.toRadians(ANGLE)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/EmptySequenceException.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/EmptySequenceException.java new file mode 100644 index 0000000..f778388 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/EmptySequenceException.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence; + + +public class EmptySequenceException extends RuntimeException { } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequence.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequence.java new file mode 100644 index 0000000..8d7ebdd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequence.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.SequenceSegment; + +import java.util.Collections; +import java.util.List; + +public class TrajectorySequence { + private final List sequenceList; + + public TrajectorySequence(List sequenceList) { + if (sequenceList.size() == 0) throw new EmptySequenceException(); + + this.sequenceList = Collections.unmodifiableList(sequenceList); + } + + public Pose2d start() { + return sequenceList.get(0).getStartPose(); + } + + public Pose2d end() { + return sequenceList.get(sequenceList.size() - 1).getEndPose(); + } + + public double duration() { + double total = 0.0; + + for (SequenceSegment segment : sequenceList) { + total += segment.getDuration(); + } + + return total; + } + + public SequenceSegment get(int i) { + return sequenceList.get(i); + } + + public int size() { + return sequenceList.size(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequenceBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequenceBuilder.java new file mode 100644 index 0000000..3cf0bc5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequenceBuilder.java @@ -0,0 +1,711 @@ +package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.path.PathContinuityViolationException; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.trajectory.DisplacementMarker; +import com.acmerobotics.roadrunner.trajectory.DisplacementProducer; +import com.acmerobotics.roadrunner.trajectory.MarkerCallback; +import com.acmerobotics.roadrunner.trajectory.SpatialMarker; +import com.acmerobotics.roadrunner.trajectory.TemporalMarker; +import com.acmerobotics.roadrunner.trajectory.TimeProducer; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.acmerobotics.roadrunner.util.Angle; + +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.SequenceSegment; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.TurnSegment; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.WaitSegment; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +public class TrajectorySequenceBuilder { + private final double resolution = 0.25; + + private final TrajectoryVelocityConstraint baseVelConstraint; + private final TrajectoryAccelerationConstraint baseAccelConstraint; + + private TrajectoryVelocityConstraint currentVelConstraint; + private TrajectoryAccelerationConstraint currentAccelConstraint; + + private final double baseTurnConstraintMaxAngVel; + private final double baseTurnConstraintMaxAngAccel; + + private double currentTurnConstraintMaxAngVel; + private double currentTurnConstraintMaxAngAccel; + + private final List sequenceSegments; + + private final List temporalMarkers; + private final List displacementMarkers; + private final List spatialMarkers; + + private Pose2d lastPose; + + private double tangentOffset; + + private boolean setAbsoluteTangent; + private double absoluteTangent; + + private TrajectoryBuilder currentTrajectoryBuilder; + + private double currentDuration; + private double currentDisplacement; + + private double lastDurationTraj; + private double lastDisplacementTraj; + + public TrajectorySequenceBuilder( + Pose2d startPose, + Double startTangent, + TrajectoryVelocityConstraint baseVelConstraint, + TrajectoryAccelerationConstraint baseAccelConstraint, + double baseTurnConstraintMaxAngVel, + double baseTurnConstraintMaxAngAccel + ) { + this.baseVelConstraint = baseVelConstraint; + this.baseAccelConstraint = baseAccelConstraint; + + this.currentVelConstraint = baseVelConstraint; + this.currentAccelConstraint = baseAccelConstraint; + + this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel; + this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel; + + this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel; + this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel; + + sequenceSegments = new ArrayList<>(); + + temporalMarkers = new ArrayList<>(); + displacementMarkers = new ArrayList<>(); + spatialMarkers = new ArrayList<>(); + + lastPose = startPose; + + tangentOffset = 0.0; + + setAbsoluteTangent = (startTangent != null); + absoluteTangent = startTangent != null ? startTangent : 0.0; + + currentTrajectoryBuilder = null; + + currentDuration = 0.0; + currentDisplacement = 0.0; + + lastDurationTraj = 0.0; + lastDisplacementTraj = 0.0; + } + + public TrajectorySequenceBuilder( + Pose2d startPose, + TrajectoryVelocityConstraint baseVelConstraint, + TrajectoryAccelerationConstraint baseAccelConstraint, + double baseTurnConstraintMaxAngVel, + double baseTurnConstraintMaxAngAccel + ) { + this( + startPose, null, + baseVelConstraint, baseAccelConstraint, + baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel + ); + } + + public TrajectorySequenceBuilder lineTo(Vector2d endPosition) { + return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineTo( + Vector2d endPosition, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) { + return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineToConstantHeading( + Vector2d endPosition, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) { + return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineToLinearHeading( + Pose2d endPose, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) { + return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineToSplineHeading( + Pose2d endPose, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) { + return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder strafeTo( + Vector2d endPosition, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder forward(double distance) { + return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder forward( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder back(double distance) { + return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder back( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder strafeLeft(double distance) { + return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder strafeLeft( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder strafeRight(double distance) { + return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder strafeRight( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineTo( + Vector2d endPosition, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineToConstantHeading( + Vector2d endPosition, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineToLinearHeading( + Pose2d endPose, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineToSplineHeading( + Pose2d endPose, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint)); + } + + private TrajectorySequenceBuilder addPath(AddPathCallback callback) { + if (currentTrajectoryBuilder == null) newPath(); + + try { + callback.run(); + } catch (PathContinuityViolationException e) { + newPath(); + callback.run(); + } + + Trajectory builtTraj = currentTrajectoryBuilder.build(); + + double durationDifference = builtTraj.duration() - lastDurationTraj; + double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj; + + lastPose = builtTraj.end(); + currentDuration += durationDifference; + currentDisplacement += displacementDifference; + + lastDurationTraj = builtTraj.duration(); + lastDisplacementTraj = builtTraj.getPath().length(); + + return this; + } + + public TrajectorySequenceBuilder setTangent(double tangent) { + setAbsoluteTangent = true; + absoluteTangent = tangent; + + pushPath(); + + return this; + } + + private TrajectorySequenceBuilder setTangentOffset(double offset) { + setAbsoluteTangent = false; + + this.tangentOffset = offset; + this.pushPath(); + + return this; + } + + public TrajectorySequenceBuilder setReversed(boolean reversed) { + return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0); + } + + public TrajectorySequenceBuilder setConstraints( + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + this.currentVelConstraint = velConstraint; + this.currentAccelConstraint = accelConstraint; + + return this; + } + + public TrajectorySequenceBuilder resetConstraints() { + this.currentVelConstraint = this.baseVelConstraint; + this.currentAccelConstraint = this.baseAccelConstraint; + + return this; + } + + public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) { + this.currentVelConstraint = velConstraint; + + return this; + } + + public TrajectorySequenceBuilder resetVelConstraint() { + this.currentVelConstraint = this.baseVelConstraint; + + return this; + } + + public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) { + this.currentAccelConstraint = accelConstraint; + + return this; + } + + public TrajectorySequenceBuilder resetAccelConstraint() { + this.currentAccelConstraint = this.baseAccelConstraint; + + return this; + } + + public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) { + this.currentTurnConstraintMaxAngVel = maxAngVel; + this.currentTurnConstraintMaxAngAccel = maxAngAccel; + + return this; + } + + public TrajectorySequenceBuilder resetTurnConstraint() { + this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel; + this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel; + + return this; + } + + public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) { + return this.addTemporalMarker(currentDuration, callback); + } + + public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) { + return this.addTemporalMarker(currentDuration + offset, callback); + } + + public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) { + return this.addTemporalMarker(0.0, time, callback); + } + + public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) { + return this.addTemporalMarker(time -> scale * time + offset, callback); + } + + public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) { + this.temporalMarkers.add(new TemporalMarker(time, callback)); + return this; + } + + public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) { + this.spatialMarkers.add(new SpatialMarker(point, callback)); + return this; + } + + public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) { + return this.addDisplacementMarker(currentDisplacement, callback); + } + + public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) { + return this.addDisplacementMarker(currentDisplacement + offset, callback); + } + + public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) { + return this.addDisplacementMarker(0.0, displacement, callback); + } + + public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) { + return addDisplacementMarker((displacement -> scale * displacement + offset), callback); + } + + public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) { + displacementMarkers.add(new DisplacementMarker(displacement, callback)); + + return this; + } + + public TrajectorySequenceBuilder turn(double angle) { + return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel); + } + + public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) { + pushPath(); + + MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile( + new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0), + new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0), + maxAngVel, + maxAngAccel + ); + + sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList())); + + lastPose = new Pose2d( + lastPose.getX(), lastPose.getY(), + Angle.norm(lastPose.getHeading() + angle) + ); + + currentDuration += turnProfile.duration(); + + return this; + } + + public TrajectorySequenceBuilder waitSeconds(double seconds) { + pushPath(); + sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList())); + + currentDuration += seconds; + return this; + } + + public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) { + pushPath(); + + sequenceSegments.add(new TrajectorySegment(trajectory)); + return this; + } + + private void pushPath() { + if (currentTrajectoryBuilder != null) { + Trajectory builtTraj = currentTrajectoryBuilder.build(); + sequenceSegments.add(new TrajectorySegment(builtTraj)); + } + + currentTrajectoryBuilder = null; + } + + private void newPath() { + if (currentTrajectoryBuilder != null) + pushPath(); + + lastDurationTraj = 0.0; + lastDisplacementTraj = 0.0; + + double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset); + + currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution); + } + + public TrajectorySequence build() { + pushPath(); + + List globalMarkers = convertMarkersToGlobal( + sequenceSegments, + temporalMarkers, displacementMarkers, spatialMarkers + ); + + return new TrajectorySequence(projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments)); + } + + private List convertMarkersToGlobal( + List sequenceSegments, + List temporalMarkers, + List displacementMarkers, + List spatialMarkers + ) { + ArrayList trajectoryMarkers = new ArrayList<>(); + + // Convert temporal markers + for (TemporalMarker marker : temporalMarkers) { + trajectoryMarkers.add( + new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback()) + ); + } + + // Convert displacement markers + for (DisplacementMarker marker : displacementMarkers) { + double time = displacementToTime( + sequenceSegments, + marker.getProducer().produce(currentDisplacement) + ); + + trajectoryMarkers.add( + new TrajectoryMarker( + time, + marker.getCallback() + ) + ); + } + + // Convert spatial markers + for (SpatialMarker marker : spatialMarkers) { + trajectoryMarkers.add( + new TrajectoryMarker( + pointToTime(sequenceSegments, marker.getPoint()), + marker.getCallback() + ) + ); + } + + return trajectoryMarkers; + } + + private List projectGlobalMarkersToLocalSegments(List markers, List sequenceSegments) { + if (sequenceSegments.isEmpty()) return Collections.emptyList(); + + double totalSequenceDuration = 0; + for (SequenceSegment segment : sequenceSegments) { + totalSequenceDuration += segment.getDuration(); + } + + for (TrajectoryMarker marker : markers) { + SequenceSegment segment = null; + int segmentIndex = 0; + double segmentOffsetTime = 0; + + double currentTime = 0; + for (int i = 0; i < sequenceSegments.size(); i++) { + SequenceSegment seg = sequenceSegments.get(i); + + double markerTime = Math.min(marker.getTime(), totalSequenceDuration); + + if (currentTime + seg.getDuration() >= markerTime) { + segment = seg; + segmentIndex = i; + segmentOffsetTime = markerTime - currentTime; + + break; + } else { + currentTime += seg.getDuration(); + } + } + + SequenceSegment newSegment = null; + + if (segment instanceof WaitSegment) { + List newMarkers = new ArrayList<>(segment.getMarkers()); + + newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers()); + newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback())); + + WaitSegment thisSegment = (WaitSegment) segment; + newSegment = new WaitSegment(thisSegment.getStartPose(), thisSegment.getDuration(), newMarkers); + } else if (segment instanceof TurnSegment) { + List newMarkers = new ArrayList<>(segment.getMarkers()); + + newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers()); + newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback())); + + TurnSegment thisSegment = (TurnSegment) segment; + newSegment = new TurnSegment(thisSegment.getStartPose(), thisSegment.getTotalRotation(), thisSegment.getMotionProfile(), newMarkers); + } else if (segment instanceof TrajectorySegment) { + TrajectorySegment thisSegment = (TrajectorySegment) segment; + + List newMarkers = new ArrayList<>(thisSegment.getTrajectory().getMarkers()); + newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback())); + + newSegment = new TrajectorySegment(new Trajectory(thisSegment.getTrajectory().getPath(), thisSegment.getTrajectory().getProfile(), newMarkers)); + } + + sequenceSegments.set(segmentIndex, newSegment); + } + + return sequenceSegments; + } + + // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private + // note: this assumes that the profile position is monotonic increasing + private Double motionProfileDisplacementToTime(MotionProfile profile, double s) { + double tLo = 0.0; + double tHi = profile.duration(); + while (!(Math.abs(tLo - tHi) < 1e-6)) { + double tMid = 0.5 * (tLo + tHi); + if (profile.get(tMid).getX() > s) { + tHi = tMid; + } else { + tLo = tMid; + } + } + return 0.5 * (tLo + tHi); + } + + private Double displacementToTime(List sequenceSegments, double s) { + double currentTime = 0.0; + double currentDisplacement = 0.0; + + for (SequenceSegment segment : sequenceSegments) { + if (segment instanceof TrajectorySegment) { + TrajectorySegment thisSegment = (TrajectorySegment) segment; + + double segmentLength = thisSegment.getTrajectory().getPath().length(); + + if (currentDisplacement + segmentLength > s) { + double target = s - currentDisplacement; + double timeInSegment = motionProfileDisplacementToTime( + thisSegment.getTrajectory().getProfile(), + target + ); + + return currentTime + timeInSegment; + } else { + currentDisplacement += segmentLength; + currentTime += thisSegment.getTrajectory().duration(); + } + } else { + currentTime += segment.getDuration(); + } + } + + return 0.0; + } + + private Double pointToTime(List sequenceSegments, Vector2d point) { + class ComparingPoints { + private final double distanceToPoint; + private final double totalDisplacement; + private final double thisPathDisplacement; + + public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) { + this.distanceToPoint = distanceToPoint; + this.totalDisplacement = totalDisplacement; + this.thisPathDisplacement = thisPathDisplacement; + } + } + + List projectedPoints = new ArrayList<>(); + + for (SequenceSegment segment : sequenceSegments) { + if (segment instanceof TrajectorySegment) { + TrajectorySegment thisSegment = (TrajectorySegment) segment; + + double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25); + Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec(); + double distanceToPoint = point.minus(projectedPoint).norm(); + + double totalDisplacement = 0.0; + + for (ComparingPoints comparingPoint : projectedPoints) { + totalDisplacement += comparingPoint.totalDisplacement; + } + + totalDisplacement += displacement; + + projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement)); + } + } + + ComparingPoints closestPoint = null; + + for (ComparingPoints comparingPoint : projectedPoints) { + if (closestPoint == null) { + closestPoint = comparingPoint; + continue; + } + + if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint) + closestPoint = comparingPoint; + } + + return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement); + } + + private interface AddPathCallback { + void run(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequenceRunner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequenceRunner.java new file mode 100644 index 0000000..a510e6f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequenceRunner.java @@ -0,0 +1,271 @@ +package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence; + +import androidx.annotation.Nullable; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.control.PIDFController; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.followers.TrajectoryFollower; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; +import com.acmerobotics.roadrunner.util.NanoClock; + +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.SequenceSegment; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.TurnSegment; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.WaitSegment; +import org.firstinspires.ftc.teamcode.roadrunner.util.DashboardUtil; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.LinkedList; +import java.util.List; + +public class TrajectorySequenceRunner { + public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a"; + public static String COLOR_INACTIVE_TURN = "#7c4dff7a"; + public static String COLOR_INACTIVE_WAIT = "#dd2c007a"; + + public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50"; + public static String COLOR_ACTIVE_TURN = "#7c4dff"; + public static String COLOR_ACTIVE_WAIT = "#dd2c00"; + + public static int POSE_HISTORY_LIMIT = 100; + + private final TrajectoryFollower follower; + + private final PIDFController turnController; + + private final NanoClock clock; + + private TrajectorySequence currentTrajectorySequence; + private double currentSegmentStartTime; + private int currentSegmentIndex; + private int lastSegmentIndex; + + private Pose2d lastPoseError = new Pose2d(); + + List remainingMarkers = new ArrayList<>(); + + private final FtcDashboard dashboard; + private final LinkedList poseHistory = new LinkedList<>(); + + public TrajectorySequenceRunner(TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients) { + this.follower = follower; + + turnController = new PIDFController(headingPIDCoefficients); + turnController.setInputBounds(0, 2 * Math.PI); + + clock = NanoClock.system(); + + dashboard = FtcDashboard.getInstance(); + dashboard.setTelemetryTransmissionInterval(25); + } + + public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) { + currentTrajectorySequence = trajectorySequence; + currentSegmentStartTime = clock.seconds(); + currentSegmentIndex = 0; + lastSegmentIndex = -1; + } + + public @Nullable + DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) { + Pose2d targetPose = null; + DriveSignal driveSignal = null; + + TelemetryPacket packet = new TelemetryPacket(); + Canvas fieldOverlay = packet.fieldOverlay(); + + SequenceSegment currentSegment = null; + + if (currentTrajectorySequence != null) { + if (currentSegmentIndex >= currentTrajectorySequence.size()) { + for (TrajectoryMarker marker : remainingMarkers) { + marker.getCallback().onMarkerReached(); + } + + remainingMarkers.clear(); + + currentTrajectorySequence = null; + } + + if (currentTrajectorySequence == null) + return new DriveSignal(); + + double now = clock.seconds(); + boolean isNewTransition = currentSegmentIndex != lastSegmentIndex; + + currentSegment = currentTrajectorySequence.get(currentSegmentIndex); + + if (isNewTransition) { + currentSegmentStartTime = now; + lastSegmentIndex = currentSegmentIndex; + + for (TrajectoryMarker marker : remainingMarkers) { + marker.getCallback().onMarkerReached(); + } + + remainingMarkers.clear(); + + remainingMarkers.addAll(currentSegment.getMarkers()); + Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime())); + } + + double deltaTime = now - currentSegmentStartTime; + + if (currentSegment instanceof TrajectorySegment) { + Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory(); + + if (isNewTransition) + follower.followTrajectory(currentTrajectory); + + if (!follower.isFollowing()) { + currentSegmentIndex++; + + driveSignal = new DriveSignal(); + } else { + driveSignal = follower.update(poseEstimate, poseVelocity); + lastPoseError = follower.getLastError(); + } + + targetPose = currentTrajectory.get(deltaTime); + } else if (currentSegment instanceof TurnSegment) { + MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime); + + turnController.setTargetPosition(targetState.getX()); + + double correction = turnController.update(poseEstimate.getHeading()); + + double targetOmega = targetState.getV(); + double targetAlpha = targetState.getA(); + + lastPoseError = new Pose2d(0, 0, turnController.getLastError()); + + Pose2d startPose = currentSegment.getStartPose(); + targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX()); + + driveSignal = new DriveSignal( + new Pose2d(0, 0, targetOmega + correction), + new Pose2d(0, 0, targetAlpha) + ); + + if (deltaTime >= currentSegment.getDuration()) { + currentSegmentIndex++; + driveSignal = new DriveSignal(); + } + } else if (currentSegment instanceof WaitSegment) { + lastPoseError = new Pose2d(); + + targetPose = currentSegment.getStartPose(); + driveSignal = new DriveSignal(); + + if (deltaTime >= currentSegment.getDuration()) { + currentSegmentIndex++; + } + } + + while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) { + remainingMarkers.get(0).getCallback().onMarkerReached(); + remainingMarkers.remove(0); + } + } + + poseHistory.add(poseEstimate); + + if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) { + poseHistory.removeFirst(); + } + + packet.put("x", poseEstimate.getX()); + packet.put("y", poseEstimate.getY()); + packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading())); + + packet.put("xError", getLastPoseError().getX()); + packet.put("yError", getLastPoseError().getY()); + packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading())); + + draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate); + + dashboard.sendTelemetryPacket(packet); + + return driveSignal; + } + + private void draw( + Canvas fieldOverlay, + TrajectorySequence sequence, SequenceSegment currentSegment, + Pose2d targetPose, Pose2d poseEstimate + ) { + if (sequence != null) { + for (int i = 0; i < sequence.size(); i++) { + SequenceSegment segment = sequence.get(i); + + if (segment instanceof TrajectorySegment) { + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY); + + DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath()); + } else if (segment instanceof TurnSegment) { + Pose2d pose = segment.getStartPose(); + + fieldOverlay.setFill(COLOR_INACTIVE_TURN); + fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2); + } else if (segment instanceof WaitSegment) { + Pose2d pose = segment.getStartPose(); + + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_INACTIVE_WAIT); + fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3); + } + } + } + + if (currentSegment != null) { + if (currentSegment instanceof TrajectorySegment) { + Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory(); + + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY); + + DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath()); + } else if (currentSegment instanceof TurnSegment) { + Pose2d pose = currentSegment.getStartPose(); + + fieldOverlay.setFill(COLOR_ACTIVE_TURN); + fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3); + } else if (currentSegment instanceof WaitSegment) { + Pose2d pose = currentSegment.getStartPose(); + + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_ACTIVE_WAIT); + fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3); + } + } + + if (targetPose != null) { + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke("#4CAF50"); + DashboardUtil.drawRobot(fieldOverlay, targetPose); + } + + fieldOverlay.setStroke("#3F51B5"); + DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory); + + fieldOverlay.setStroke("#3F51B5"); + DashboardUtil.drawRobot(fieldOverlay, poseEstimate); + } + + public Pose2d getLastPoseError() { + return lastPoseError; + } + + public boolean isBusy() { + return currentTrajectorySequence != null; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.java new file mode 100644 index 0000000..b7ab8dd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.java @@ -0,0 +1,40 @@ +package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; + +import java.util.List; + +public abstract class SequenceSegment { + private final double duration; + private final Pose2d startPose; + private final Pose2d endPose; + private final List markers; + + protected SequenceSegment( + double duration, + Pose2d startPose, Pose2d endPose, + List markers + ) { + this.duration = duration; + this.startPose = startPose; + this.endPose = endPose; + this.markers = markers; + } + + public double getDuration() { + return this.duration; + } + + public Pose2d getStartPose() { + return startPose; + } + + public Pose2d getEndPose() { + return endPose; + } + + public List getMarkers() { + return markers; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.java new file mode 100644 index 0000000..06db735 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.trajectory.Trajectory; + +import java.util.Collections; + +public final class TrajectorySegment extends SequenceSegment { + private final Trajectory trajectory; + + public TrajectorySegment(Trajectory trajectory) { + // Note: Markers are already stored in the `Trajectory` itself. + // This class should not hold any markers + super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList()); + this.trajectory = trajectory; + } + + public Trajectory getTrajectory() { + return this.trajectory; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/TurnSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/TurnSegment.java new file mode 100644 index 0000000..7ee14ac --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/TurnSegment.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; +import com.acmerobotics.roadrunner.util.Angle; + +import java.util.List; + +public final class TurnSegment extends SequenceSegment { + private final double totalRotation; + private final MotionProfile motionProfile; + + public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List markers) { + super( + motionProfile.duration(), + startPose, + new Pose2d( + startPose.getX(), startPose.getY(), + Angle.norm(startPose.getHeading() + totalRotation) + ), + markers + ); + + this.totalRotation = totalRotation; + this.motionProfile = motionProfile; + } + + public final double getTotalRotation() { + return this.totalRotation; + } + + public final MotionProfile getMotionProfile() { + return this.motionProfile; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/WaitSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/WaitSegment.java new file mode 100644 index 0000000..11d8e56 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/WaitSegment.java @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; + +import java.util.List; + +public final class WaitSegment extends SequenceSegment { + public WaitSegment(Pose2d pose, double seconds, List markers) { + super(seconds, pose, pose, markers); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AssetsTrajectoryManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AssetsTrajectoryManager.java new file mode 100644 index 0000000..7135a7e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AssetsTrajectoryManager.java @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.teamcode.roadrunner.util; + +import androidx.annotation.Nullable; + +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig; +import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager; +import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig; + +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; + +import java.io.IOException; +import java.io.InputStream; + +/** + * Set of utilities for loading trajectories from assets (the plugin save location). + */ +public class AssetsTrajectoryManager { + + /** + * Loads the group config. + */ + public static @Nullable + TrajectoryGroupConfig loadGroupConfig() { + try { + InputStream inputStream = AppUtil.getDefContext().getAssets().open( + "trajectory/" + TrajectoryConfigManager.GROUP_FILENAME); + return TrajectoryConfigManager.loadGroupConfig(inputStream); + } catch (IOException e) { + return null; + } + } + + /** + * Loads a trajectory config with the given name. + */ + public static @Nullable TrajectoryConfig loadConfig(String name) { + try { + InputStream inputStream = AppUtil.getDefContext().getAssets().open( + "trajectory/" + name + ".yaml"); + return TrajectoryConfigManager.loadConfig(inputStream); + } catch (IOException e) { + return null; + } + } + + /** + * Loads a trajectory builder with the given name. + */ + public static @Nullable TrajectoryBuilder loadBuilder(String name) { + TrajectoryGroupConfig groupConfig = loadGroupConfig(); + TrajectoryConfig config = loadConfig(name); + if (groupConfig == null || config == null) { + return null; + } + return config.toTrajectoryBuilder(groupConfig); + } + + /** + * Loads a trajectory with the given name. + */ + public static @Nullable Trajectory load(String name) { + TrajectoryBuilder builder = loadBuilder(name); + if (builder == null) { + return null; + } + return builder.build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AxesSigns.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AxesSigns.java new file mode 100644 index 0000000..7690722 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AxesSigns.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.teamcode.roadrunner.util; + +/** + * IMU axes signs in the order XYZ (after remapping). + */ +public enum AxesSigns { + PPP(0b000), + PPN(0b001), + PNP(0b010), + PNN(0b011), + NPP(0b100), + NPN(0b101), + NNP(0b110), + NNN(0b111); + + public final int bVal; + + AxesSigns(int bVal) { + this.bVal = bVal; + } + + public static AxesSigns fromBinaryValue(int bVal) { + int maskedVal = bVal & 0x07; + switch (maskedVal) { + case 0b000: + return AxesSigns.PPP; + case 0b001: + return AxesSigns.PPN; + case 0b010: + return AxesSigns.PNP; + case 0b011: + return AxesSigns.PNN; + case 0b100: + return AxesSigns.NPP; + case 0b101: + return AxesSigns.NPN; + case 0b110: + return AxesSigns.NNP; + case 0b111: + return AxesSigns.NNN; + default: + throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AxisDirection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AxisDirection.java new file mode 100644 index 0000000..cbb9605 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AxisDirection.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.roadrunner.util; + +/** + * A direction for an axis to be remapped to + */ +public enum AxisDirection { + POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/BNO055IMUUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/BNO055IMUUtil.java new file mode 100644 index 0000000..1c76186 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/BNO055IMUUtil.java @@ -0,0 +1,128 @@ +package org.firstinspires.ftc.teamcode.roadrunner.util; + +import com.qualcomm.hardware.bosch.BNO055IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; + +/** + * Various utility functions for the BNO055 IMU. + */ +public class BNO055IMUUtil { + /** + * Error for attempting an illegal remapping (lhs or multiple same axes) + */ + public static class InvalidAxisRemapException extends RuntimeException { + public InvalidAxisRemapException(String detailMessage) { + super(detailMessage); + } + } + + /** + * Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}. + * Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't + * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion. + * + * Adapted from this post. + * Reference the BNO055 Datasheet for details. + * + * NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if appropriate. + * + * @param imu IMU + * @param order axes order + * @param signs axes signs + */ + public static void swapThenFlipAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) { + try { + // the indices correspond with the 2-bit axis encodings specified in the datasheet + int[] indices = order.indices(); + // AxesSign's values align with the datasheet + int axisMapSigns = signs.bVal; + + if (indices[0] == indices[1] || indices[0] == indices[2] || indices[1] == indices[2]) { + throw new InvalidAxisRemapException("Same axis cannot be included in AxesOrder twice"); + } + + // ensure right-handed coordinate system + boolean isXSwapped = indices[0] != 0; + boolean isYSwapped = indices[1] != 1; + boolean isZSwapped = indices[2] != 2; + boolean areTwoAxesSwapped = (isXSwapped || isYSwapped || isZSwapped) + && (!isXSwapped || !isYSwapped || !isZSwapped); + boolean oddNumOfFlips = (((axisMapSigns >> 2) ^ (axisMapSigns >> 1) ^ axisMapSigns) & 1) == 1; + // != functions as xor + if (areTwoAxesSwapped != oddNumOfFlips) { + throw new InvalidAxisRemapException("Coordinate system is left-handed"); + } + + // Bit: 7 6 | 5 4 | 3 2 | 1 0 | + // reserved | z axis | y axis | x axis | + int axisMapConfig = indices[2] << 4 | indices[1] << 2 | indices[0]; + + // Enter CONFIG mode + imu.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F); + + Thread.sleep(100); + + // Write the AXIS_MAP_CONFIG register + imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F); + + // Write the AXIS_MAP_SIGN register + imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSigns & 0x07); + + // Switch back to the previous mode + imu.write8(BNO055IMU.Register.OPR_MODE, imu.getParameters().mode.bVal & 0x0F); + + Thread.sleep(100); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + } + + /** + * Remaps the IMU coordinate system so that the remapped +Z faces the provided + * {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping. + * + * @param imu IMU + * @param direction axis direction + */ + public static void remapZAxis(BNO055IMU imu, AxisDirection direction) { + switch (direction) { + case POS_X: + swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.NPP); + break; + case NEG_X: + swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.PPN); + break; + case POS_Y: + swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PNP); + break; + case NEG_Y: + swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PPN); + break; + case POS_Z: + swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PPP); + break; + case NEG_Z: + swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PNN); + break; + } + } + + /** + * Now deprecated due to unintuitive parameter order. + * Use {@link #swapThenFlipAxes} or {@link #remapZAxis} instead. + * + * @param imu IMU + * @param order axes order + * @param signs axes signs + */ + @Deprecated + public static void remapAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) { + AxesOrder adjustedAxesOrder = order.reverse(); + int[] indices = order.indices(); + int axisSignValue = signs.bVal ^ (0b100 >> indices[0]); + AxesSigns adjustedAxesSigns = AxesSigns.fromBinaryValue(axisSignValue); + + swapThenFlipAxes(imu, adjustedAxesOrder, adjustedAxesSigns); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/DashboardUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/DashboardUtil.java new file mode 100644 index 0000000..f244b39 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/DashboardUtil.java @@ -0,0 +1,54 @@ +package org.firstinspires.ftc.teamcode.roadrunner.util; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.path.Path; + +import java.util.List; + +/** + * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases. + */ +public class DashboardUtil { + private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches + private static final double ROBOT_RADIUS = 9; // in + + + public static void drawPoseHistory(Canvas canvas, List poseHistory) { + double[] xPoints = new double[poseHistory.size()]; + double[] yPoints = new double[poseHistory.size()]; + for (int i = 0; i < poseHistory.size(); i++) { + Pose2d pose = poseHistory.get(i); + xPoints[i] = pose.getX(); + yPoints[i] = pose.getY(); + } + canvas.strokePolyline(xPoints, yPoints); + } + + public static void drawSampledPath(Canvas canvas, Path path, double resolution) { + int samples = (int) Math.ceil(path.length() / resolution); + double[] xPoints = new double[samples]; + double[] yPoints = new double[samples]; + double dx = path.length() / (samples - 1); + for (int i = 0; i < samples; i++) { + double displacement = i * dx; + Pose2d pose = path.get(displacement); + xPoints[i] = pose.getX(); + yPoints[i] = pose.getY(); + } + canvas.strokePolyline(xPoints, yPoints); + } + + public static void drawSampledPath(Canvas canvas, Path path) { + drawSampledPath(canvas, path, DEFAULT_RESOLUTION); + } + + public static void drawRobot(Canvas canvas, Pose2d pose) { + canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS); + Vector2d v = pose.headingVec().times(ROBOT_RADIUS); + double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2; + double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY(); + canvas.strokeLine(x1, y1, x2, y2); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/Encoder.java new file mode 100644 index 0000000..f3adb99 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/Encoder.java @@ -0,0 +1,98 @@ +package org.firstinspires.ftc.teamcode.roadrunner.util; + +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** + * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding + * slot's motor direction + */ +public class Encoder { + private final static int CPS_STEP = 0x10000; + + private static double inverseOverflow(double input, double estimate) { + double real = input; + while (Math.abs(estimate - real) > CPS_STEP / 2.0) { + real += Math.signum(estimate - real) * CPS_STEP; + } + return real; + } + + public enum Direction { + FORWARD(1), + REVERSE(-1); + + private int multiplier; + + Direction(int multiplier) { + this.multiplier = multiplier; + } + + public int getMultiplier() { + return multiplier; + } + } + + private DcMotorEx motor; + private NanoClock clock; + + private Direction direction; + + private int lastPosition; + private double velocityEstimate; + private double lastUpdateTime; + + public Encoder(DcMotorEx motor, NanoClock clock) { + this.motor = motor; + this.clock = clock; + + this.direction = Direction.FORWARD; + + this.lastPosition = 0; + this.velocityEstimate = 0.0; + this.lastUpdateTime = clock.seconds(); + } + + public Encoder(DcMotorEx motor) { + this(motor, NanoClock.system()); + } + + public Direction getDirection() { + return direction; + } + + private int getMultiplier() { + return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); + } + + /** + * Allows you to set the direction of the counts and velocity without modifying the motor's direction state + * @param direction either reverse or forward depending on if encoder counts should be negated + */ + public void setDirection(Direction direction) { + this.direction = direction; + } + + public int getCurrentPosition() { + int multiplier = getMultiplier(); + int currentPosition = motor.getCurrentPosition() * multiplier; + if (currentPosition != lastPosition) { + double currentTime = clock.seconds(); + double dt = currentTime - lastUpdateTime; + velocityEstimate = (currentPosition - lastPosition) / dt; + lastPosition = currentPosition; + lastUpdateTime = currentTime; + } + return currentPosition; + } + + public double getRawVelocity() { + int multiplier = getMultiplier(); + return motor.getVelocity() * multiplier; + } + + public double getCorrectedVelocity() { + return inverseOverflow(getRawVelocity(), velocityEstimate); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/LoggingUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/LoggingUtil.java new file mode 100644 index 0000000..5df601d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/LoggingUtil.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.roadrunner.util; + +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; + +import java.io.File; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +/** + * Utility functions for log files. + */ +public class LoggingUtil { + public static final File ROAD_RUNNER_FOLDER = + new File(AppUtil.ROOT_FOLDER + "/RoadRunner/"); + + private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now + + private static void buildLogList(List logFiles, File dir) { + for (File file : dir.listFiles()) { + if (file.isDirectory()) { + buildLogList(logFiles, file); + } else { + logFiles.add(file); + } + } + } + + private static void pruneLogsIfNecessary() { + List logFiles = new ArrayList<>(); + buildLogList(logFiles, ROAD_RUNNER_FOLDER); + Collections.sort(logFiles, (lhs, rhs) -> + Long.compare(lhs.lastModified(), rhs.lastModified())); + + long dirSize = 0; + for (File file: logFiles) { + dirSize += file.length(); + } + + while (dirSize > LOG_QUOTA) { + if (logFiles.size() == 0) break; + File fileToRemove = logFiles.remove(0); + dirSize -= fileToRemove.length(); + //noinspection ResultOfMethodCallIgnored + fileToRemove.delete(); + } + } + + /** + * Obtain a log file with the provided name + */ + public static File getLogFile(String name) { + //noinspection ResultOfMethodCallIgnored + ROAD_RUNNER_FOLDER.mkdirs(); + + pruneLogsIfNecessary(); + + return new File(ROAD_RUNNER_FOLDER, name); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/LynxModuleUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/LynxModuleUtil.java new file mode 100644 index 0000000..1c7366c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/LynxModuleUtil.java @@ -0,0 +1,124 @@ +package org.firstinspires.ftc.teamcode.roadrunner.util; + +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; + +import java.util.HashMap; +import java.util.Map; + +/** + * Collection of utilites for interacting with Lynx modules. + */ +public class LynxModuleUtil { + + private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2); + + /** + * Parsed representation of a Lynx module firmware version. + */ + public static class LynxFirmwareVersion implements Comparable { + public final int major; + public final int minor; + public final int eng; + + public LynxFirmwareVersion(int major, int minor, int eng) { + this.major = major; + this.minor = minor; + this.eng = eng; + } + + @Override + public boolean equals(Object other) { + if (other instanceof LynxFirmwareVersion) { + LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other; + return major == otherVersion.major && minor == otherVersion.minor && + eng == otherVersion.eng; + } else { + return false; + } + } + + @Override + public int compareTo(LynxFirmwareVersion other) { + int majorComp = Integer.compare(major, other.major); + if (majorComp == 0) { + int minorComp = Integer.compare(minor, other.minor); + if (minorComp == 0) { + return Integer.compare(eng, other.eng); + } else { + return minorComp; + } + } else { + return majorComp; + } + } + + @Override + public String toString() { + return Misc.formatInvariant("%d.%d.%d", major, minor, eng); + } + } + + /** + * Retrieve and parse Lynx module firmware version. + * @param module Lynx module + * @return parsed firmware version + */ + public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) { + String versionString = module.getNullableFirmwareVersionString(); + if (versionString == null) { + return null; + } + + String[] parts = versionString.split("[ :,]+"); + try { + // note: for now, we ignore the hardware entry + return new LynxFirmwareVersion( + Integer.parseInt(parts[3]), + Integer.parseInt(parts[5]), + Integer.parseInt(parts[7]) + ); + } catch (NumberFormatException e) { + return null; + } + } + + /** + * Exception indicating an outdated Lynx firmware version. + */ + public static class LynxFirmwareVersionException extends RuntimeException { + public LynxFirmwareVersionException(String detailMessage) { + super(detailMessage); + } + } + + /** + * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement. + * @param hardwareMap hardware map containing Lynx modules + */ + public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) { + Map outdatedModules = new HashMap<>(); + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + LynxFirmwareVersion version = getFirmwareVersion(module); + if (version == null || version.compareTo(MIN_VERSION) < 0) { + for (String name : hardwareMap.getNamesOf(module)) { + outdatedModules.put(name, version); + } + } + } + if (outdatedModules.size() > 0) { + StringBuilder msgBuilder = new StringBuilder(); + msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n"); + msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n", + MIN_VERSION.toString())); + for (Map.Entry entry : outdatedModules.entrySet()) { + msgBuilder.append(Misc.formatInvariant( + "\t%s: %s\n", entry.getKey(), + entry.getValue() == null ? "Unknown" : entry.getValue().toString())); + } + throw new LynxFirmwareVersionException(msgBuilder.toString()); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CameraPosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CameraPosition.java new file mode 100644 index 0000000..0e65173 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CameraPosition.java @@ -0,0 +1,5 @@ +package org.firstinspires.ftc.teamcode.util; + +public enum CameraPosition { + LEFT, CENTER, RIGHT +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java index 50b418e..b1b0645 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java @@ -41,6 +41,8 @@ public class Configurables { public static Color CAMERA_RED_POWERSHOT_UPPER = new Color(15, 255, 255); public static Color CAMERA_BLUE_GOAL_LOWER = new Color(75, 40, 80); public static Color CAMERA_BLUE_GOAL_UPPER = new Color(120, 255, 255); + public static Color CAMERA_BLACK_GOAL_LOWER = new Color(0, 0, 0); + public static Color CAMERA_BLACK_GOAL_UPPER = new Color(360, 0, 0); public static Color CAMERA_BLUE_POWERSHOT_LOWER = new Color(75, 30, 50); public static Color CAMERA_BLUE_POWERSHOT_UPPER = new Color(120, 255, 255); public static Color CAMERA_ORANGE_LOWER = new Color(0, 70, 50); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagDetectionPipeline.java new file mode 100644 index 0000000..b11420a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagDetectionPipeline.java @@ -0,0 +1,194 @@ +package org.firstinspires.ftc.teamcode.vision; + +import static org.firstinspires.ftc.teamcode.vision.OpenCVUtil.LEFT_BOUNDARY_APRILTAG; +import static org.firstinspires.ftc.teamcode.vision.OpenCVUtil.RIGHT_BOUNDARY_APRILTAG; + +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point; +import org.opencv.core.Point3; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.apriltag.AprilTagDetection; +import org.openftc.apriltag.AprilTagDetectorJNI; +import org.openftc.easyopencv.OpenCvPipeline; + +import java.util.ArrayList; + +public class AprilTagDetectionPipeline extends OpenCvPipeline { + + double fx; + double fy; + double cx; + double cy; + double tagsize; + double tagsizeX; + double tagsizeY; + Mat cameraMatrix; + private float decimation; + private Mat grey = new Mat(); + Scalar blue = new Scalar(7,197,235,255); + Scalar red = new Scalar(255,0,0,255); + Scalar green = new Scalar(0,255,0,255); + Scalar white = new Scalar(255,255,255,255); + private long nativeApriltagPtr; + private boolean needToSetDecimation; + private final Object decimationSync = new Object(); + private final Object detectionsUpdateSync = new Object(); + private ArrayList detectionsUpdate = new ArrayList<>(); + private ArrayList aprilTagDetections = new ArrayList<>(); + + @Override + public Mat processFrame(Mat input) + { + // Convert to greyscale + Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY); + + synchronized (decimationSync) + { + if(needToSetDecimation) + { + AprilTagDetectorJNI.setApriltagDetectorDecimation(nativeApriltagPtr, decimation); + needToSetDecimation = false; + } + } + + // Run AprilTag + aprilTagDetections = AprilTagDetectorJNI.runAprilTagDetectorSimple(nativeApriltagPtr, grey, tagsize, fx, fy, cx, cy); + + synchronized (detectionsUpdateSync) + { + detectionsUpdate = aprilTagDetections; + } + + for(AprilTagDetection detection : aprilTagDetections) + { + Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY); + drawAxisMarker(input, tagsizeY/2.0, 6, pose.rvec, pose.tvec, cameraMatrix); + draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix); + } + Imgproc.line(input, new Point(LEFT_BOUNDARY_APRILTAG,0), new Point(LEFT_BOUNDARY_APRILTAG,800), green, 2); + Imgproc.line(input, new Point(RIGHT_BOUNDARY_APRILTAG,0), new Point(RIGHT_BOUNDARY_APRILTAG,800), green, 2); + + return input; + } + + Pose poseFromTrapezoid(Point[] points, Mat cameraMatrix, double tagsizeX , double tagsizeY) + { + // The actual 2d points of the tag detected in the image + MatOfPoint2f points2d = new MatOfPoint2f(points); + + // The 3d points of the tag in an 'ideal projection' + Point3[] arrayPoints3d = new Point3[4]; + arrayPoints3d[0] = new Point3(-tagsizeX/2, tagsizeY/2, 0); + arrayPoints3d[1] = new Point3(tagsizeX/2, tagsizeY/2, 0); + arrayPoints3d[2] = new Point3(tagsizeX/2, -tagsizeY/2, 0); + arrayPoints3d[3] = new Point3(-tagsizeX/2, -tagsizeY/2, 0); + MatOfPoint3f points3d = new MatOfPoint3f(arrayPoints3d); + + // Using this information, actually solve for pose + Pose pose = new Pose(); + Calib3d.solvePnP(points3d, points2d, cameraMatrix, new MatOfDouble(), pose.rvec, pose.tvec, false); + + return pose; + } + + void drawAxisMarker(Mat buf, double length, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix) + { + // The points in 3D space we wish to project onto the 2D image plane. + // The origin of the coordinate space is assumed to be in the center of the detection. + MatOfPoint3f axis = new MatOfPoint3f( + new Point3(0,0,0), + new Point3(length,0,0), + new Point3(0,length,0), + new Point3(0,0,-length) + ); + + // Project those points + MatOfPoint2f matProjectedPoints = new MatOfPoint2f(); + Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints); + Point[] projectedPoints = matProjectedPoints.toArray(); + + // Draw the marker! + Imgproc.line(buf, projectedPoints[0], projectedPoints[1], red, thickness); + Imgproc.line(buf, projectedPoints[0], projectedPoints[2], green, thickness); + Imgproc.line(buf, projectedPoints[0], projectedPoints[3], blue, thickness); + + Imgproc.circle(buf, projectedPoints[0], thickness, white, -1); + } + + void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix) + { + //axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0], + // [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ]) + + // The points in 3D space we wish to project onto the 2D image plane. + // The origin of the coordinate space is assumed to be in the center of the detection. + MatOfPoint3f axis = new MatOfPoint3f( + new Point3(-tagWidth/2, tagHeight/2,0), + new Point3( tagWidth/2, tagHeight/2,0), + new Point3( tagWidth/2,-tagHeight/2,0), + new Point3(-tagWidth/2,-tagHeight/2,0), + new Point3(-tagWidth/2, tagHeight/2,-length), + new Point3( tagWidth/2, tagHeight/2,-length), + new Point3( tagWidth/2,-tagHeight/2,-length), + new Point3(-tagWidth/2,-tagHeight/2,-length)); + + // Project those points + MatOfPoint2f matProjectedPoints = new MatOfPoint2f(); + Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints); + Point[] projectedPoints = matProjectedPoints.toArray(); + + // Pillars + for(int i = 0; i < 4; i++) + { + Imgproc.line(buf, projectedPoints[i], projectedPoints[i+4], blue, thickness); + } + + // Base lines + //Imgproc.line(buf, projectedPoints[0], projectedPoints[1], blue, thickness); + //Imgproc.line(buf, projectedPoints[1], projectedPoints[2], blue, thickness); + //Imgproc.line(buf, projectedPoints[2], projectedPoints[3], blue, thickness); + //Imgproc.line(buf, projectedPoints[3], projectedPoints[0], blue, thickness); + + // Top lines + Imgproc.line(buf, projectedPoints[4], projectedPoints[5], green, thickness); + Imgproc.line(buf, projectedPoints[5], projectedPoints[6], green, thickness); + Imgproc.line(buf, projectedPoints[6], projectedPoints[7], green, thickness); + Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness); + } + + public ArrayList getLatestDetections() + { + return aprilTagDetections; + } + + public void setDecimation(float decimation) + { + synchronized (decimationSync) + { + this.decimation = decimation; + needToSetDecimation = true; + } + } + + class Pose + { + Mat rvec; + Mat tvec; + + public Pose(){ + rvec = new Mat(); + tvec = new Mat(); + } + + public Pose(Mat rvec, Mat tvec) + { + this.rvec = rvec; + this.tvec = tvec; + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/OpenCVUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/OpenCVUtil.java new file mode 100644 index 0000000..b0580de --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/OpenCVUtil.java @@ -0,0 +1,109 @@ +package org.firstinspires.ftc.teamcode.vision; + +import org.firstinspires.ftc.teamcode.util.Color; +import org.opencv.core.Mat; +import org.opencv.core.MatOfInt; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.RotatedRect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.opencv.imgproc.Moments; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +public class OpenCVUtil { + public static Color YELLOW_LOWER = new Color(38, 25, 0); + public static Color YELLOW_UPPER = new Color(105, 300, 300); + + public static int LEFT_BOUNDARY = -25; + public static int RIGHT_BOUNDARY = 25; + + public static int LEFT_BOUNDARY_APRILTAG = 0; + public static int RIGHT_BOUNDARY_APRILTAG = 800; + + // Draw a point + // Draw a point + public static void drawPoint(Mat img, Point point, Scalar color, int radiusPx) { + Imgproc.circle(img, point, radiusPx, color, -1); + } + + // Get the center of a contour + public static Point getCenterOfContour(MatOfPoint contour) { + Moments moments = Imgproc.moments(contour); + return new Point(moments.m10 / moments.m00, moments.m01/ moments.m00); + } + + // Get the bottom left of a contour + public static Point getBottomLeftOfContour(MatOfPoint contour) { + Rect boundingRect = Imgproc.boundingRect(contour); + return new Point(boundingRect.x, boundingRect.y+boundingRect.height); + } + + // Get the bottom right of a contour + public static Point getBottomRightOfContour(MatOfPoint contour) { + Rect boundingRect = Imgproc.boundingRect(contour); + return new Point(boundingRect.x+boundingRect.width, boundingRect.y+boundingRect.height); + } + + // Draw a contour + public static void drawContour(Mat img, MatOfPoint contour, Scalar color) { + Imgproc.drawContours(img, Collections.singletonList(contour), 0, color, 2); + } + + // Draw a convex hull around a contour + public static void drawConvexHull(Mat img, MatOfPoint contour, Scalar color) { + MatOfInt hull = new MatOfInt(); + Imgproc.convexHull(contour, hull); + Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, 2); + } + + // Draw a filled in convex hull around a contour + public static void fillConvexHull(Mat img, MatOfPoint contour, Scalar color) { + MatOfInt hull = new MatOfInt(); + Imgproc.convexHull(contour, hull); + Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, -1); + } + + // Convert indexes to points that is used in order to draw the contours + public static MatOfPoint convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) { + int[] arrIndex = indexes.toArray(); + Point[] arrContour = contour.toArray(); + Point[] arrPoints = new Point[arrIndex.length]; + + for (int i=0;i contours) { + if (contours.size() == 0) { + return null; + } + return getLargestContours(contours, 1).get(0); + } + + // Get the top largest contours + public static List getLargestContours(List contours, int numContours) { + Collections.sort(contours, (a, b) -> (int) Imgproc.contourArea(b) - (int) Imgproc.contourArea(a)); + return contours.subList(0, Math.min(numContours, contours.size())); + } + + public static void drawAngledRect(Mat img, MatOfPoint contour, Scalar color, boolean fill) { + RotatedRect rect = Imgproc.minAreaRect(new MatOfPoint2f(contour.toArray())); + Point[] vertices = new Point[4]; + rect.points(vertices); + List boxContours = new ArrayList<>(); + boxContours.add(new MatOfPoint(vertices)); + Imgproc.drawContours(img, boxContours, 0, color, fill ? -1 : 2); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java index 16b9a38..cd773cb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java @@ -1,16 +1,22 @@ package org.firstinspires.ftc.teamcode.vision; +import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLACK_GOAL_LOWER; +import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLUE_GOAL_LOWER; +import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLUE_GOAL_UPPER; import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_RED_GOAL_LOWER; import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_RED_GOAL_UPPER; import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MAX_GOAL_AREA; import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MIN_GOAL_AREA; import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR; +import static org.firstinspires.ftc.teamcode.util.Constants.BLACK; +import static org.firstinspires.ftc.teamcode.util.Constants.BLUE; import static org.firstinspires.ftc.teamcode.util.Constants.BLUR_SIZE; import static org.firstinspires.ftc.teamcode.util.Constants.ERODE_DILATE_ITERATIONS; import static org.firstinspires.ftc.teamcode.util.Constants.RED; import static org.firstinspires.ftc.teamcode.util.Constants.STRUCTURING_ELEMENT; import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour; +import org.firstinspires.ftc.teamcode.util.Color; import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint; @@ -26,18 +32,31 @@ public class TargetingPipeline extends OpenCvPipeline { Mat redMask1 = new Mat(); Mat redMask2 = new Mat(); Mat redMask = new Mat(); - Mat whiteMask = new Mat(); + Mat blueMask = new Mat(); + Mat blackMask = new Mat(); Scalar redGoalLower1; Scalar redGoalUpper1; Scalar redGoalLower2; Scalar redGoalUpper2; + Scalar blueGoalLower1; + Scalar blueGoalUpper1; + Scalar blueGoalLower2; + Scalar blueGoalUpper2; + Scalar blackGoalLower1; + Scalar blackGoalUpper1; + Scalar blackGoalLower2; + Scalar blackGoalUpper2; private Detection red; + private Detection blue; + private Detection black; // Init @Override public void init(Mat input) { red = new Detection(input.size(), CV_MIN_GOAL_AREA); + blue = new Detection(input.size(), CV_MIN_GOAL_AREA); + black = new Detection(input.size(), CV_MIN_GOAL_AREA); } // Process each frame that is received from the webcam @@ -47,7 +66,8 @@ public class TargetingPipeline extends OpenCvPipeline { Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV); updateRed(input); - + updateBlue(input); + updateBlack(input); return input; } @@ -76,7 +96,59 @@ public class TargetingPipeline extends OpenCvPipeline { red.fill(input, RED); } + private void updateBlue(Mat input) { + // take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise + blueGoalLower1 = new Scalar(CAMERA_BLUE_GOAL_LOWER.getH(), CAMERA_BLUE_GOAL_LOWER.getS(), CAMERA_BLUE_GOAL_LOWER.getV()); + blueGoalUpper1 = new Scalar(180, CAMERA_BLUE_GOAL_UPPER.getS(), CAMERA_BLUE_GOAL_UPPER.getV()); + blueGoalLower2 = new Scalar(0, CAMERA_BLUE_GOAL_LOWER.getS(), CAMERA_BLUE_GOAL_LOWER.getV()); + blueGoalUpper2 = new Scalar(CAMERA_BLUE_GOAL_UPPER.getH(), CAMERA_BLUE_GOAL_UPPER.getS(), CAMERA_BLUE_GOAL_UPPER.getV()); + Core.inRange(hsv, blueGoalLower1, blueGoalUpper1, blueMask); + Core.inRange(hsv, blueGoalLower2, blueGoalUpper2, blueMask); + Imgproc.erode(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); + Imgproc.dilate(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); + + ArrayList contours = new ArrayList<>(); + Imgproc.findContours(blueMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE); + for (int i = 0; i < contours.size(); i++) { + Detection newDetection = new Detection(input.size(), CV_MIN_GOAL_AREA, CV_MAX_GOAL_AREA); + newDetection.setContour(contours.get(i)); + newDetection.draw(input, BLUE); + } + + blue.setContour(getLargestContour(contours)); + blue.fill(input, BLUE); + } + + private void updateBlack(Mat input) { + // take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise + blackGoalLower1 = new Scalar(CAMERA_BLACK_GOAL_LOWER.getH(), CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV()); + blackGoalUpper1 = new Scalar(180, CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV()); + blackGoalLower2 = new Scalar(0, CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV()); + blackGoalUpper2 = new Scalar(CAMERA_BLACK_GOAL_LOWER.getH(), CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV()); + Core.inRange(hsv, blackGoalLower1, blackGoalUpper1, blackMask); + Core.inRange(hsv, blackGoalLower2, blackGoalUpper2, blackMask); + Imgproc.erode(blackMask, blackMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); + Imgproc.dilate(blackMask, blackMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); + + ArrayList contours = new ArrayList<>(); + Imgproc.findContours(blackMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE); + for (int i = 0; i < contours.size(); i++) { + Detection newDetection = new Detection(input.size(), CV_MIN_GOAL_AREA, CV_MAX_GOAL_AREA); + newDetection.setContour(contours.get(i)); + newDetection.draw(input, BLACK); + } + + black.setContour(getLargestContour(contours)); + black.fill(input, BLACK); + } + public Detection getRed() { return this.red; } + public Detection getBlue() { + return this.blue; + } + public Detection getBlack() { + return this.black; + } } \ No newline at end of file diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 9775838..e73a9ca 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -1,6 +1,7 @@ repositories { mavenCentral() google() // Needed for androidx + maven { url = 'https://maven.brott.dev/' } } dependencies { @@ -17,5 +18,6 @@ dependencies { implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3' runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0' implementation 'androidx.appcompat:appcompat:1.2.0' + implementation 'com.acmerobotics.dashboard:dashboard:0.4.12' }