Add drive base, add intake, and other small details.
This commit is contained in:
UntitledRice
parent
49d4939366
commit
edd589fb79
|
|
@ -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'
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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() {
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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.
|
||||
// * <p>
|
||||
// * The buttons, analog sticks, and triggers are represented a public
|
||||
// * member variables that can be read from or written to directly.
|
||||
// * <p>
|
||||
// * 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.
|
||||
// * <p>
|
||||
// * Triggers are represented as floats that range from 0.0 to 1.0. They will be at 0.0 while at
|
||||
// * rest.
|
||||
// * <p>
|
||||
// * Buttons are boolean values. They will be true if the button is pressed, otherwise they will be
|
||||
// * false.
|
||||
// * <p>
|
||||
// * 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.
|
||||
// * <p>
|
||||
// * 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<LedEffect> ledQueue = new EvictingBlockingQueue<>(new ArrayBlockingQueue<LedEffect>(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<Step> steps;
|
||||
// public final boolean repeating;
|
||||
// public int user;
|
||||
//
|
||||
// private LedEffect(ArrayList<Step> 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<Step> 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<RumbleEffect> rumbleQueue = new EvictingBlockingQueue<>(new ArrayBlockingQueue<RumbleEffect>(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<Step> steps;
|
||||
//
|
||||
// private RumbleEffect(ArrayList<Step> 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<Step> 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;
|
||||
// }
|
||||
//}
|
||||
|
|
@ -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 && x>-DEADZONE) {
|
||||
return 0;
|
||||
}
|
||||
else{
|
||||
return x;
|
||||
}
|
||||
}
|
||||
|
||||
public double getY() {
|
||||
if(y<DEADZONE && y>-DEADZONE) {
|
||||
return 0;
|
||||
}
|
||||
else{
|
||||
return y;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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<AprilTagDetection> 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
// }
|
||||
//}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
// )
|
||||
// );
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
package org.firstinspires.ftc.teamcode.roadrunner.drive;//package org.firstinspires.ftc.teamcode.roadrunner.drive;
|
||||
//import java.awt.
|
||||
//
|
||||
//public class MouseOdometry {
|
||||
//}
|
||||
|
|
@ -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<DcMotorEx> 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<Double> getWheelPositions() {
|
||||
List<Double> wheelPositions = new ArrayList<>();
|
||||
for (DcMotorEx motor : motors) {
|
||||
wheelPositions.add(DriveConstants.encoderTicksToInches(motor.getCurrentPosition()));
|
||||
}
|
||||
return wheelPositions;
|
||||
}
|
||||
|
||||
@Override
|
||||
public List<Double> getWheelVelocities() {
|
||||
List<Double> 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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<DcMotorEx> 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<Double> 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<Double> 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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<Double> getWheelPositions() {
|
||||
return Arrays.asList(
|
||||
encoderTicksToInches(leftEncoder.getCurrentPosition()),
|
||||
encoderTicksToInches(rightEncoder.getCurrentPosition()),
|
||||
encoderTicksToInches(frontEncoder.getCurrentPosition())
|
||||
);
|
||||
}
|
||||
|
||||
@NonNull
|
||||
@Override
|
||||
public List<Double> 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())
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<Double> getWheelPositions() {
|
||||
return Arrays.asList(
|
||||
encoderTicksToInches(parallelEncoder.getCurrentPosition()),
|
||||
encoderTicksToInches(perpendicularEncoder.getCurrentPosition())
|
||||
);
|
||||
}
|
||||
|
||||
@NonNull
|
||||
@Override
|
||||
public List<Double> 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())
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<Double> timeSamples = new ArrayList<>();
|
||||
List<Double> positionSamples = new ArrayList<>();
|
||||
List<Double> 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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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<Double> 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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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.
|
||||
* <p>
|
||||
* Upon pressing start, your bot will turn at max power for RUNTIME seconds.
|
||||
* <p>
|
||||
* 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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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.
|
||||
* <p>
|
||||
* Upon pressing start, your bot will run at max power for RUNTIME seconds.
|
||||
* <p>
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> X / ▢ - Front Left</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> Y / Δ - Front Right</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> B / O - Rear Right</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> A / X - Rear Left</font>");
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
@ -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()) ;
|
||||
}
|
||||
}
|
||||
|
|
@ -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()) ;
|
||||
}
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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));
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence;
|
||||
|
||||
|
||||
public class EmptySequenceException extends RuntimeException { }
|
||||
|
|
@ -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<SequenceSegment> sequenceList;
|
||||
|
||||
public TrajectorySequence(List<SequenceSegment> 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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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<SequenceSegment> sequenceSegments;
|
||||
|
||||
private final List<TemporalMarker> temporalMarkers;
|
||||
private final List<DisplacementMarker> displacementMarkers;
|
||||
private final List<SpatialMarker> 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<TrajectoryMarker> globalMarkers = convertMarkersToGlobal(
|
||||
sequenceSegments,
|
||||
temporalMarkers, displacementMarkers, spatialMarkers
|
||||
);
|
||||
|
||||
return new TrajectorySequence(projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments));
|
||||
}
|
||||
|
||||
private List<TrajectoryMarker> convertMarkersToGlobal(
|
||||
List<SequenceSegment> sequenceSegments,
|
||||
List<TemporalMarker> temporalMarkers,
|
||||
List<DisplacementMarker> displacementMarkers,
|
||||
List<SpatialMarker> spatialMarkers
|
||||
) {
|
||||
ArrayList<TrajectoryMarker> 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<SequenceSegment> projectGlobalMarkersToLocalSegments(List<TrajectoryMarker> markers, List<SequenceSegment> 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<TrajectoryMarker> 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<TrajectoryMarker> 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<TrajectoryMarker> 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<SequenceSegment> 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<SequenceSegment> 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<ComparingPoints> 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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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<TrajectoryMarker> remainingMarkers = new ArrayList<>();
|
||||
|
||||
private final FtcDashboard dashboard;
|
||||
private final LinkedList<Pose2d> 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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<TrajectoryMarker> markers;
|
||||
|
||||
protected SequenceSegment(
|
||||
double duration,
|
||||
Pose2d startPose, Pose2d endPose,
|
||||
List<TrajectoryMarker> 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<TrajectoryMarker> getMarkers() {
|
||||
return markers;
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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<TrajectoryMarker> 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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<TrajectoryMarker> markers) {
|
||||
super(seconds, pose, pose, markers);
|
||||
}
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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 <a href="https://ftcforum.firstinspires.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587">this post</a>.
|
||||
* Reference the <a href="https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf">BNO055 Datasheet</a> 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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<Pose2d> 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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<File> logFiles, File dir) {
|
||||
for (File file : dir.listFiles()) {
|
||||
if (file.isDirectory()) {
|
||||
buildLogList(logFiles, file);
|
||||
} else {
|
||||
logFiles.add(file);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void pruneLogsIfNecessary() {
|
||||
List<File> 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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<LynxFirmwareVersion> {
|
||||
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<String, LynxFirmwareVersion> 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<String, LynxFirmwareVersion> 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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
package org.firstinspires.ftc.teamcode.util;
|
||||
|
||||
public enum CameraPosition {
|
||||
LEFT, CENTER, RIGHT
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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<org.openftc.apriltag.AprilTagDetection> detectionsUpdate = new ArrayList<>();
|
||||
private ArrayList<org.openftc.apriltag.AprilTagDetection> 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<AprilTagDetection> 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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<arrIndex.length;i++) {
|
||||
arrPoints[i] = arrContour[arrIndex[i]];
|
||||
}
|
||||
|
||||
MatOfPoint hull = new MatOfPoint();
|
||||
hull.fromArray(arrPoints);
|
||||
return hull;
|
||||
}
|
||||
|
||||
// Get the largest contour out of a list
|
||||
public static MatOfPoint getLargestContour(List<MatOfPoint> contours) {
|
||||
if (contours.size() == 0) {
|
||||
return null;
|
||||
}
|
||||
return getLargestContours(contours, 1).get(0);
|
||||
}
|
||||
|
||||
// Get the top largest contours
|
||||
public static List<MatOfPoint> getLargestContours(List<MatOfPoint> 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<MatOfPoint> boxContours = new ArrayList<>();
|
||||
boxContours.add(new MatOfPoint(vertices));
|
||||
Imgproc.drawContours(img, boxContours, 0, color, fill ? -1 : 2);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<MatOfPoint> 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<MatOfPoint> 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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'
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue