upgrade pedro
This commit is contained in:
@@ -24,11 +24,6 @@ android {
|
||||
}
|
||||
}
|
||||
|
||||
repositories {
|
||||
maven {
|
||||
url "https://repo.dairy.foundation/releases"
|
||||
}
|
||||
}
|
||||
|
||||
dependencies {
|
||||
implementation project(':FtcRobotController')
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
package org.firstinspires.ftc.teamcode.pedro;
|
||||
|
||||
import com.pedropathing.follower.Follower;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
public class Constants {
|
||||
public static Follower create(HardwareMap h) {
|
||||
// return new Follower(Drivetrain, Localizer, Foresight);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,5 @@
|
||||
package org.firstinspires.ftc.teamcode.pedro;
|
||||
|
||||
public class Tuning {
|
||||
// Tuners go here
|
||||
}
|
||||
+1222
File diff suppressed because it is too large
Load Diff
+96
@@ -0,0 +1,96 @@
|
||||
package org.firstinspires.ftc.teamcode.pedro.procedures;
|
||||
|
||||
import com.pedropathing.tuning.autotune.*;
|
||||
import com.pedropathing.tuning.autotune.Display.FourWheelBot.Wheel;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
enum Direction {
|
||||
@DisplayName("Forward") FORWARD,
|
||||
@DisplayName("Reversed") REVERSE
|
||||
}
|
||||
|
||||
public class MecanumTuner extends Procedure {
|
||||
public MecanumTuner() {
|
||||
super("Mecanum Tuner", "A procedure to find the directions of mecanum wheels.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() throws InterruptedException {
|
||||
Inputs motorNames = inputs("Mecanum Motor Names", "Enter the names in HardwareMap of your drivetrain motors.");
|
||||
Inputs.Field<String> frontLeftName = motorNames.s("Front Left Name");
|
||||
Inputs.Field<String> frontRightName = motorNames.s("Front Right Name");
|
||||
Inputs.Field<String> backLeftName = motorNames.s("Back Left Name");
|
||||
Inputs.Field<String> backRightName = motorNames.s("Back Right Name");
|
||||
awaitInputs(motorNames);
|
||||
|
||||
confirmation("Motor Directions", "Each drivetrain motor will spin, one at a time. After each one, you will enter whether it spun forward or reversed. You may use the interactive diagram to see which wheel should be spinning and which direction is forward.");
|
||||
|
||||
Direction frontLeftDirection = testMotor(Wheel.FRONT_LEFT, "Front Left", frontLeftName.get());
|
||||
Direction frontRightDirection = testMotor(Wheel.FRONT_RIGHT, "Front Right", frontRightName.get());
|
||||
Direction backLeftDirection = testMotor(Wheel.BACK_LEFT, "Back Left", backLeftName.get());
|
||||
Direction backRightDirection = testMotor(Wheel.BACK_RIGHT, "Back Right", backRightName.get());
|
||||
|
||||
result("frontLeftName", frontLeftName.get());
|
||||
result("frontRightName", frontRightName.get());
|
||||
result("backLeftName", backLeftName.get());
|
||||
result("backRightName", backRightName.get());
|
||||
result("frontLeftDirection", frontLeftDirection);
|
||||
result("frontRightDirection", frontRightDirection);
|
||||
result("backLeftDirection", backLeftDirection);
|
||||
result("backRightDirection", backRightDirection);
|
||||
|
||||
code(Language.JAVA, "public static MecanumConfig drivetrainConfig = new MecanumConfig(c -> {\n" +
|
||||
" c.frontLeftName.set(\"" + frontLeftName.get() + "\");\n" +
|
||||
" c.frontRightName.set(\"" + frontRightName.get() + "\");\n" +
|
||||
" c.backLeftName.set(\"" + backLeftName.get() + "\");\n" +
|
||||
" c.backRightName.set(\"" + backRightName.get() + "\");\n" +
|
||||
" c.frontLeftDirection.set(DcMotorSimple.Direction." + frontLeftDirection + ");\n" +
|
||||
" c.frontRightDirection.set(DcMotorSimple.Direction." + frontRightDirection + ");\n" +
|
||||
" c.backLeftDirection.set(DcMotorSimple.Direction." + backLeftDirection + ");\n" +
|
||||
" c.backRightDirection.set(DcMotorSimple.Direction." + backRightDirection + ");\n" +
|
||||
"});");
|
||||
}
|
||||
|
||||
private Direction testMotor(Wheel wheel, String displayName, String hardwareName) throws InterruptedException {
|
||||
final boolean[] correctMotor = new boolean[1];
|
||||
final Direction[] direction = new Direction[1];
|
||||
|
||||
withDisplay(Display.fourWheelBot(wheel, false), () -> {
|
||||
runOpMode(new SpinMotor(displayName, hardwareName));
|
||||
|
||||
Inputs inputs = inputs(displayName, "Determine the " + displayName.toLowerCase() + " motor direction.");
|
||||
Inputs.Field<Boolean> correctMotorField = inputs.b("Did the " + displayName.toLowerCase() + " motor spin?").withDefault(true);
|
||||
Inputs.Field<Direction> directionField = inputs.e("Which way did the motor spin?", Direction.class);
|
||||
awaitInputs(inputs);
|
||||
|
||||
correctMotor[0] = correctMotorField.get();
|
||||
direction[0] = directionField.get();
|
||||
});
|
||||
|
||||
if (!correctMotor[0])
|
||||
abort("The wrong motor spun. Check that your motors are plugged into the correct ports, and that they are configured correctly. Then, try again.");
|
||||
|
||||
return direction[0];
|
||||
}
|
||||
}
|
||||
|
||||
class SpinMotor extends TuningOpMode<Void> {
|
||||
private final String name;
|
||||
|
||||
public SpinMotor(String displayName, String hardwareName) {
|
||||
super(displayName, "The " + displayName.toLowerCase() + " motor will spin. The interactive diagram shows which way is forward. Click stop when you know if it is spinning forward or reversed.", true);
|
||||
this.name = hardwareName;
|
||||
}
|
||||
|
||||
@SuppressWarnings("StatementWithEmptyBody")
|
||||
@Override
|
||||
protected Void runTuningOpMode() {
|
||||
DcMotor motor = hardwareMap.dcMotor.get(name);
|
||||
waitForStart();
|
||||
motor.setPower(0.5);
|
||||
while (opModeIsActive()) {
|
||||
}
|
||||
motor.setPower(0);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
+210
@@ -0,0 +1,210 @@
|
||||
package org.firstinspires.ftc.teamcode.pedro.procedures;
|
||||
|
||||
import com.pedropathing.math.Pose;
|
||||
import com.pedropathing.revhub.localizers.OTOSConfig;
|
||||
import com.pedropathing.revhub.localizers.OTOSLocalizer;
|
||||
import com.pedropathing.tuning.autotune.Inputs;
|
||||
import com.pedropathing.tuning.autotune.Procedure;
|
||||
import com.pedropathing.tuning.autotune.TuningOpMode;
|
||||
import com.pedropathing.utils.Angle;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
public class OTOSTuner extends Procedure {
|
||||
public OTOSTuner() {
|
||||
super("OTOS Tuner", "A procedure for tuning the OTOS localizer.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() throws InterruptedException {
|
||||
Inputs inputs = inputs("Setup", "Set OTOS HardwareMap Name");
|
||||
Inputs.Field<String> name = inputs.s("HardwareMap Name").withDefault("otos");
|
||||
awaitInputs(inputs);
|
||||
|
||||
Inputs scalar = inputs(
|
||||
"Scalar Identification",
|
||||
"Set the distance you will push your robot forward in inches and the number of full rotations for the angular test"
|
||||
);
|
||||
Inputs.Field<Double> distance = scalar.d("Distance to push robot").withDefault(48.0);
|
||||
Inputs.Field<Integer> turns = scalar.i("Full rotations").withDefault(10);
|
||||
awaitInputs(scalar);
|
||||
|
||||
if (!(distance.get() > 0.0)) {
|
||||
abort("Enter a positive push distance in inches.");
|
||||
return;
|
||||
}
|
||||
if (turns.get() <= 0) {
|
||||
abort("Enter a positive number of full rotations.");
|
||||
return;
|
||||
}
|
||||
|
||||
Double angularScalar = runOpMode(new OTOSAngularScalar(name.get(), turns.get()));
|
||||
Double linearScalar = runOpMode(new OTOSLinearScalar(name.get(), distance.get()));
|
||||
|
||||
List<Double> offsets = runOpMode(new OTOSOffsets(name.get(), linearScalar, angularScalar));
|
||||
if (offsets == null) {
|
||||
abort("Offset stage ended without a saved pose. Rotate the robot 180 degrees about the robot center, then press Stop.");
|
||||
return;
|
||||
}
|
||||
|
||||
result("name", name.get());
|
||||
result("linearScalar", linearScalar);
|
||||
result("angularScalar", angularScalar);
|
||||
result("xOffset", offsets.get(0));
|
||||
result("yOffset", offsets.get(1));
|
||||
|
||||
code(Language.JAVA,"public static OTOSConfig localizerConfig = new OTOSConfig(c -> {\n" +
|
||||
" c.name.set(\"" + name.get() + "\");\n" +
|
||||
" c.linearScalar.set(" + linearScalar + ");\n" +
|
||||
" c.angularScalar.set(" + angularScalar + ");\n" +
|
||||
" c.offset.set(new Pose(" + offsets.get(0) + ", " + offsets.get(1) + "));\n" +
|
||||
" c.linearUnit.set(DistanceUnit.INCH);\n" +
|
||||
"});");
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class OTOSLinearScalar extends TuningOpMode<Double> {
|
||||
String name;
|
||||
double distance;
|
||||
|
||||
public OTOSLinearScalar(String name, double distance) {
|
||||
super("Linear Scalar Identification",
|
||||
"Determines the linear scalar for the OTOS localizer. \n"
|
||||
+ "Push your robot forward " + distance + " inches, stop moving, then press Stop",
|
||||
true);
|
||||
this.name = name;
|
||||
this.distance = distance;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Double runTuningOpMode() {
|
||||
OTOSConfig config = new OTOSConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.linearUnit.set(DistanceUnit.INCH);
|
||||
c.linearScalar.set(1.0);
|
||||
c.angularScalar.set(1.0);
|
||||
c.offset.set(Pose.zero());
|
||||
});
|
||||
OTOSLocalizer localizer = new OTOSLocalizer(hardwareMap, config);
|
||||
localizer.setPose(Pose.zero());
|
||||
localizer.update();
|
||||
|
||||
Pose position = null;
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
position = localizer.pose();
|
||||
|
||||
|
||||
}
|
||||
|
||||
if (position == null || Math.abs(position.x()) <= 1e-9) {
|
||||
return null;
|
||||
}
|
||||
|
||||
return Math.abs(distance / position.x());
|
||||
}
|
||||
}
|
||||
|
||||
class OTOSAngularScalar extends TuningOpMode<Double> {
|
||||
String name;
|
||||
int turns;
|
||||
double targetRadians;
|
||||
|
||||
public OTOSAngularScalar(String name, int turns) {
|
||||
super("Angular Scalar Identification",
|
||||
"Determines the angular scalar for the OTOS localizer. \n"
|
||||
+ "Spin your robot " + turns + " full rotations, stop moving, then press Stop",
|
||||
true);
|
||||
this.name = name;
|
||||
this.turns = turns;
|
||||
this.targetRadians = turns * 2.0 * Math.PI;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Double runTuningOpMode() {
|
||||
OTOSConfig config = new OTOSConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.linearUnit.set(DistanceUnit.INCH);
|
||||
c.linearScalar.set(1.0);
|
||||
c.angularScalar.set(1.0);
|
||||
c.offset.set(Pose.zero());
|
||||
});
|
||||
OTOSLocalizer localizer = new OTOSLocalizer(hardwareMap, config);
|
||||
localizer.setPose(Pose.zero());
|
||||
localizer.update();
|
||||
|
||||
waitForStart();
|
||||
|
||||
localizer.update();
|
||||
double prevHeading = localizer.pose().heading();
|
||||
double totalHeading = 0.0;
|
||||
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
|
||||
Pose position = localizer.pose();
|
||||
double currentHeading = position.heading();
|
||||
totalHeading += Angle.normalizeSigned(currentHeading - prevHeading);
|
||||
prevHeading = currentHeading;
|
||||
|
||||
|
||||
}
|
||||
|
||||
if (Math.abs(totalHeading) <= 1e-9) {
|
||||
return null;
|
||||
}
|
||||
|
||||
return Math.abs(targetRadians / totalHeading);
|
||||
}
|
||||
}
|
||||
|
||||
class OTOSOffsets extends TuningOpMode<List<Double>> {
|
||||
String name;
|
||||
double linearScalar, angularScalar;
|
||||
|
||||
public OTOSOffsets(String name, double linearScalar, double angularScalar) {
|
||||
super("OTOS Offset Identification",
|
||||
"Automatically identifies the X/Y offset for your OTOS localizer. \n"
|
||||
+ "Rotate the robot 180 degrees counterclockwise about the robot center without translating it, stop moving, then press Stop",
|
||||
true);
|
||||
this.name = name;
|
||||
this.linearScalar = linearScalar;
|
||||
this.angularScalar = angularScalar;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected List<Double> runTuningOpMode() {
|
||||
OTOSConfig config = new OTOSConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.linearUnit.set(DistanceUnit.INCH);
|
||||
c.linearScalar.set(linearScalar);
|
||||
c.angularScalar.set(angularScalar);
|
||||
c.offset.set(Pose.zero());
|
||||
});
|
||||
OTOSLocalizer localizer = new OTOSLocalizer(hardwareMap, config);
|
||||
localizer.setPose(Pose.zero());
|
||||
localizer.update();
|
||||
|
||||
Pose position = null;
|
||||
|
||||
waitForStart();
|
||||
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
position = localizer.pose();
|
||||
telemetry.addData("heading", localizer.pose().heading());
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
if (position == null) {
|
||||
return null;
|
||||
}
|
||||
|
||||
return List.of(-position.x() / 2.0, -position.y() / 2.0);
|
||||
}
|
||||
}
|
||||
+350
@@ -0,0 +1,350 @@
|
||||
package org.firstinspires.ftc.teamcode.pedro.procedures;
|
||||
|
||||
import com.pedropathing.math.Pose;
|
||||
import com.pedropathing.revhub.localizers.OctoQuadConfig;
|
||||
import com.pedropathing.revhub.localizers.OctoQuadLocalizer;
|
||||
import com.pedropathing.tuning.autotune.Inputs;
|
||||
import com.pedropathing.tuning.autotune.Procedure;
|
||||
import com.pedropathing.tuning.autotune.TuningOpMode;
|
||||
import com.pedropathing.utils.Angle;
|
||||
import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
public class OctoQuadTuner extends Procedure {
|
||||
enum PodType {
|
||||
SWING_ARM,
|
||||
FOUR_BAR,
|
||||
CUSTOM
|
||||
}
|
||||
|
||||
public static double SWING_ARM = 336.877962768;
|
||||
public static double FOUR_BAR = 505.316944406;
|
||||
|
||||
public OctoQuadTuner() {
|
||||
super("OctoQuad Tuner", "A procedure for tuning the OctoQuad localizer.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() throws InterruptedException {
|
||||
Inputs inputs = inputs("Setup", "Set OctoQuad HardwareMap Name and Odometry Pod Type");
|
||||
Inputs.Field<String> octoquadName = inputs.s("HardwareMap Name").withDefault("octoquad");
|
||||
Inputs.Field<Integer> xPort = inputs.i("Forward Pod Port").withDefault(0);
|
||||
Inputs.Field<Integer> yPort = inputs.i("Strafe Pod Port").withDefault(1);
|
||||
Inputs.Field<OctoQuadTuner.PodType> podType = inputs.e("Odometry Pod Type", OctoQuadTuner.PodType.class).withDefault(OctoQuadTuner.PodType.FOUR_BAR);
|
||||
Inputs.Field<OctoQuad.I2cRecoveryMode> recoveryMode = inputs.e("Recovery Mode", OctoQuad.I2cRecoveryMode.class).withDefault(OctoQuad.I2cRecoveryMode.MODE_1_PERIPH_RST_ON_FRAME_ERR);
|
||||
awaitInputs(inputs);
|
||||
|
||||
double customPodScalar = 0;
|
||||
|
||||
Inputs inputsHeadingScalar = inputs("Custom Scalar Identification Turns", "Set the number of times you will turn your robot.");
|
||||
Inputs.Field<Integer> turns = inputsHeadingScalar.i("Turns").withDefault(10);
|
||||
awaitInputs(inputsHeadingScalar);
|
||||
double headingScalar = runOpMode(new OctoQuadHeadingScalar(octoquadName.get(), turns.get(), xPort.get(), yPort.get()));
|
||||
|
||||
if (podType.get() == PodType.CUSTOM) {
|
||||
Inputs inputsCustom = inputs("Custom Scalar Identification Push Distance", "Set the distance you will push your robot forward in inches");
|
||||
Inputs.Field<Double> distance = inputsCustom.d("Distance").withDefault(48.0);
|
||||
awaitInputs(inputsCustom);
|
||||
customPodScalar = runOpMode(new OctoQuadCustomPodScalar(distance.get(), octoquadName.get(), xPort.get(), yPort.get()));
|
||||
}
|
||||
|
||||
boolean forwardPodReversed = runOpMode(new OctoQuadForwardDirection(octoquadName.get(), podType.get(), customPodScalar, headingScalar, xPort.get(), yPort.get()));
|
||||
boolean strafePodReversed = runOpMode(new OctoQuadStrafeDirection(octoquadName.get(), podType.get(), customPodScalar, headingScalar, xPort.get(), yPort.get()));
|
||||
|
||||
List<Double> offsets = runOpMode(new OctoQuadOffsets(octoquadName.get(), podType.get(), customPodScalar, forwardPodReversed, strafePodReversed, headingScalar, xPort.get(), yPort.get()));
|
||||
|
||||
result("name", octoquadName.get());
|
||||
result("headingScalar", headingScalar);
|
||||
|
||||
if (podType.get() == PodType.CUSTOM) {
|
||||
result("podType", "Custom");
|
||||
result("ticksPerUnit", customPodScalar);
|
||||
} else {
|
||||
result("podType", podType.get() == PodType.SWING_ARM ? SWING_ARM : FOUR_BAR);
|
||||
}
|
||||
|
||||
result("xPodDirection", forwardPodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
|
||||
result("yPodDirection", strafePodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
|
||||
result("xPodOffset", offsets.get(0));
|
||||
result("yPodOffset", offsets.get(1));
|
||||
|
||||
code(Language.JAVA,"public static OctoQuadConfig localizerConfig = new OctoQuadConfig(c -> {\n" +
|
||||
" c.name.set(\"" + octoquadName.get() + "\");\n" +
|
||||
" c.xPodPort.set(" + xPort.get() + ");\n" +
|
||||
" c.yPodPort.set(" + yPort.get() + ");\n" +
|
||||
(podType.get() == PodType.CUSTOM ? " c.ticksPerUnit.set(" + customPodScalar + ");\n" : " c.ticksPerUnit.set(" + (podType.get() == OctoQuadTuner.PodType.SWING_ARM ? SWING_ARM : FOUR_BAR) + ");\n") +
|
||||
" c.xPodOffset.set(" + offsets.get(0) + ");\n" +
|
||||
" c.yPodOffset.set(" + offsets.get(1) + ");\n" +
|
||||
" c.xPodDirection.set(" + (forwardPodReversed ? "OctoQuad.EncoderDirection.REVERSE" : "OctoQuad.EncoderDirection.FORWARD") + ");\n" +
|
||||
" c.yPodDirection.set(" + (strafePodReversed ? "OctoQuad.EncoderDirection.REVERSE" : "OctoQuad.EncoderDirection.FORWARD") + ");\n" +
|
||||
" c.globalDistanceUnit.set(DistanceUnit.INCH);\n" +
|
||||
" c.offsetUnits.set(DistanceUnit.INCH);\n" +
|
||||
" c.i2cRecoveryMode.set(OctoQuad.I2cRecoveryMode." + recoveryMode.get() + ");\n" +
|
||||
" c.headingScalar.set(" + headingScalar + ");\n" +
|
||||
"});");
|
||||
}
|
||||
}
|
||||
|
||||
class OctoQuadHeadingScalar extends TuningOpMode<Double> {
|
||||
String name;
|
||||
int turns;
|
||||
double totalHeading = 0;
|
||||
double prevHeading = 0;
|
||||
int xPodPort;
|
||||
int yPodPort;
|
||||
|
||||
public OctoQuadHeadingScalar(String name, int turns, int xPodPort, int yPodPort) {
|
||||
super("Heading Scalar Identification",
|
||||
"Determines the scalar for the custom pods of the OctoQuad localizer. \n"
|
||||
+ "Turn your robot " + turns * 360 + " degrees exactly ("+ turns + " times) exactly and then stop the OpMode.",
|
||||
true);
|
||||
this.name = name;
|
||||
this.turns = turns;
|
||||
this.xPodPort = xPodPort;
|
||||
this.yPodPort = yPodPort;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Double runTuningOpMode() throws InterruptedException {
|
||||
OctoQuadConfig config = new OctoQuadConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.xPodPort.set(xPodPort);
|
||||
c.yPodPort.set(yPodPort);
|
||||
c.ticksPerUnit.set(1.0);
|
||||
c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
|
||||
c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
|
||||
c.xPodOffset.set(0.0);
|
||||
c.yPodOffset.set(0.0);
|
||||
});
|
||||
|
||||
OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
localizer.update();
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
|
||||
if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y() || localizer.pose().heading() != Pose.zero().heading()) {
|
||||
double currentHeading = localizer.pose().heading();
|
||||
totalHeading += Angle.normalizeSigned(currentHeading - prevHeading);
|
||||
prevHeading = currentHeading;
|
||||
}
|
||||
}
|
||||
return Math.abs((turns * Math.PI * 2 / totalHeading));
|
||||
}
|
||||
}
|
||||
|
||||
class OctoQuadCustomPodScalar extends TuningOpMode<Double> {
|
||||
String name;
|
||||
double distance;
|
||||
int xPodPort, yPodPort;
|
||||
|
||||
public OctoQuadCustomPodScalar(Double distance, String name, int xPodPort, int yPodPort) {
|
||||
super("Custom Scalar Identification",
|
||||
"Determines the scalar for the custom pods of the OctoQuad localizer. \n"
|
||||
+ "Push your robot forward " + distance + " inches exactly and then stop the Opmode",
|
||||
true);
|
||||
this.name = name;
|
||||
this.distance = distance;
|
||||
this.xPodPort = xPodPort;
|
||||
this.yPodPort = yPodPort;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Double runTuningOpMode() {
|
||||
OctoQuadConfig config = new OctoQuadConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.xPodPort.set(xPodPort);
|
||||
c.yPodPort.set(yPodPort);
|
||||
c.ticksPerUnit.set(1.0);
|
||||
c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
|
||||
c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
|
||||
c.xPodOffset.set(0.0);
|
||||
c.yPodOffset.set(0.0);
|
||||
});
|
||||
OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
waitForStart();
|
||||
|
||||
double startTicks = localizer.octoQuad.readAllEncoderData().positions[xPodPort];
|
||||
double lastLastTicksPerInch = 0;
|
||||
double lastTicksPerInch = 0;
|
||||
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
|
||||
double pos = localizer.octoQuad.readAllEncoderData().positions[xPodPort];
|
||||
lastLastTicksPerInch = lastTicksPerInch;
|
||||
lastTicksPerInch = Math.abs(pos - startTicks) / distance;
|
||||
}
|
||||
|
||||
return lastLastTicksPerInch;
|
||||
}
|
||||
}
|
||||
|
||||
class OctoQuadForwardDirection extends TuningOpMode<Boolean> {
|
||||
String name;
|
||||
OctoQuadTuner.PodType podType;
|
||||
double customPodScalar;
|
||||
double headingScalar;
|
||||
int xPodPort, yPodPort;
|
||||
|
||||
public OctoQuadForwardDirection(String name, OctoQuadTuner.PodType podType, double customPodScalar, double headingScalar,
|
||||
int xPodPort, int yPodPort) {
|
||||
super("Forward Direction Identification",
|
||||
"Determines if your forward pod needs to be reversed. \n"
|
||||
+ "Push your robot forward and then stop the Opmode",
|
||||
true);
|
||||
this.name = name;
|
||||
this.podType = podType;
|
||||
this.customPodScalar = customPodScalar;
|
||||
this.headingScalar = headingScalar;
|
||||
this.xPodPort = xPodPort;
|
||||
this.yPodPort = yPodPort;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Boolean runTuningOpMode() {
|
||||
OctoQuadConfig config = new OctoQuadConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.xPodPort.set(xPodPort);
|
||||
c.yPodPort.set(yPodPort);
|
||||
c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
|
||||
c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
|
||||
c.xPodOffset.set(0.0);
|
||||
c.yPodOffset.set(0.0);
|
||||
c.headingScalar.set(headingScalar);
|
||||
if (podType == OctoQuadTuner.PodType.CUSTOM) {
|
||||
c.ticksPerUnit.set(customPodScalar);
|
||||
} else {
|
||||
c.ticksPerUnit.set(podType == OctoQuadTuner.PodType.SWING_ARM ? OctoQuadTuner.SWING_ARM : OctoQuadTuner.FOUR_BAR);
|
||||
}
|
||||
});
|
||||
OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
}
|
||||
|
||||
return localizer.pose().x() < 0;
|
||||
}
|
||||
}
|
||||
|
||||
class OctoQuadStrafeDirection extends TuningOpMode<Boolean> {
|
||||
String name;
|
||||
OctoQuadTuner.PodType podType;
|
||||
double customPodScalar;
|
||||
double headingScalar;
|
||||
int xPodPort, yPodPort;
|
||||
|
||||
public OctoQuadStrafeDirection(String name, OctoQuadTuner.PodType podType, double customPodScalar, double headingScalar,
|
||||
int xPodPort, int yPodPort) {
|
||||
super("Strafe Direction Identification",
|
||||
"Determines if your strafe pod needs to be reversed. \n"
|
||||
+ "Push your robot to the left and then stop the Opmode",
|
||||
true);
|
||||
this.name = name;
|
||||
this.podType = podType;
|
||||
this.customPodScalar = customPodScalar;
|
||||
this.headingScalar = headingScalar;
|
||||
this.xPodPort = xPodPort;
|
||||
this.yPodPort = yPodPort;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Boolean runTuningOpMode() {
|
||||
OctoQuadConfig config = new OctoQuadConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.xPodPort.set(xPodPort);
|
||||
c.yPodPort.set(yPodPort);
|
||||
c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
|
||||
c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
|
||||
c.xPodOffset.set(0.0);
|
||||
c.yPodOffset.set(0.0);
|
||||
c.headingScalar.set(headingScalar);
|
||||
|
||||
if (podType == OctoQuadTuner.PodType.CUSTOM) {
|
||||
c.ticksPerUnit.set(customPodScalar);
|
||||
} else {
|
||||
c.ticksPerUnit.set(podType == OctoQuadTuner.PodType.SWING_ARM ? OctoQuadTuner.SWING_ARM : OctoQuadTuner.FOUR_BAR);
|
||||
}
|
||||
});
|
||||
OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
}
|
||||
|
||||
return localizer.pose().y() < 0;
|
||||
}
|
||||
}
|
||||
|
||||
class OctoQuadOffsets extends TuningOpMode<List<Double>> {
|
||||
String name;
|
||||
OctoQuadTuner.PodType podType;
|
||||
double customPodScalar;
|
||||
boolean forwardPodReversed, strafePodReversed;
|
||||
double headingScalar;
|
||||
Pose previous;
|
||||
int xPodPort, yPodPort;
|
||||
|
||||
public OctoQuadOffsets(String name, OctoQuadTuner.PodType podType, double customPodScalar, Boolean forwardPodReversed, Boolean strafePodReversed, double headingScalar,
|
||||
int xPodPort, int yPodPort) {
|
||||
super("OctoQuadOffsets Identification",
|
||||
"Automatically identifies the offsets for your OctoQuad localizer. \n"
|
||||
+ "Spin your robot in place 180 degrees counterclockwise and then stop the Opmode",
|
||||
true);
|
||||
this.name = name;
|
||||
this.podType = podType;
|
||||
this.customPodScalar = customPodScalar;
|
||||
this.forwardPodReversed = forwardPodReversed;
|
||||
this.strafePodReversed = strafePodReversed;
|
||||
this.headingScalar = headingScalar;
|
||||
this.xPodPort = xPodPort;
|
||||
this.yPodPort = yPodPort;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected List<Double> runTuningOpMode() {
|
||||
OctoQuadConfig config = new OctoQuadConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.xPodPort.set(xPodPort);
|
||||
c.yPodPort.set(yPodPort);
|
||||
c.xPodDirection.set(forwardPodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
|
||||
c.yPodDirection.set(strafePodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
|
||||
if (podType.equals(OctoQuadTuner.PodType.CUSTOM)) {
|
||||
c.encoderResolutionUnit.set(DistanceUnit.INCH);
|
||||
c.ticksPerUnit.set(customPodScalar);
|
||||
} else {
|
||||
c.encoderResolutionUnit.set(DistanceUnit.INCH);
|
||||
c.ticksPerUnit.set(podType == OctoQuadTuner.PodType.SWING_ARM ? OctoQuadTuner.SWING_ARM : OctoQuadTuner.FOUR_BAR);
|
||||
}
|
||||
c.xPodOffset.set(0.0);
|
||||
c.yPodOffset.set(0.0);
|
||||
c.headingScalar.set(headingScalar);
|
||||
});
|
||||
OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
localizer.update();
|
||||
|
||||
waitForStart();
|
||||
|
||||
while (!isStopRequested()) {
|
||||
previous = localizer.pose();
|
||||
localizer.update();
|
||||
telemetry.addData("heading", localizer.pose().heading());
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y()) {
|
||||
previous = localizer.pose();
|
||||
}
|
||||
|
||||
return List.of(((-previous.y()) / 2.0), ((-previous.x()) / 2.0));
|
||||
}
|
||||
}
|
||||
+256
@@ -0,0 +1,256 @@
|
||||
package org.firstinspires.ftc.teamcode.pedro.procedures;
|
||||
|
||||
import com.pedropathing.math.Pose;
|
||||
import com.pedropathing.revhub.localizers.PinpointConfig;
|
||||
import com.pedropathing.revhub.localizers.PinpointLocalizer;
|
||||
import com.pedropathing.tuning.autotune.Inputs;
|
||||
import com.pedropathing.tuning.autotune.Procedure;
|
||||
import com.pedropathing.tuning.autotune.TuningOpMode;
|
||||
import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
import java.util.*;
|
||||
|
||||
public class PinpointTuner extends Procedure {
|
||||
enum PodType {
|
||||
SWING_ARM,
|
||||
FOUR_BAR,
|
||||
CUSTOM
|
||||
}
|
||||
public PinpointTuner() {
|
||||
super("Pinpoint Tuner", "A procedure for tuning the Pinpoint localizer.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() throws InterruptedException {
|
||||
Inputs inputs = inputs("Setup", "Set Pinpoint HardwareMap Name and Odometry Pod Type");
|
||||
Inputs.Field<String> pinpointName = inputs.s("HardwareMap Name").withDefault("pinpoint");
|
||||
Inputs.Field<PodType> podType = inputs.e("Odometry Pod Type", PodType.class).withDefault(PodType.FOUR_BAR);
|
||||
awaitInputs(inputs);
|
||||
|
||||
OptionalDouble customPodScalar = OptionalDouble.empty();
|
||||
|
||||
if (podType.get() == PodType.CUSTOM) {
|
||||
Inputs inputsCustom = inputs("Custom Scalar Identification Push Distance", "Set the distance you will push your robot forward in inches");
|
||||
Inputs.Field<Double> distance = inputsCustom.d("Distance").withDefault(48.0);
|
||||
awaitInputs(inputsCustom);
|
||||
customPodScalar = OptionalDouble.of(runOpMode(new PinpointCustomPodScalar(distance.get(), pinpointName.get())));
|
||||
}
|
||||
|
||||
boolean forwardPodReversed = runOpMode(new PinpointForwardDirection(pinpointName.get(), podType.get(), customPodScalar));
|
||||
boolean strafePodReversed = runOpMode(new PinpointStrafeDirection(pinpointName.get(), podType.get(), customPodScalar));
|
||||
|
||||
List<Double> offsets = runOpMode(new PinpointOffsets(pinpointName.get(), podType.get(), customPodScalar, forwardPodReversed, strafePodReversed));
|
||||
|
||||
result("name", pinpointName.get());
|
||||
|
||||
if (customPodScalar.isPresent()) {
|
||||
result("podType", "Custom");
|
||||
result("ticksPerUnit", customPodScalar.getAsDouble());
|
||||
} else {
|
||||
result("podType", podType.get() == PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
|
||||
}
|
||||
|
||||
result("xPodDirection", forwardPodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||
result("yPodDirection", strafePodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||
result("xPodOffset", offsets.get(0));
|
||||
result("yPodOffset", offsets.get(1));
|
||||
|
||||
code(Language.JAVA,"public static PinpointConfig localizerConfig = new PinpointConfig(c -> {\n" +
|
||||
" c.name.set(\"" + pinpointName.get() + "\");\n" +
|
||||
(customPodScalar.isPresent() ? " c.ticksPerUnit.set(OptionalDouble.of(" + customPodScalar.getAsDouble() + "));\n" : " c.podType.set(" + (podType.get() == PodType.SWING_ARM ? "GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD" : "GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD") + ");\n") +
|
||||
" c.xPodOffset.set(" + offsets.get(0) + ");\n" +
|
||||
" c.yPodOffset.set(" + offsets.get(1) + ");\n" +
|
||||
" c.xPodDirection.set(" + (forwardPodReversed ? "GoBildaPinpointDriver.EncoderDirection.REVERSED" : "GoBildaPinpointDriver.EncoderDirection.FORWARD") + ");\n" +
|
||||
" c.yPodDirection.set(" + (strafePodReversed ? "GoBildaPinpointDriver.EncoderDirection.REVERSED" : "GoBildaPinpointDriver.EncoderDirection.FORWARD") + ");\n" +
|
||||
" c.globalDistanceUnit.set(DistanceUnit.INCH);\n" +
|
||||
" c.offsetUnits.set(DistanceUnit.INCH);\n" +
|
||||
"});");
|
||||
}
|
||||
}
|
||||
|
||||
class PinpointCustomPodScalar extends TuningOpMode<Double> {
|
||||
|
||||
String name;
|
||||
double distance;
|
||||
|
||||
public PinpointCustomPodScalar(Double distance, String name) {
|
||||
super("Custom Scalar Identification",
|
||||
"Determines the scalar for the custom pods of the Pinpoint localizer. \n"
|
||||
+ "Push your robot forward " + distance + " inches exactly and then stop the Opmode",
|
||||
true);
|
||||
this.name = name;
|
||||
this.distance = distance;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Double runTuningOpMode() {
|
||||
PinpointConfig config = new PinpointConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.ticksPerUnit.set(OptionalDouble.of(1.0));
|
||||
c.xPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||
c.yPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||
c.xPodOffset.set(0.0);
|
||||
c.yPodOffset.set(0.0);
|
||||
});
|
||||
PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
}
|
||||
return Math.abs((localizer.pose().x() / distance));
|
||||
}
|
||||
}
|
||||
|
||||
class PinpointForwardDirection extends TuningOpMode<Boolean> {
|
||||
String name;
|
||||
PinpointTuner.PodType podType;
|
||||
OptionalDouble customPodScalar;
|
||||
|
||||
public PinpointForwardDirection(String name, PinpointTuner.PodType podType, OptionalDouble customPodScalar) {
|
||||
super("Forward Direction Identification",
|
||||
"Determines if your forward pod needs to be reversed. \n"
|
||||
+ "Push your robot forward and then stop the Opmode",
|
||||
true);
|
||||
this.name = name;
|
||||
this.podType = podType;
|
||||
this.customPodScalar = customPodScalar;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Boolean runTuningOpMode() {
|
||||
PinpointConfig config = new PinpointConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.xPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||
c.yPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||
c.xPodOffset.set(0.0);
|
||||
c.yPodOffset.set(0.0);
|
||||
if (customPodScalar.isPresent()) {
|
||||
c.encoderResolutionUnit.set(DistanceUnit.INCH);
|
||||
c.ticksPerUnit.set(OptionalDouble.of(customPodScalar.getAsDouble()));
|
||||
} else {
|
||||
c.podType.set(podType == PinpointTuner.PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
|
||||
}
|
||||
});
|
||||
PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
waitForStart();
|
||||
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
}
|
||||
|
||||
return localizer.pose().x() < 0;
|
||||
}
|
||||
}
|
||||
|
||||
class PinpointStrafeDirection extends TuningOpMode<Boolean> {
|
||||
String name;
|
||||
PinpointTuner.PodType podType;
|
||||
OptionalDouble customPodScalar;
|
||||
|
||||
public PinpointStrafeDirection(String name, PinpointTuner.PodType podType, OptionalDouble customPodScalar) {
|
||||
super("Strafe Direction Identification",
|
||||
"Determines if your strafe pod needs to be reversed. \n"
|
||||
+ "Push your robot to the left and then stop the Opmode",
|
||||
true);
|
||||
this.name = name;
|
||||
this.podType = podType;
|
||||
this.customPodScalar = customPodScalar;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Boolean runTuningOpMode() {
|
||||
PinpointConfig config = new PinpointConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.xPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||
c.yPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||
c.xPodOffset.set(0.0);
|
||||
c.yPodOffset.set(0.0);
|
||||
if (customPodScalar.isPresent()) {
|
||||
c.encoderResolutionUnit.set(DistanceUnit.INCH);
|
||||
c.ticksPerUnit.set(OptionalDouble.of(customPodScalar.getAsDouble()));
|
||||
} else {
|
||||
c.podType.set(podType == PinpointTuner.PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
|
||||
}
|
||||
});
|
||||
PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
}
|
||||
|
||||
return localizer.pose().y() < 0;
|
||||
}
|
||||
}
|
||||
|
||||
class PinpointOffsets extends TuningOpMode<List<Double>> {
|
||||
String name;
|
||||
PinpointTuner.PodType podType;
|
||||
OptionalDouble customPodScalar = OptionalDouble.empty();
|
||||
boolean forwardPodReversed, strafePodReversed;
|
||||
private Pose previous = Pose.zero();
|
||||
|
||||
public PinpointOffsets(String name, PinpointTuner.PodType podType, OptionalDouble customPodScalar, Boolean forwardPodReversed, Boolean strafePodReversed) {
|
||||
super("Offsets Identification",
|
||||
"Automatically identifies the offsets for your Pinpoint localizer. \n"
|
||||
+ "Spin your robot in place 180 degrees counterclockwise and then stop the Opmode",
|
||||
true);
|
||||
this.name = name;
|
||||
this.podType = podType;
|
||||
if (customPodScalar.isPresent()) {
|
||||
this.customPodScalar = customPodScalar;
|
||||
}
|
||||
this.forwardPodReversed = forwardPodReversed;
|
||||
this.strafePodReversed = strafePodReversed;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected List<Double> runTuningOpMode() {
|
||||
PinpointConfig config = new PinpointConfig(c -> {
|
||||
c.name.set(name);
|
||||
c.xPodDirection.set(forwardPodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||
c.yPodDirection.set(strafePodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD);
|
||||
if (customPodScalar.isPresent()) {
|
||||
c.encoderResolutionUnit.set(DistanceUnit.INCH);
|
||||
c.ticksPerUnit.set(OptionalDouble.of(customPodScalar.getAsDouble()));
|
||||
c.resetMode.set(PinpointLocalizer.ResetMode.RESET_AND_RECALIBRATE_IMU);
|
||||
} else {
|
||||
c.podType.set(podType == PinpointTuner.PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
|
||||
}
|
||||
c.xPodOffset.set(0.0);
|
||||
c.yPodOffset.set(0.0);
|
||||
c.globalDistanceUnit.set(DistanceUnit.INCH);
|
||||
c.offsetUnits.set(DistanceUnit.INCH);
|
||||
});
|
||||
PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config);
|
||||
if (customPodScalar.isPresent()) {
|
||||
localizer.reset();
|
||||
}
|
||||
localizer.setPose(Pose.zero());
|
||||
localizer.update();
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
localizer.setPose(Pose.zero());
|
||||
|
||||
while (!isStopRequested()) {
|
||||
previous = localizer.pose();
|
||||
localizer.update();
|
||||
|
||||
telemetry.addData("heading", localizer.pose().heading());
|
||||
telemetry.addData("pose", localizer.pose());
|
||||
telemetry.addData("previous", previous);
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y()) {
|
||||
previous = localizer.pose();
|
||||
}
|
||||
|
||||
return List.of(((-previous.y()) / 2.0), ((-previous.x()) / 2.0));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,327 @@
|
||||
package org.firstinspires.ftc.teamcode.pedro.procedures;
|
||||
|
||||
import com.pedropathing.algorithm.Algorithm;
|
||||
import com.pedropathing.drivetrain.DrivePowers;
|
||||
import com.pedropathing.drivetrain.Drivetrain;
|
||||
import com.pedropathing.follower.Follower;
|
||||
import com.pedropathing.localization.Localizer;
|
||||
import com.pedropathing.math.Pose;
|
||||
import com.pedropathing.paths.Path;
|
||||
import com.pedropathing.paths.interpolator.Interpolator;
|
||||
import com.pedropathing.tuning.autotune.DisplayName;
|
||||
import com.pedropathing.tuning.autotune.Inputs;
|
||||
import com.pedropathing.tuning.autotune.Procedure;
|
||||
import com.pedropathing.tuning.autotune.TuningOpMode;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import java.util.function.Function;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import static com.pedropathing.api.Paths.curve;
|
||||
import static com.pedropathing.api.Paths.line;
|
||||
|
||||
public class Tests extends Procedure {
|
||||
enum Test {
|
||||
@DisplayName("Hold Test")
|
||||
HOLD,
|
||||
@DisplayName("Line Test")
|
||||
LINE,
|
||||
@DisplayName("Curve Test")
|
||||
CURVED,
|
||||
@DisplayName("Interpolation Test")
|
||||
INTERPOLATION_CURVED,
|
||||
@DisplayName("Localization Test")
|
||||
LOCALIZATION,
|
||||
@DisplayName("Driving Test")
|
||||
DRIVING,
|
||||
@DisplayName("Pose Test")
|
||||
POSE
|
||||
|
||||
}
|
||||
Function<HardwareMap, Drivetrain> drivetrainFunction;
|
||||
Function<HardwareMap, Localizer> localizerFunction;
|
||||
Supplier<Algorithm> algorithmSupplier;
|
||||
Function<HardwareMap, Follower> followerFunction;
|
||||
|
||||
public Tests(Function<HardwareMap, Drivetrain> drivetrainFunction, Function<HardwareMap, Localizer> localizerFunction, Supplier<Algorithm> algorithmSupplier) {
|
||||
super("Tests", "A procedure for testing the Follower.");
|
||||
this.drivetrainFunction = drivetrainFunction;
|
||||
this.localizerFunction = localizerFunction;
|
||||
this.algorithmSupplier = algorithmSupplier;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() throws InterruptedException {
|
||||
boolean completed = false;
|
||||
boolean algorithm = true, localizer = true, drivetrain = true;
|
||||
|
||||
if (algorithmSupplier == null)
|
||||
algorithm = false;
|
||||
|
||||
if (localizerFunction == null)
|
||||
localizer = false;
|
||||
|
||||
if (drivetrainFunction == null)
|
||||
drivetrain = false;
|
||||
|
||||
if (algorithm && localizer && drivetrain)
|
||||
followerFunction = (hardwareMap) -> new Follower(localizerFunction.apply(hardwareMap), drivetrainFunction.apply(hardwareMap), algorithmSupplier.get());
|
||||
|
||||
Inputs inputs = inputs("Select", "Select");
|
||||
Inputs.Field<Test> selectedTest = inputs.e("Test", Test.class).withDefault(Test.LINE);
|
||||
Inputs.Field<Double> distance = inputs.d("Distance").withDefault(48.0);
|
||||
|
||||
awaitInputs(inputs);
|
||||
|
||||
switch (selectedTest.get()) {
|
||||
case HOLD:
|
||||
if (!algorithm)
|
||||
throw new IllegalArgumentException("Algorithm is required for Hold Test.");
|
||||
completed = runOpMode(new TestsHold(followerFunction));
|
||||
break;
|
||||
case LINE:
|
||||
if (!algorithm)
|
||||
throw new IllegalArgumentException("Algorithm is required for Hold Test.");
|
||||
completed = runOpMode(new TestsLine(followerFunction, distance.get()));
|
||||
break;
|
||||
case CURVED:
|
||||
if (!algorithm)
|
||||
throw new IllegalArgumentException("Algorithm is required for Hold Test.");
|
||||
completed = runOpMode(new TestsCurve(followerFunction, distance.get()));
|
||||
break;
|
||||
case INTERPOLATION_CURVED:
|
||||
if (!algorithm)
|
||||
throw new IllegalArgumentException("Algorithm is required for Hold Test.");
|
||||
completed = runOpMode(new TestsInterpolation(followerFunction, distance.get()));
|
||||
break;
|
||||
case LOCALIZATION:
|
||||
if (!drivetrain)
|
||||
throw new IllegalArgumentException("Drivetrain is required for Localization Test.");
|
||||
if (!localizer)
|
||||
throw new IllegalArgumentException("Localizer is required for Localization Test.");
|
||||
completed = runOpMode(new TestsLocalization(drivetrainFunction, localizerFunction));
|
||||
break;
|
||||
case POSE:
|
||||
if (!localizer)
|
||||
throw new IllegalArgumentException("Localizer is required for Pose Test.");
|
||||
completed = runOpMode(new TestsPose(localizerFunction));
|
||||
break;
|
||||
case DRIVING:
|
||||
if (!drivetrain)
|
||||
throw new IllegalArgumentException("Drivetrain is required for Driving Test.");
|
||||
completed = runOpMode(new TestsDriving(drivetrainFunction));
|
||||
break;
|
||||
}
|
||||
|
||||
result("Completed", completed);
|
||||
}
|
||||
}
|
||||
|
||||
class TestsHold extends TuningOpMode<Boolean> {
|
||||
Function<HardwareMap, Follower> followerFunction;
|
||||
|
||||
public TestsHold(Function<HardwareMap, Follower> followerFunction) {
|
||||
super("Hold Test", "Tests the Follower's ability to hold a position.", true);
|
||||
this.followerFunction = followerFunction;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Boolean runTuningOpMode() throws InterruptedException {
|
||||
Follower follower = followerFunction.apply(hardwareMap);
|
||||
follower.setPose(Pose.zero());
|
||||
waitForStart();
|
||||
follower.hold(Pose.zero());
|
||||
while (opModeIsActive()) {
|
||||
follower.update();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
class TestsLine extends TuningOpMode<Boolean> {
|
||||
Function<HardwareMap, Follower> followerFunction;
|
||||
double distance;
|
||||
|
||||
public TestsLine(Function<HardwareMap, Follower> followerFunction, double distance) {
|
||||
super("Line Test", "Tests the Follower's ability to follow a line.", true);
|
||||
this.followerFunction = followerFunction;
|
||||
this.distance = distance;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Boolean runTuningOpMode() throws InterruptedException {
|
||||
Follower follower = followerFunction.apply(hardwareMap);
|
||||
follower.setPose(Pose.zero());
|
||||
|
||||
double distance = 48;
|
||||
boolean forward = true;
|
||||
|
||||
Path path1 = line(Pose.zero(), new Pose(distance,0, 0)).constant(0);
|
||||
Path path2 = line(new Pose(distance,0, 0), Pose.zero()).constant(0);
|
||||
|
||||
waitForStart();
|
||||
follower.follow(path1);
|
||||
|
||||
while (opModeIsActive()) {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
if (forward) {
|
||||
follower.follow(path2);
|
||||
} else {
|
||||
follower.follow(path1);
|
||||
}
|
||||
forward = !forward;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
class TestsCurve extends TuningOpMode<Boolean> {
|
||||
Function<HardwareMap, Follower> followerFunction;
|
||||
double distance;
|
||||
|
||||
public TestsCurve(Function<HardwareMap, Follower> followerFunction, double distance) {
|
||||
super("Curve Test", "Tests the Follower's ability to follow a curve.", true);
|
||||
this.followerFunction = followerFunction;
|
||||
this.distance = distance;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Boolean runTuningOpMode() throws InterruptedException {
|
||||
Follower follower = followerFunction.apply(hardwareMap);
|
||||
follower.setPose(Pose.zero());
|
||||
|
||||
double distance = 48;
|
||||
boolean forward = true;
|
||||
|
||||
Path path1 = curve(Pose.zero(), new Pose(distance + 0,0), new Pose(distance,distance)).tangent();
|
||||
Path path2 = curve(new Pose(distance,distance), new Pose(distance,0), Pose.zero()).tangent();
|
||||
|
||||
waitForStart();
|
||||
follower.follow(path1);
|
||||
|
||||
while (opModeIsActive()) {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
if (forward) {
|
||||
follower.follow(path2);
|
||||
} else {
|
||||
follower.follow(path1);
|
||||
}
|
||||
forward = !forward;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
class TestsInterpolation extends TuningOpMode<Boolean> {
|
||||
Function<HardwareMap, Follower> followerFunction;
|
||||
double distance;
|
||||
|
||||
public TestsInterpolation(Function<HardwareMap, Follower> followerFunction, double distance) {
|
||||
super("Interpolation Curve Test", "Tests the Follower's ability to follow a curve with several interpolations.", true);
|
||||
this.followerFunction = followerFunction;
|
||||
this.distance = distance;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Boolean runTuningOpMode() throws InterruptedException {
|
||||
Follower follower = followerFunction.apply(hardwareMap);
|
||||
follower.setPose(Pose.zero());
|
||||
|
||||
double distance = 48;
|
||||
boolean forward = true;
|
||||
|
||||
Path path1 = curve(Pose.zero(), new Pose(distance + 0,0), new Pose(distance,distance)).heading((curve, t) -> Math.PI);
|
||||
Path path2 = curve(new Pose(distance,distance), new Pose(distance,0), Pose.zero()).heading(Interpolator.piecewise().until(0.5, Interpolator.tangent).until(1.0, Interpolator.constant(0)));
|
||||
|
||||
waitForStart();
|
||||
follower.follow(path1);
|
||||
|
||||
while (opModeIsActive()) {
|
||||
follower.update();
|
||||
if (follower.atParametricEnd()) {
|
||||
if (forward) {
|
||||
follower.follow(path2);
|
||||
} else {
|
||||
follower.follow(path1);
|
||||
}
|
||||
forward = !forward;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
class TestsLocalization extends TuningOpMode<Boolean> {
|
||||
Function<HardwareMap, Drivetrain> drivetrainFunction;
|
||||
Function<HardwareMap, Localizer> localizerFunction;
|
||||
|
||||
public TestsLocalization(Function<HardwareMap, Drivetrain> drivetrainFunction, Function<HardwareMap, Localizer> localizerFunction) {
|
||||
super("Localization Test", "Verifies localization and manual control.", true);
|
||||
this.drivetrainFunction = drivetrainFunction;
|
||||
this.localizerFunction = localizerFunction;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Boolean runTuningOpMode() throws InterruptedException {
|
||||
Localizer localizer = localizerFunction.apply(hardwareMap);
|
||||
Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap);
|
||||
|
||||
localizer.setPose(Pose.zero());
|
||||
|
||||
waitForStart();
|
||||
|
||||
while (opModeIsActive()) {
|
||||
drivetrain.drive(new DrivePowers(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x), false);
|
||||
localizer.update();
|
||||
telemetry.addData("Pose", localizer.pose());
|
||||
telemetry.update();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
class TestsDriving extends TuningOpMode<Boolean> {
|
||||
Function<HardwareMap, Drivetrain> drivetrainFunction;
|
||||
|
||||
public TestsDriving(Function<HardwareMap, Drivetrain> drivetrainFunction) {
|
||||
super("Driving Test", "Tests raw drivetrain control without localization.", true);
|
||||
this.drivetrainFunction = drivetrainFunction;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Boolean runTuningOpMode() throws InterruptedException {
|
||||
Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap);
|
||||
waitForStart();
|
||||
while (opModeIsActive()) {
|
||||
drivetrain.drive(new DrivePowers(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x), true);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
class TestsPose extends TuningOpMode<Boolean> {
|
||||
Function<HardwareMap, Localizer> localizerFunction;
|
||||
|
||||
public TestsPose(Function<HardwareMap, Localizer> localizerFunction) {
|
||||
super("Pose Test", "Verifies localizer output without a drivetrain.", true);
|
||||
this.localizerFunction = localizerFunction;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Boolean runTuningOpMode() throws InterruptedException {
|
||||
Localizer localizer = localizerFunction.apply(hardwareMap);
|
||||
waitForStart();
|
||||
while (opModeIsActive()) {
|
||||
localizer.update();
|
||||
telemetry.addData("Pose", localizer.pose());
|
||||
telemetry.update();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
+328
@@ -0,0 +1,328 @@
|
||||
package org.firstinspires.ftc.teamcode.pedro.procedures;
|
||||
|
||||
import com.pedropathing.math.Pose;
|
||||
import com.pedropathing.revhub.localizers.Encoder;
|
||||
import com.pedropathing.revhub.localizers.RevHubIMU;
|
||||
import com.pedropathing.revhub.localizers.ThreeWheelIMUConfig;
|
||||
import com.pedropathing.revhub.localizers.ThreeWheelIMULocalizer;
|
||||
import com.pedropathing.tuning.autotune.Inputs;
|
||||
import com.pedropathing.tuning.autotune.Procedure;
|
||||
import com.pedropathing.tuning.autotune.TuningOpMode;
|
||||
import com.qualcomm.hardware.lynx.LynxModule;
|
||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
public class ThreeWheelIMUTuner extends Procedure {
|
||||
|
||||
private static String leftEncoderName = "lf";
|
||||
private static String rightEncoderName = "rr";
|
||||
private static String strafeEncoderName = "lr";
|
||||
private static String imuName = "imu";
|
||||
private static RevHubOrientationOnRobot.LogoFacingDirection logoDirection =
|
||||
RevHubOrientationOnRobot.LogoFacingDirection.UP;
|
||||
private static RevHubOrientationOnRobot.UsbFacingDirection usbDirection =
|
||||
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
|
||||
|
||||
public ThreeWheelIMUTuner() {
|
||||
super("Three Wheel + IMU Tuner", "Tune three odometry pods with an IMU");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() throws InterruptedException {
|
||||
Inputs setup = inputs("Encoder + IMU Setup",
|
||||
"Set encoder motor ports, IMU HardwareMap name, and Control Hub orientation.");
|
||||
Inputs.Field<String> leftEncoder = setup.s("Left Encoder Motor Name").withDefault("lf");
|
||||
Inputs.Field<String> rightEncoder = setup.s("Right Encoder Motor Name").withDefault("rr");
|
||||
Inputs.Field<String> strafeEncoder = setup.s("Strafe Encoder Motor Name").withDefault("lr");
|
||||
Inputs.Field<String> imu = setup.s("IMU HardwareMap Name").withDefault("imu");
|
||||
Inputs.Field<RevHubOrientationOnRobot.LogoFacingDirection> logo =
|
||||
setup.e("Logo Facing Direction", RevHubOrientationOnRobot.LogoFacingDirection.class)
|
||||
.withDefault(RevHubOrientationOnRobot.LogoFacingDirection.UP);
|
||||
Inputs.Field<RevHubOrientationOnRobot.UsbFacingDirection> usb =
|
||||
setup.e("USB Facing Direction", RevHubOrientationOnRobot.UsbFacingDirection.class)
|
||||
.withDefault(RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD);
|
||||
awaitInputs(setup);
|
||||
leftEncoderName = leftEncoder.get();
|
||||
rightEncoderName = rightEncoder.get();
|
||||
strafeEncoderName = strafeEncoder.get();
|
||||
imuName = imu.get();
|
||||
logoDirection = logo.get();
|
||||
usbDirection = usb.get();
|
||||
|
||||
Inputs resolution = inputs("Encoder Resolution Identification",
|
||||
"Set a positive push distance in inches. Keep the robot straight during each push.");
|
||||
Inputs.Field<Double> distance = resolution.d("Distance").withDefault(48.0);
|
||||
awaitInputs(resolution);
|
||||
if (!(distance.get() > 0.0)) {
|
||||
abort("Enter a positive distance in inches.");
|
||||
return;
|
||||
}
|
||||
|
||||
List<Double> left = measure("Left", distance.get());
|
||||
if (left == null) {
|
||||
return;
|
||||
}
|
||||
List<Double> right = measure("Right", distance.get());
|
||||
if (right == null) {
|
||||
return;
|
||||
}
|
||||
List<Double> strafe = measure("Strafe", distance.get());
|
||||
if (strafe == null) {
|
||||
return;
|
||||
}
|
||||
|
||||
double leftTicksPerInch = left.get(0);
|
||||
double rightTicksPerInch = right.get(0);
|
||||
double strafeTicksPerInch = strafe.get(0);
|
||||
double forwardTicksPerInch = 2.0 / (1.0 / leftTicksPerInch + 1.0 / rightTicksPerInch);
|
||||
|
||||
double forward = 1.0 / forwardTicksPerInch;
|
||||
double lateral = 1.0 / strafeTicksPerInch;
|
||||
|
||||
List<Double> leftOffsets = runOpMode(new ThreeWheelIMUOffsets(
|
||||
true, forward, lateral, left.get(1), right.get(1), strafe.get(1)));
|
||||
if (leftOffsets == null) {
|
||||
abort("Left stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop.");
|
||||
return;
|
||||
}
|
||||
List<Double> rightOffsets = runOpMode(new ThreeWheelIMUOffsets(
|
||||
false, forward, lateral, left.get(1), right.get(1), strafe.get(1)));
|
||||
|
||||
if (rightOffsets == null) {
|
||||
abort("Right stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop.");
|
||||
return;
|
||||
}
|
||||
ThreeWheelIMUConfig config = config(true, forward, lateral,
|
||||
left.get(1), right.get(1), strafe.get(1));
|
||||
config.leftPodY.set(leftOffsets.get(0));
|
||||
config.rightPodY.set(rightOffsets.get(0));
|
||||
config.turnTicksToRadians.set(forward);
|
||||
|
||||
Double turn = runOpMode(new ThreeWheelIMUTurn(config));
|
||||
if (turn == null) {
|
||||
abort("Turn stage ended without positive rotation. Rotate 360 degrees CCW, then press Stop.");
|
||||
return;
|
||||
}
|
||||
double strafeX = (leftOffsets.get(1) + rightOffsets.get(1)) / 2.0 * turn / lateral;
|
||||
|
||||
result("leftEncoderName", leftEncoderName);
|
||||
result("rightEncoderName", rightEncoderName);
|
||||
result("strafeEncoderName", strafeEncoderName);
|
||||
result("imuName", imuName);
|
||||
result("logoDirection", logoDirection);
|
||||
result("usbDirection", usbDirection);
|
||||
result("leftPodY", leftOffsets.get(0));
|
||||
result("rightPodY", rightOffsets.get(0));
|
||||
result("strafePodX", strafeX);
|
||||
result("leftTicksPerInch", leftTicksPerInch);
|
||||
result("rightTicksPerInch", rightTicksPerInch);
|
||||
result("forwardTicksPerInch", forwardTicksPerInch);
|
||||
result("strafeTicksPerInch", strafeTicksPerInch);
|
||||
result("forwardTicksToInches", forward);
|
||||
result("strafeTicksToInches", lateral);
|
||||
result("turnTicksToRadians", turn);
|
||||
result("leftEncoderDirection", direction(left.get(1)));
|
||||
result("rightEncoderDirection", direction(right.get(1)));
|
||||
result("strafeEncoderDirection", direction(strafe.get(1)));
|
||||
|
||||
code(Language.JAVA,
|
||||
"public static ThreeWheelIMUConfig localizerConfig = new ThreeWheelIMUConfig(c -> {\n" +
|
||||
" c.leftEncoderName.set(\"" + leftEncoderName + "\");\n" +
|
||||
" c.rightEncoderName.set(\"" + rightEncoderName + "\");\n" +
|
||||
" c.strafeEncoderName.set(\"" + strafeEncoderName + "\");\n" +
|
||||
" c.imuName.set(\"" + imuName + "\");\n" +
|
||||
" c.imuOrientation.set(new RevHubOrientationOnRobot(\n" +
|
||||
" RevHubOrientationOnRobot.LogoFacingDirection." + logoDirection.name() + ",\n" +
|
||||
" RevHubOrientationOnRobot.UsbFacingDirection." + usbDirection.name() + "\n" +
|
||||
" ));\n" +
|
||||
" c.leftPodY.set(" + leftOffsets.get(0) + ");\n" +
|
||||
" c.rightPodY.set(" + rightOffsets.get(0) + ");\n" +
|
||||
" c.strafePodX.set(" + strafeX + ");\n" +
|
||||
" c.forwardTicksToInches.set(" + forward + ");\n" +
|
||||
" c.strafeTicksToInches.set(" + lateral + ");\n" +
|
||||
" c.turnTicksToRadians.set(" + turn + ");\n" +
|
||||
" c.leftEncoderDirection.set(" + direction(left.get(1)) + ");\n" +
|
||||
" c.rightEncoderDirection.set(" + direction(right.get(1)) + ");\n" +
|
||||
" c.strafeEncoderDirection.set(" + direction(strafe.get(1)) + ");\n" +
|
||||
"});");
|
||||
}
|
||||
|
||||
private List<Double> measure(String pod, double distance) throws InterruptedException {
|
||||
List<Double> measured = runOpMode(new ThreeWheelIMUResolution(pod, distance));
|
||||
if (measured == null) {
|
||||
abort(pod + " stage ended without a nonzero measurement. Check the displayed ticks, complete the push, then press Stop.");
|
||||
return null;
|
||||
}
|
||||
return measured;
|
||||
}
|
||||
|
||||
static String direction(double direction) {
|
||||
return direction == Encoder.REVERSE ? "Encoder.REVERSE" : "Encoder.FORWARD";
|
||||
}
|
||||
|
||||
static ThreeWheelIMUConfig config(boolean left, double forward, double strafe,
|
||||
double leftDirection, double rightDirection, double strafeDirection) {
|
||||
return new ThreeWheelIMUConfig(c -> {
|
||||
c.leftEncoderName.set(leftEncoderName);
|
||||
c.rightEncoderName.set(rightEncoderName);
|
||||
c.strafeEncoderName.set(strafeEncoderName);
|
||||
c.imuName.set(imuName);
|
||||
c.imu.set(new RevHubIMU(new RevHubOrientationOnRobot(logoDirection, usbDirection))); c.leftPodY.set(left ? 0.0 : 1.0);
|
||||
c.rightPodY.set(left ? -1.0 : 0.0);
|
||||
c.strafePodX.set(0.0);
|
||||
c.forwardTicksToInches.set(forward);
|
||||
c.strafeTicksToInches.set(strafe);
|
||||
c.turnTicksToRadians.set(0.0);
|
||||
c.leftEncoderDirection.set(leftDirection);
|
||||
c.rightEncoderDirection.set(rightDirection);
|
||||
c.strafeEncoderDirection.set(strafeDirection);
|
||||
});
|
||||
}
|
||||
|
||||
static ThreeWheelIMULocalizer localizer(HardwareMap map, ThreeWheelIMUConfig config) {
|
||||
for (LynxModule hub : map.getAll(LynxModule.class)) {
|
||||
hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
|
||||
}
|
||||
for (String name : new String[]{"lf", "lr", "rf", "rr"}) {
|
||||
DcMotorEx motor = map.get(DcMotorEx.class, name);
|
||||
motor.setPower(0);
|
||||
motor.setDirection(name.equals("lf") || name.equals("lr")
|
||||
? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
|
||||
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
|
||||
}
|
||||
return new ThreeWheelIMULocalizer(map, config);
|
||||
}
|
||||
}
|
||||
|
||||
class ThreeWheelIMUResolution extends TuningOpMode<List<Double>> {
|
||||
|
||||
String pod;
|
||||
double distance;
|
||||
|
||||
ThreeWheelIMUResolution(String pod, double distance) {
|
||||
super(pod + " Encoder Resolution and Direction",
|
||||
"After Start, push the robot " + (pod.equals("Strafe") ? "left " : "forward ") +
|
||||
distance + " inches exactly without turning. Stop moving, press Stop to save this measurement.", true);
|
||||
this.pod = pod;
|
||||
this.distance = distance;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected List<Double> runTuningOpMode() {
|
||||
ThreeWheelIMUConfig config = ThreeWheelIMUTuner.config(!pod.equals("Right"), 1.0, 1.0,
|
||||
Encoder.FORWARD, Encoder.FORWARD, Encoder.FORWARD);
|
||||
ThreeWheelIMULocalizer localizer = ThreeWheelIMUTuner.localizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
Pose position = null;
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
position = localizer.pose();
|
||||
}
|
||||
|
||||
if (position == null) {
|
||||
return null;
|
||||
}
|
||||
double movement = pod.equals("Strafe") ? position.y() : position.x();
|
||||
if (movement == 0.0) {
|
||||
return null;
|
||||
}
|
||||
return List.of(Math.abs(movement / distance), movement < 0 ? Encoder.REVERSE : Encoder.FORWARD);
|
||||
}
|
||||
}
|
||||
|
||||
class ThreeWheelIMUOffsets extends TuningOpMode<List<Double>> {
|
||||
|
||||
boolean left;
|
||||
double forward;
|
||||
double strafe;
|
||||
double leftDirection;
|
||||
double rightDirection;
|
||||
double strafeDirection;
|
||||
|
||||
ThreeWheelIMUOffsets(boolean left, double forward, double strafe,
|
||||
double leftDirection, double rightDirection, double strafeDirection) {
|
||||
super((left ? "Left" : "Right") + " Pod Offset Identification",
|
||||
"After Start, rotate exactly 180 degrees counterclockwise about the robot center. " +
|
||||
"Keep that center fixed. Stop moving, press Stop to save this measurement.", true);
|
||||
this.left = left;
|
||||
this.forward = forward;
|
||||
this.strafe = strafe;
|
||||
this.leftDirection = leftDirection;
|
||||
this.rightDirection = rightDirection;
|
||||
this.strafeDirection = strafeDirection;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected List<Double> runTuningOpMode() {
|
||||
ThreeWheelIMUConfig config = ThreeWheelIMUTuner.config(left, forward, strafe,
|
||||
leftDirection, rightDirection, strafeDirection);
|
||||
boolean previousUseIMU = ThreeWheelIMULocalizer.useIMU;
|
||||
ThreeWheelIMULocalizer.useIMU = false;
|
||||
try {
|
||||
ThreeWheelIMULocalizer localizer = ThreeWheelIMUTuner.localizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
localizer.update();
|
||||
Pose position = null;
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
position = localizer.pose();
|
||||
}
|
||||
|
||||
if (position == null) {
|
||||
return null;
|
||||
}
|
||||
return List.of(-position.x() / Math.PI, position.y() / Math.PI);
|
||||
} finally {
|
||||
ThreeWheelIMULocalizer.useIMU = previousUseIMU;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
class ThreeWheelIMUTurn extends TuningOpMode<Double> {
|
||||
|
||||
ThreeWheelIMUConfig config;
|
||||
|
||||
ThreeWheelIMUTurn(ThreeWheelIMUConfig config) {
|
||||
super("Turn Multiplier Identification",
|
||||
"After Start, rotate exactly 360 degrees counterclockwise. " +
|
||||
"Stop moving, press Stop to save this measurement.", true);
|
||||
this.config = config;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Double runTuningOpMode() {
|
||||
boolean previousUseIMU = ThreeWheelIMULocalizer.useIMU;
|
||||
ThreeWheelIMULocalizer.useIMU = false;
|
||||
try {
|
||||
ThreeWheelIMULocalizer localizer = ThreeWheelIMUTuner.localizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
localizer.update();
|
||||
double startHeading = localizer.getTotalHeading();
|
||||
Double heading = null;
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
heading = localizer.getTotalHeading();
|
||||
}
|
||||
|
||||
if (heading == null || heading <= startHeading) {
|
||||
return null;
|
||||
}
|
||||
return config.turnTicksToRadians.get() * (2.0 * Math.PI) / (heading - startHeading);
|
||||
} finally {
|
||||
ThreeWheelIMULocalizer.useIMU = previousUseIMU;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
+290
@@ -0,0 +1,290 @@
|
||||
package org.firstinspires.ftc.teamcode.pedro.procedures;
|
||||
|
||||
import com.pedropathing.math.Pose;
|
||||
import com.pedropathing.revhub.localizers.Encoder;
|
||||
import com.pedropathing.revhub.localizers.ThreeWheelConfig;
|
||||
import com.pedropathing.revhub.localizers.ThreeWheelLocalizer;
|
||||
import com.pedropathing.tuning.autotune.Inputs;
|
||||
import com.pedropathing.tuning.autotune.Procedure;
|
||||
import com.pedropathing.tuning.autotune.TuningOpMode;
|
||||
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.HardwareMap;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
public class ThreeWheelTuner extends Procedure {
|
||||
|
||||
private static String leftEncoderName = "lf";
|
||||
private static String rightEncoderName = "rr";
|
||||
private static String strafeEncoderName = "lr";
|
||||
|
||||
public ThreeWheelTuner() {
|
||||
super("Three Wheel Tuner", "Tune three odometry pods");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() throws InterruptedException {
|
||||
Inputs setup = inputs("Encoder Setup",
|
||||
"Set the motor ports that the three odometry encoders are plugged into.");
|
||||
Inputs.Field<String> leftEncoder = setup.s("Left Encoder Motor Name").withDefault("lf");
|
||||
Inputs.Field<String> rightEncoder = setup.s("Right Encoder Motor Name").withDefault("rr");
|
||||
Inputs.Field<String> strafeEncoder = setup.s("Strafe Encoder Motor Name").withDefault("lr");
|
||||
awaitInputs(setup);
|
||||
leftEncoderName = leftEncoder.get();
|
||||
rightEncoderName = rightEncoder.get();
|
||||
strafeEncoderName = strafeEncoder.get();
|
||||
|
||||
Inputs resolution = inputs("Encoder Resolution Identification",
|
||||
"Set a positive push distance in inches. Keep the robot straight during each push.");
|
||||
Inputs.Field<Double> distance = resolution.d("Distance").withDefault(48.0);
|
||||
awaitInputs(resolution);
|
||||
if (!(distance.get() > 0.0)) {
|
||||
abort("Enter a positive distance in inches.");
|
||||
return;
|
||||
}
|
||||
|
||||
List<Double> left = measure("Left", distance.get());
|
||||
if (left == null) {
|
||||
return;
|
||||
}
|
||||
List<Double> right = measure("Right", distance.get());
|
||||
if (right == null) {
|
||||
return;
|
||||
}
|
||||
List<Double> strafe = measure("Strafe", distance.get());
|
||||
if (strafe == null) {
|
||||
return;
|
||||
}
|
||||
|
||||
double leftTicksPerInch = left.get(0);
|
||||
double rightTicksPerInch = right.get(0);
|
||||
double strafeTicksPerInch = strafe.get(0);
|
||||
double forwardTicksPerInch = 2.0 / (1.0 / leftTicksPerInch + 1.0 / rightTicksPerInch);
|
||||
|
||||
double forward = 1.0 / forwardTicksPerInch;
|
||||
double lateral = 1.0 / strafeTicksPerInch;
|
||||
|
||||
List<Double> leftOffsets = runOpMode(new ThreeWheelOffsets(
|
||||
true, forward, lateral, left.get(1), right.get(1), strafe.get(1)));
|
||||
if (leftOffsets == null) {
|
||||
abort("Left stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop.");
|
||||
return;
|
||||
}
|
||||
List<Double> rightOffsets = runOpMode(new ThreeWheelOffsets(
|
||||
false, forward, lateral, left.get(1), right.get(1), strafe.get(1)));
|
||||
|
||||
if (rightOffsets == null) {
|
||||
abort("Right stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop.");
|
||||
return;
|
||||
}
|
||||
ThreeWheelConfig config = config(true, forward, lateral,
|
||||
left.get(1), right.get(1), strafe.get(1));
|
||||
config.leftPodY.set(leftOffsets.get(0));
|
||||
config.rightPodY.set(rightOffsets.get(0));
|
||||
config.turnTicksToRadians.set(forward);
|
||||
|
||||
Double turn = runOpMode(new ThreeWheelTurn(config));
|
||||
if (turn == null) {
|
||||
abort("Turn stage ended without positive rotation. Rotate 360 degrees CCW, then press Stop.");
|
||||
return;
|
||||
}
|
||||
double strafeX = (leftOffsets.get(1) + rightOffsets.get(1)) / 2.0 * turn / lateral;
|
||||
|
||||
result("leftEncoderName", leftEncoderName);
|
||||
result("rightEncoderName", rightEncoderName);
|
||||
result("strafeEncoderName", strafeEncoderName);
|
||||
result("leftPodY", leftOffsets.get(0));
|
||||
result("rightPodY", rightOffsets.get(0));
|
||||
result("strafePodX", strafeX);
|
||||
result("leftTicksPerInch", leftTicksPerInch);
|
||||
result("rightTicksPerInch", rightTicksPerInch);
|
||||
result("forwardTicksPerInch", forwardTicksPerInch);
|
||||
result("strafeTicksPerInch", strafeTicksPerInch);
|
||||
result("forwardTicksToInches", forward);
|
||||
result("strafeTicksToInches", lateral);
|
||||
result("turnTicksToRadians", turn);
|
||||
result("leftEncoderDirection", direction(left.get(1)));
|
||||
result("rightEncoderDirection", direction(right.get(1)));
|
||||
result("strafeEncoderDirection", direction(strafe.get(1)));
|
||||
|
||||
code(Language.JAVA,
|
||||
"public static ThreeWheelConfig localizerConfig = new ThreeWheelConfig(c -> {\n" +
|
||||
" c.leftEncoderName.set(\"" + leftEncoderName + "\");\n" +
|
||||
" c.rightEncoderName.set(\"" + rightEncoderName + "\");\n" +
|
||||
" c.strafeEncoderName.set(\"" + strafeEncoderName + "\");\n" +
|
||||
" c.leftPodY.set(" + leftOffsets.get(0) + ");\n" +
|
||||
" c.rightPodY.set(" + rightOffsets.get(0) + ");\n" +
|
||||
" c.strafePodX.set(" + strafeX + ");\n" +
|
||||
" c.forwardTicksToInches.set(" + forward + ");\n" +
|
||||
" c.strafeTicksToInches.set(" + lateral + ");\n" +
|
||||
" c.turnTicksToRadians.set(" + turn + ");\n" +
|
||||
" c.leftEncoderDirection.set(" + direction(left.get(1)) + ");\n" +
|
||||
" c.rightEncoderDirection.set(" + direction(right.get(1)) + ");\n" +
|
||||
" c.strafeEncoderDirection.set(" + direction(strafe.get(1)) + ");\n" +
|
||||
"});");
|
||||
}
|
||||
|
||||
private List<Double> measure(String pod, double distance) throws InterruptedException {
|
||||
List<Double> measured = runOpMode(new ThreeWheelResolution(pod, distance));
|
||||
if (measured == null) {
|
||||
abort(pod + " stage ended without a nonzero measurement. Check the displayed ticks, complete the push, then press Stop.");
|
||||
return null;
|
||||
}
|
||||
return measured;
|
||||
}
|
||||
|
||||
static String direction(double direction) {
|
||||
return direction == Encoder.REVERSE ? "Encoder.REVERSE" : "Encoder.FORWARD";
|
||||
}
|
||||
|
||||
static ThreeWheelConfig config(boolean left, double forward, double strafe,
|
||||
double leftDirection, double rightDirection, double strafeDirection) {
|
||||
return new ThreeWheelConfig(c -> {
|
||||
c.leftEncoderName.set(leftEncoderName);
|
||||
c.rightEncoderName.set(rightEncoderName);
|
||||
c.strafeEncoderName.set(strafeEncoderName);
|
||||
c.leftPodY.set(left ? 0.0 : 1.0);
|
||||
c.rightPodY.set(left ? -1.0 : 0.0);
|
||||
c.strafePodX.set(0.0);
|
||||
c.forwardTicksToInches.set(forward);
|
||||
c.strafeTicksToInches.set(strafe);
|
||||
c.turnTicksToRadians.set(0.0);
|
||||
c.leftEncoderDirection.set(leftDirection);
|
||||
c.rightEncoderDirection.set(rightDirection);
|
||||
c.strafeEncoderDirection.set(strafeDirection);
|
||||
});
|
||||
}
|
||||
|
||||
static ThreeWheelLocalizer localizer(HardwareMap map, ThreeWheelConfig config) {
|
||||
for (LynxModule hub : map.getAll(LynxModule.class)) {
|
||||
hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
|
||||
}
|
||||
for (String name : new String[]{"lf", "lr", "rf", "rr"}) {
|
||||
DcMotorEx motor = map.get(DcMotorEx.class, name);
|
||||
motor.setPower(0);
|
||||
motor.setDirection(name.equals("lf") || name.equals("lr")
|
||||
? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
|
||||
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
|
||||
}
|
||||
return new ThreeWheelLocalizer(map, config);
|
||||
}
|
||||
}
|
||||
|
||||
class ThreeWheelResolution extends TuningOpMode<List<Double>> {
|
||||
|
||||
String pod;
|
||||
double distance;
|
||||
|
||||
ThreeWheelResolution(String pod, double distance) {
|
||||
super(pod + " Encoder Resolution and Direction",
|
||||
"After Start, push the robot " + (pod.equals("Strafe") ? "left " : "forward ") +
|
||||
distance + " inches exactly without turning. Stop moving, press Stop to save this measurement.", true);
|
||||
this.pod = pod;
|
||||
this.distance = distance;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected List<Double> runTuningOpMode() {
|
||||
ThreeWheelConfig config = ThreeWheelTuner.config(!pod.equals("Right"), 1.0, 1.0,
|
||||
Encoder.FORWARD, Encoder.FORWARD, Encoder.FORWARD);
|
||||
ThreeWheelLocalizer localizer = ThreeWheelTuner.localizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
Pose position = null;
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
position = localizer.pose();
|
||||
}
|
||||
|
||||
if (position == null) {
|
||||
return null;
|
||||
}
|
||||
double movement = pod.equals("Strafe") ? position.y() : position.x();
|
||||
if (movement == 0.0) {
|
||||
return null;
|
||||
}
|
||||
return List.of(Math.abs(movement / distance), movement < 0 ? Encoder.REVERSE : Encoder.FORWARD);
|
||||
}
|
||||
}
|
||||
|
||||
class ThreeWheelOffsets extends TuningOpMode<List<Double>> {
|
||||
|
||||
boolean left;
|
||||
double forward;
|
||||
double strafe;
|
||||
double leftDirection;
|
||||
double rightDirection;
|
||||
double strafeDirection;
|
||||
|
||||
ThreeWheelOffsets(boolean left, double forward, double strafe,
|
||||
double leftDirection, double rightDirection, double strafeDirection) {
|
||||
super((left ? "Left" : "Right") + " Pod Offset Identification",
|
||||
"After Start, rotate exactly 180 degrees counterclockwise about the robot center. " +
|
||||
"Keep that center fixed. Stop moving, press Stop to save this measurement.", true);
|
||||
this.left = left;
|
||||
this.forward = forward;
|
||||
this.strafe = strafe;
|
||||
this.leftDirection = leftDirection;
|
||||
this.rightDirection = rightDirection;
|
||||
this.strafeDirection = strafeDirection;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected List<Double> runTuningOpMode() {
|
||||
ThreeWheelConfig config = ThreeWheelTuner.config(left, forward, strafe,
|
||||
leftDirection, rightDirection, strafeDirection);
|
||||
ThreeWheelLocalizer localizer = ThreeWheelTuner.localizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
localizer.update();
|
||||
Pose position = null;
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
position = localizer.pose();
|
||||
}
|
||||
|
||||
if (position == null) {
|
||||
return null;
|
||||
}
|
||||
return List.of(-position.x() / Math.PI, position.y() / Math.PI);
|
||||
}
|
||||
}
|
||||
|
||||
class ThreeWheelTurn extends TuningOpMode<Double> {
|
||||
|
||||
ThreeWheelConfig config;
|
||||
|
||||
ThreeWheelTurn(ThreeWheelConfig config) {
|
||||
super("Turn Multiplier Identification",
|
||||
"After Start, rotate exactly 360 degrees counterclockwise. " +
|
||||
"Stop moving, press Stop to save this measurement.", true);
|
||||
this.config = config;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Double runTuningOpMode() {
|
||||
ThreeWheelLocalizer localizer = ThreeWheelTuner.localizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
localizer.update();
|
||||
double startHeading = localizer.getTotalHeading();
|
||||
Double heading = null;
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
heading = localizer.getTotalHeading();
|
||||
}
|
||||
|
||||
if (heading == null || heading <= startHeading) {
|
||||
return null;
|
||||
}
|
||||
return config.turnTicksToRadians.get() * (2.0 * Math.PI) / (heading - startHeading);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
+404
@@ -0,0 +1,404 @@
|
||||
package org.firstinspires.ftc.teamcode.pedro.procedures;
|
||||
|
||||
import com.pedropathing.math.Pose;
|
||||
import com.pedropathing.revhub.localizers.Encoder;
|
||||
import com.pedropathing.revhub.localizers.RevHubIMU;
|
||||
import com.pedropathing.revhub.localizers.TwoWheelConfig;
|
||||
import com.pedropathing.revhub.localizers.TwoWheelLocalizer;
|
||||
import com.pedropathing.tuning.autotune.Inputs;
|
||||
import com.pedropathing.tuning.autotune.Procedure;
|
||||
import com.pedropathing.tuning.autotune.TuningOpMode;
|
||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
public class TwoWheelTuner extends Procedure {
|
||||
|
||||
public TwoWheelTuner() {
|
||||
super("Two Wheel Tuner", "A procedure for tuning the Two Wheel localizer.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void run() throws InterruptedException {
|
||||
Inputs setup = inputs("Setup", "Set encoder, IMU, and Control Hub orientation");
|
||||
Inputs.Field<String> forwardPodName = setup.s("Forward Encoder Motor Name").withDefault("lf");
|
||||
Inputs.Field<String> strafePodName = setup.s("Strafe Encoder Motor Name").withDefault("rr");
|
||||
Inputs.Field<String> imuName = setup.s("IMU HardwareMap Name").withDefault("imu");
|
||||
Inputs.Field<RevHubOrientationOnRobot.LogoFacingDirection> logoDirection =
|
||||
setup.e("Logo Facing Direction", RevHubOrientationOnRobot.LogoFacingDirection.class)
|
||||
.withDefault(RevHubOrientationOnRobot.LogoFacingDirection.UP);
|
||||
Inputs.Field<RevHubOrientationOnRobot.UsbFacingDirection> usbDirection =
|
||||
setup.e("USB Facing Direction", RevHubOrientationOnRobot.UsbFacingDirection.class)
|
||||
.withDefault(RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD);
|
||||
awaitInputs(setup);
|
||||
|
||||
Inputs resolution = inputs("Encoder Resolution Identification", "Set the exact distance you will push the robot in inches");
|
||||
Inputs.Field<Double> distance = resolution.d("Distance").withDefault(48.0);
|
||||
awaitInputs(resolution);
|
||||
|
||||
TwoWheelSetup values = new TwoWheelSetup(
|
||||
forwardPodName.get(),
|
||||
strafePodName.get(),
|
||||
imuName.get(),
|
||||
logoDirection.get(),
|
||||
usbDirection.get()
|
||||
);
|
||||
|
||||
Double forwardTicksPerInchResult = runOpMode(new TwoWheelForwardResolution(values, distance.get()));
|
||||
Double strafeTicksPerInchResult = runOpMode(new TwoWheelStrafeResolution(values, distance.get()));
|
||||
|
||||
if (forwardTicksPerInchResult == null || strafeTicksPerInchResult == null
|
||||
|| forwardTicksPerInchResult == 0.0 || strafeTicksPerInchResult == 0.0) {
|
||||
abort("Encoder resolution measurement was zero. Complete both pushes before pressing Stop.");
|
||||
return;
|
||||
}
|
||||
|
||||
double forwardTicksPerInch = forwardTicksPerInchResult;
|
||||
double strafeTicksPerInch = strafeTicksPerInchResult;
|
||||
|
||||
double forwardTicksToInches = 1.0 / forwardTicksPerInch;
|
||||
double strafeTicksToInches = 1.0 / strafeTicksPerInch;
|
||||
|
||||
boolean forwardPodReversed = runOpMode(
|
||||
new TwoWheelForwardDirection(values, forwardTicksToInches, strafeTicksToInches)
|
||||
);
|
||||
|
||||
boolean strafePodReversed = runOpMode(
|
||||
new TwoWheelStrafeDirection(values, forwardTicksToInches, strafeTicksToInches)
|
||||
);
|
||||
|
||||
List<Double> offsets = runOpMode(
|
||||
new TwoWheelOffsets(
|
||||
values,
|
||||
forwardTicksToInches,
|
||||
strafeTicksToInches,
|
||||
forwardPodReversed,
|
||||
strafePodReversed
|
||||
)
|
||||
);
|
||||
|
||||
result("xPodName", values.forwardPodName);
|
||||
result("yPodName", values.strafePodName);
|
||||
result("imuName", values.imuName);
|
||||
result("logoDirection", values.logoDirection);
|
||||
result("usbDirection", values.usbDirection);
|
||||
result("forwardTicksPerInch", forwardTicksPerInch);
|
||||
result("strafeTicksPerInch", strafeTicksPerInch);
|
||||
result("forwardTicksToInches", forwardTicksToInches);
|
||||
result("strafeTicksToInches", strafeTicksToInches);
|
||||
result("xPodDirection", forwardPodReversed ? "REVERSED" : "FORWARD");
|
||||
result("yPodDirection", strafePodReversed ? "REVERSED" : "FORWARD");
|
||||
result("xPodOffset", offsets.get(0));
|
||||
result("yPodOffset", offsets.get(1));
|
||||
|
||||
code(Language.JAVA,
|
||||
"public static TwoWheelConfig localizerConfig = new TwoWheelConfig(c -> {\n" +
|
||||
" c.xPodName.set(\"" + values.forwardPodName + "\");\n" +
|
||||
" c.yPodName.set(\"" + values.strafePodName + "\");\n" +
|
||||
" c.imuName.set(\"" + values.imuName + "\");\n" +
|
||||
" c.xPodOffset.set(" + offsets.get(0) + ");\n" +
|
||||
" c.yPodOffset.set(" + offsets.get(1) + ");\n" +
|
||||
" c.forwardTicksToInches.set(" + forwardTicksToInches + ");\n" +
|
||||
" c.strafeTicksToInches.set(" + strafeTicksToInches + ");\n" +
|
||||
" c.xPodDirection.set(" +
|
||||
(forwardPodReversed ? "Encoder.REVERSE" : "Encoder.FORWARD") +
|
||||
");\n" +
|
||||
" c.yPodDirection.set(" +
|
||||
(strafePodReversed ? "Encoder.REVERSE" : "Encoder.FORWARD") +
|
||||
");\n" +
|
||||
" c.imu.set(new RevHubIMU(new RevHubOrientationOnRobot(\n" +
|
||||
" RevHubOrientationOnRobot.LogoFacingDirection." + values.logoDirection.name() + ",\n" +
|
||||
" RevHubOrientationOnRobot.UsbFacingDirection." + values.usbDirection.name() + "\n" +
|
||||
" )));\n" +
|
||||
"});"
|
||||
);
|
||||
}
|
||||
|
||||
static TwoWheelConfig config(
|
||||
TwoWheelSetup values,
|
||||
double forwardTicksToInches,
|
||||
double strafeTicksToInches,
|
||||
double xPodDirection,
|
||||
double yPodDirection,
|
||||
double xPodOffset,
|
||||
double yPodOffset
|
||||
) {
|
||||
return new TwoWheelConfig(c -> {
|
||||
c.xPodName.set(values.forwardPodName);
|
||||
c.yPodName.set(values.strafePodName);
|
||||
c.imuName.set(values.imuName);
|
||||
c.xPodOffset.set(xPodOffset);
|
||||
c.yPodOffset.set(yPodOffset);
|
||||
c.forwardTicksToInches.set(forwardTicksToInches);
|
||||
c.strafeTicksToInches.set(strafeTicksToInches);
|
||||
c.xPodDirection.set(xPodDirection);
|
||||
c.yPodDirection.set(yPodDirection);
|
||||
c.imu.set(new RevHubIMU(new RevHubOrientationOnRobot(values.logoDirection, values.usbDirection)));
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
class TwoWheelSetup {
|
||||
|
||||
String forwardPodName;
|
||||
String strafePodName;
|
||||
String imuName;
|
||||
RevHubOrientationOnRobot.LogoFacingDirection logoDirection;
|
||||
RevHubOrientationOnRobot.UsbFacingDirection usbDirection;
|
||||
|
||||
TwoWheelSetup(
|
||||
String forwardPodName,
|
||||
String strafePodName,
|
||||
String imuName,
|
||||
RevHubOrientationOnRobot.LogoFacingDirection logoDirection,
|
||||
RevHubOrientationOnRobot.UsbFacingDirection usbDirection
|
||||
) {
|
||||
this.forwardPodName = forwardPodName;
|
||||
this.strafePodName = strafePodName;
|
||||
this.imuName = imuName;
|
||||
this.logoDirection = logoDirection;
|
||||
this.usbDirection = usbDirection;
|
||||
}
|
||||
}
|
||||
|
||||
class TwoWheelForwardResolution extends TuningOpMode<Double> {
|
||||
|
||||
TwoWheelSetup values;
|
||||
double distance;
|
||||
|
||||
TwoWheelForwardResolution(TwoWheelSetup values, double distance) {
|
||||
super(
|
||||
"Forward Encoder Resolution Identification",
|
||||
"Push your robot forward " + distance + " inches exactly and then stop the Opmode",
|
||||
true
|
||||
);
|
||||
this.values = values;
|
||||
this.distance = distance;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Double runTuningOpMode() {
|
||||
TwoWheelConfig config = TwoWheelTuner.config(
|
||||
values,
|
||||
1.0,
|
||||
1.0,
|
||||
Encoder.FORWARD,
|
||||
Encoder.FORWARD,
|
||||
0.0,
|
||||
0.0
|
||||
);
|
||||
|
||||
TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
Pose position = null;
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
position = localizer.pose();
|
||||
telemetry.addData("heading", localizer.pose().heading());
|
||||
telemetry.addData("pose", localizer.pose());
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
if (position == null || position.x() == 0.0) {
|
||||
return null;
|
||||
}
|
||||
return Math.abs(position.x() / distance);
|
||||
}
|
||||
}
|
||||
|
||||
class TwoWheelStrafeResolution extends TuningOpMode<Double> {
|
||||
|
||||
TwoWheelSetup values;
|
||||
double distance;
|
||||
|
||||
TwoWheelStrafeResolution(TwoWheelSetup values, double distance) {
|
||||
super(
|
||||
"Strafe Encoder Resolution Identification",
|
||||
"Push your robot left " + distance + " inches exactly and then stop the Opmode",
|
||||
true
|
||||
);
|
||||
this.values = values;
|
||||
this.distance = distance;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Double runTuningOpMode() {
|
||||
TwoWheelConfig config = TwoWheelTuner.config(
|
||||
values,
|
||||
1.0,
|
||||
1.0,
|
||||
Encoder.FORWARD,
|
||||
Encoder.FORWARD,
|
||||
0.0,
|
||||
0.0
|
||||
);
|
||||
|
||||
TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
Pose position = null;
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
position = localizer.pose();
|
||||
}
|
||||
|
||||
if (position == null || position.y() == 0.0) {
|
||||
return null;
|
||||
}
|
||||
return Math.abs(position.y() / distance);
|
||||
}
|
||||
}
|
||||
|
||||
class TwoWheelForwardDirection extends TuningOpMode<Boolean> {
|
||||
|
||||
TwoWheelSetup values;
|
||||
double forwardTicksToInches;
|
||||
double strafeTicksToInches;
|
||||
|
||||
TwoWheelForwardDirection(TwoWheelSetup values, double forwardTicksToInches, double strafeTicksToInches) {
|
||||
super(
|
||||
"Forward Direction Identification",
|
||||
"Determines if your forward pod needs to be reversed.\n"
|
||||
+ "Push your robot forward and then stop the Opmode",
|
||||
true
|
||||
);
|
||||
this.values = values;
|
||||
this.forwardTicksToInches = forwardTicksToInches;
|
||||
this.strafeTicksToInches = strafeTicksToInches;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Boolean runTuningOpMode() {
|
||||
TwoWheelConfig config = TwoWheelTuner.config(
|
||||
values,
|
||||
forwardTicksToInches,
|
||||
strafeTicksToInches,
|
||||
Encoder.FORWARD,
|
||||
Encoder.FORWARD,
|
||||
0.0,
|
||||
0.0
|
||||
);
|
||||
|
||||
TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
}
|
||||
|
||||
return localizer.pose().x() < 0;
|
||||
}
|
||||
}
|
||||
|
||||
class TwoWheelStrafeDirection extends TuningOpMode<Boolean> {
|
||||
|
||||
TwoWheelSetup values;
|
||||
double forwardTicksToInches;
|
||||
double strafeTicksToInches;
|
||||
|
||||
TwoWheelStrafeDirection(TwoWheelSetup values, double forwardTicksToInches, double strafeTicksToInches) {
|
||||
super(
|
||||
"Strafe Direction Identification",
|
||||
"Determines if your strafe pod needs to be reversed.\n"
|
||||
+ "Push your robot left and then stop the Opmode",
|
||||
true
|
||||
);
|
||||
this.values = values;
|
||||
this.forwardTicksToInches = forwardTicksToInches;
|
||||
this.strafeTicksToInches = strafeTicksToInches;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected Boolean runTuningOpMode() {
|
||||
TwoWheelConfig config = TwoWheelTuner.config(
|
||||
values,
|
||||
forwardTicksToInches,
|
||||
strafeTicksToInches,
|
||||
Encoder.FORWARD,
|
||||
Encoder.FORWARD,
|
||||
0.0,
|
||||
0.0
|
||||
);
|
||||
|
||||
TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config);
|
||||
localizer.setPose(new Pose(0, 0));
|
||||
|
||||
waitForStart();
|
||||
while (!isStopRequested()) {
|
||||
localizer.update();
|
||||
}
|
||||
|
||||
return localizer.pose().y() < 0;
|
||||
}
|
||||
}
|
||||
|
||||
class TwoWheelOffsets extends TuningOpMode<List<Double>> {
|
||||
|
||||
TwoWheelSetup values;
|
||||
double forwardTicksToInches;
|
||||
double strafeTicksToInches;
|
||||
boolean forwardPodReversed;
|
||||
boolean strafePodReversed;
|
||||
Pose previous = Pose.zero();
|
||||
|
||||
TwoWheelOffsets(
|
||||
TwoWheelSetup values,
|
||||
double forwardTicksToInches,
|
||||
double strafeTicksToInches,
|
||||
boolean forwardPodReversed,
|
||||
boolean strafePodReversed
|
||||
) {
|
||||
super(
|
||||
"Two Wheel Offset Identification",
|
||||
"Automatically identifies the offsets for your Two Wheel localizer.\n"
|
||||
+ "Spin your robot in place 180 degrees counterclockwise and then stop the Opmode",
|
||||
true
|
||||
);
|
||||
this.values = values;
|
||||
this.forwardTicksToInches = forwardTicksToInches;
|
||||
this.strafeTicksToInches = strafeTicksToInches;
|
||||
this.forwardPodReversed = forwardPodReversed;
|
||||
this.strafePodReversed = strafePodReversed;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected List<Double> runTuningOpMode() {
|
||||
TwoWheelConfig config = TwoWheelTuner.config(
|
||||
values,
|
||||
forwardTicksToInches,
|
||||
strafeTicksToInches,
|
||||
forwardPodReversed ? Encoder.REVERSE : Encoder.FORWARD,
|
||||
strafePodReversed ? Encoder.REVERSE : Encoder.FORWARD,
|
||||
0.0,
|
||||
0.0
|
||||
);
|
||||
|
||||
TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config);
|
||||
localizer.setPose(Pose.zero());
|
||||
localizer.update();
|
||||
|
||||
waitForStart();
|
||||
|
||||
localizer.setPose(Pose.zero());
|
||||
|
||||
while (!isStopRequested()) {
|
||||
previous = localizer.pose();
|
||||
localizer.update();
|
||||
|
||||
telemetry.addData("heading", localizer.pose().heading());
|
||||
telemetry.addData("pose", localizer.pose());
|
||||
telemetry.addData("previous", previous);
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y()) {
|
||||
previous = localizer.pose();
|
||||
}
|
||||
|
||||
return List.of(((-previous.y()) / 2.0), ((-previous.x()) / 2.0));
|
||||
}
|
||||
}
|
||||
@@ -1,19 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode.pedroPathing;
|
||||
|
||||
import com.pedropathing.follower.Follower;
|
||||
import com.pedropathing.follower.FollowerConstants;
|
||||
import com.pedropathing.ftc.FollowerBuilder;
|
||||
import com.pedropathing.paths.PathConstraints;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
public class Constants {
|
||||
public static FollowerConstants followerConstants = new FollowerConstants();
|
||||
|
||||
public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
|
||||
|
||||
public static Follower createFollower(HardwareMap hardwareMap) {
|
||||
return new FollowerBuilder(followerConstants, hardwareMap)
|
||||
.pathConstraints(pathConstraints)
|
||||
.build();
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user