SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Use RR FTC module

Browse Source
This commit is contained in:
Ryan Brott
2023-08-26 10:41:44 -07:00
parent 9fa28159da
commit e002d64689
6 changed files with 319 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
TeamCode/build.gradle
View File

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

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PoseMessage.java Normal file
View File

@ -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();
}
}

61
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java Normal file
View File

@ -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();
}
}
}

44
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java Normal file
View File

@ -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();
}
}
}

38
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java Normal file
View File

@ -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();
}
}
}

157
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java Normal file
View File

@ -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<? extends OpMode> 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<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
List<Encoder> 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<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
List<Encoder> 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);
}
}