From e002d6468959326152dd880a1254475d99f89fdc Mon Sep 17 00:00:00 2001 From: Ryan Brott Date: Sat, 26 Aug 2023 10:41:44 -0700 Subject: [PATCH] Use RR FTC module --- TeamCode/build.gradle | 9 +- .../ftc/teamcode/PoseMessage.java | 18 ++ .../ftc/teamcode/tuning/LocalizationTest.java | 61 +++++++ .../teamcode/tuning/ManualFeedbackTuner.java | 44 +++++ .../ftc/teamcode/tuning/SplineTest.java | 38 +++++ .../ftc/teamcode/tuning/TuningOpModes.java | 157 ++++++++++++++++++ 6 files changed, 319 insertions(+), 8 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PoseMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index fee9e96..5ddfccf 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -27,18 +27,11 @@ repositories { maven { url = 'https://maven.brott.dev/' } - mavenLocal() } dependencies { implementation project(':FtcRobotController') annotationProcessor files('lib/OpModeAnnotationProcessor.jar') - implementation 'com.acmerobotics.dashboard:dashboard:0.4.9' - implementation 'com.acmerobotics.roadrunner:core:1.0.0-beta2' - implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta2' - - implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' - - implementation "com.acmerobotics.roadrunner:ftc:0.1.0-SNAPSHOT" + implementation "com.acmerobotics.roadrunner:ftc:0.1.0" } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PoseMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PoseMessage.java new file mode 100644 index 0000000..b3fcf3e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PoseMessage.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.roadrunner.Pose2d; + +public final class PoseMessage { + public long timestamp; + public double x; + public double y; + public double heading; + + public PoseMessage(Pose2d pose) { + this.timestamp = System.nanoTime(); + this.x = pose.position.x; + this.y = pose.position.y; + this.heading = pose.heading.log(); + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java new file mode 100644 index 0000000..a5b5bbc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java @@ -0,0 +1,61 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; +import com.acmerobotics.roadrunner.Twist2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.firstinspires.ftc.teamcode.TankDrive; + +public class LocalizationTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + while (opModeIsActive()) { + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x + ), + -gamepad1.right_stick_x + )); + + drive.updatePoseEstimate(); + + telemetry.addData("x", drive.pose.position.x); + telemetry.addData("y", drive.pose.position.y); + telemetry.addData("heading", drive.pose.heading); + telemetry.update(); + } + } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { + TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + while (opModeIsActive()) { + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y, + 0.0 + ), + -gamepad1.right_stick_x + )); + + drive.updatePoseEstimate(); + + telemetry.addData("x", drive.pose.position.x); + telemetry.addData("y", drive.pose.position.y); + telemetry.addData("heading", drive.pose.heading); + telemetry.update(); + } + } else { + throw new AssertionError(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java new file mode 100644 index 0000000..d1efe81 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.roadrunner.Pose2d; +// TODO: remove Actions from the core module? +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.firstinspires.ftc.teamcode.TankDrive; + +public final class ManualFeedbackTuner extends LinearOpMode { + public static double DISTANCE = 64; + + @Override + public void runOpMode() throws InterruptedException { + if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + while (opModeIsActive()) { + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .lineToX(DISTANCE) + .lineToX(0) + .build()); + } + } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { + TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + while (opModeIsActive()) { + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .lineToX(DISTANCE) + .lineToX(0) + .build()); + } + } else { + throw new AssertionError(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java new file mode 100644 index 0000000..7dc5822 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.firstinspires.ftc.teamcode.TankDrive; + +public final class SplineTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .splineTo(new Vector2d(30, 30), Math.PI / 2) + .splineTo(new Vector2d(60, 0), Math.PI) + .build()); + } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { + TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .splineTo(new Vector2d(30, 30), Math.PI / 2) + .splineTo(new Vector2d(60, 0), Math.PI) + .build()); + } else { + throw new AssertionError(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java new file mode 100644 index 0000000..23ea53e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java @@ -0,0 +1,157 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.ftc.AngularRampLogger; +import com.acmerobotics.roadrunner.ftc.DriveType; +import com.acmerobotics.roadrunner.ftc.DriveView; +import com.acmerobotics.roadrunner.ftc.DriveViewFactory; +import com.acmerobotics.roadrunner.ftc.Encoder; +import com.acmerobotics.roadrunner.ftc.ForwardPushTest; +import com.acmerobotics.roadrunner.ftc.ForwardRampLogger; +import com.acmerobotics.roadrunner.ftc.LateralPushTest; +import com.acmerobotics.roadrunner.ftc.LateralRampLogger; +import com.acmerobotics.roadrunner.ftc.ManualFeedforwardTuner; +import com.acmerobotics.roadrunner.ftc.MecanumMotorDirectionDebugger; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.OpModeManager; +import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar; + +import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta; +import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.firstinspires.ftc.teamcode.TankDrive; +import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer; +import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public final class TuningOpModes { + public static final Class DRIVE_CLASS = MecanumDrive.class; + + public static final String GROUP = "quickstart"; + public static final boolean DISABLED = false; + + private TuningOpModes() {} + + private static OpModeMeta metaForClass(Class cls) { + return new OpModeMeta.Builder() + .setName(cls.getSimpleName()) + .setGroup(GROUP) + .setFlavor(OpModeMeta.Flavor.TELEOP) + .build(); + } + + @OpModeRegistrar + public static void register(OpModeManager manager) { + if (DISABLED) return; + + DriveViewFactory dvf; + if (DRIVE_CLASS.equals(MecanumDrive.class)) { + dvf = hardwareMap -> { + MecanumDrive md = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + List leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>(); + List parEncs = new ArrayList<>(), perpEncs = new ArrayList<>(); + if (md.localizer instanceof MecanumDrive.DriveLocalizer) { + MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) md.localizer; + leftEncs.add(dl.leftFront); + leftEncs.add(dl.leftRear); + rightEncs.add(dl.rightFront); + rightEncs.add(dl.rightRear); + } else if (md.localizer instanceof ThreeDeadWheelLocalizer) { + ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) md.localizer; + parEncs.add(dl.par0); + parEncs.add(dl.par1); + perpEncs.add(dl.perp); + } else if (md.localizer instanceof TwoDeadWheelLocalizer) { + TwoDeadWheelLocalizer dl = (TwoDeadWheelLocalizer) md.localizer; + parEncs.add(dl.par); + perpEncs.add(dl.perp); + } else { + throw new IllegalArgumentException("unknown localizer: " + md.localizer.getClass().getName()); + } + + return new DriveView( + DriveType.MECANUM, + MecanumDrive.PARAMS.inPerTick, + MecanumDrive.PARAMS.maxWheelVel, + MecanumDrive.PARAMS.minProfileAccel, + MecanumDrive.PARAMS.maxProfileAccel, + hardwareMap.getAll(LynxModule.class), + Arrays.asList( + md.leftFront, + md.leftBack + ), + Arrays.asList( + md.rightFront, + md.rightBack + ), + leftEncs, + rightEncs, + parEncs, + perpEncs, + md.imu, + md.voltageSensor, + md.feedforward + ); + }; + } else if (DRIVE_CLASS.equals(TankDrive.class)) { + dvf = hardwareMap -> { + TankDrive td = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); + + List leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>(); + List parEncs = new ArrayList<>(), perpEncs = new ArrayList<>(); + if (td.localizer instanceof TankDrive.DriveLocalizer) { + TankDrive.DriveLocalizer dl = (TankDrive.DriveLocalizer) td.localizer; + leftEncs.addAll(dl.leftEncs); + rightEncs.addAll(dl.rightEncs); + } else if (td.localizer instanceof ThreeDeadWheelLocalizer) { + ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) td.localizer; + parEncs.add(dl.par0); + parEncs.add(dl.par1); + perpEncs.add(dl.perp); + } else if (td.localizer instanceof TwoDeadWheelLocalizer) { + TwoDeadWheelLocalizer dl = (TwoDeadWheelLocalizer) td.localizer; + parEncs.add(dl.par); + perpEncs.add(dl.perp); + } else { + throw new IllegalArgumentException("unknown localizer: " + td.localizer.getClass().getName()); + } + + return new DriveView( + DriveType.TANK, + TankDrive.PARAMS.inPerTick, + TankDrive.PARAMS.maxWheelVel, + TankDrive.PARAMS.minProfileAccel, + TankDrive.PARAMS.maxProfileAccel, + hardwareMap.getAll(LynxModule.class), + td.leftMotors, + td.rightMotors, + leftEncs, + rightEncs, + parEncs, + perpEncs, + td.imu, + td.voltageSensor, + td.feedforward + ); + }; + } else { + throw new AssertionError(); + } + + manager.register(metaForClass(AngularRampLogger.class), new AngularRampLogger(dvf)); + manager.register(metaForClass(ForwardPushTest.class), new ForwardPushTest(dvf)); + manager.register(metaForClass(ForwardRampLogger.class), new ForwardRampLogger(dvf)); + manager.register(metaForClass(LateralPushTest.class), new LateralPushTest(dvf)); + manager.register(metaForClass(LateralRampLogger.class), new LateralRampLogger(dvf)); + manager.register(metaForClass(ManualFeedforwardTuner.class), new ManualFeedforwardTuner(dvf)); + manager.register(metaForClass(MecanumMotorDirectionDebugger.class), new MecanumMotorDirectionDebugger(dvf)); + + manager.register(metaForClass(ManualFeedbackTuner.class), ManualFeedbackTuner.class); + manager.register(metaForClass(SplineTest.class), SplineTest.class); + manager.register(metaForClass(LocalizationTest.class), LocalizationTest.class); + } +}