diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index fb49198ecfe0..d3a8f1bbae00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -21,7 +21,6 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TurnConstraints; import com.acmerobotics.roadrunner.Twist2dDual; -import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.VelConstraint; import com.acmerobotics.roadrunner.ftc.DownsampledWriter; import com.acmerobotics.roadrunner.ftc.Encoder; @@ -325,10 +324,10 @@ public boolean run(@NonNull TelemetryPacket p) { drawPoseHistory(c); c.setStroke("#4CAF50"); - drawRobot(c, txWorldTarget.value()); + Drawing.drawRobot(c, txWorldTarget.value()); c.setStroke("#3F51B5"); - drawRobot(c, pose); + Drawing.drawRobot(c, pose); c.setStroke("#4CAF50FF"); c.setStrokeWidth(1); @@ -406,10 +405,10 @@ public boolean run(@NonNull TelemetryPacket p) { drawPoseHistory(c); c.setStroke("#4CAF50"); - drawRobot(c, txWorldTarget.value()); + Drawing.drawRobot(c, txWorldTarget.value()); c.setStroke("#3F51B5"); - drawRobot(c, pose); + Drawing.drawRobot(c, pose); c.setStroke("#7C4DFFFF"); c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2); @@ -455,18 +454,6 @@ private void drawPoseHistory(Canvas c) { c.strokePolyline(xPoints, yPoints); } - private static void drawRobot(Canvas c, Pose2d t) { - final double ROBOT_RADIUS = 9; - - c.setStrokeWidth(1); - c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS); - - Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS); - Vector2d p1 = t.position.plus(halfv); - Vector2d p2 = p1.plus(halfv); - c.strokeLine(p1.x, p1.y, p2.x, p2.y); - } - public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) { return new TrajectoryActionBuilder( TurnAction::new, diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java index 589a965a4f7c..9cc112f235e0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java @@ -343,10 +343,10 @@ public boolean run(@NonNull TelemetryPacket p) { drawPoseHistory(c); c.setStroke("#4CAF50"); - drawRobot(c, txWorldTarget.value()); + Drawing.drawRobot(c, txWorldTarget.value()); c.setStroke("#3F51B5"); - drawRobot(c, pose); + Drawing.drawRobot(c, pose); c.setStroke("#4CAF50FF"); c.setStrokeWidth(1); @@ -426,10 +426,10 @@ public boolean run(@NonNull TelemetryPacket p) { drawPoseHistory(c); c.setStroke("#4CAF50"); - drawRobot(c, txWorldTarget.value()); + Drawing.drawRobot(c, txWorldTarget.value()); c.setStroke("#3F51B5"); - drawRobot(c, pose); + Drawing.drawRobot(c, pose); c.setStroke("#7C4DFFFF"); c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2); @@ -475,18 +475,6 @@ private void drawPoseHistory(Canvas c) { c.strokePolyline(xPoints, yPoints); } - private static void drawRobot(Canvas c, Pose2d t) { - final double ROBOT_RADIUS = 9; - - c.setStrokeWidth(1); - c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS); - - Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS); - Vector2d p1 = t.position.plus(halfv); - Vector2d p2 = p1.plus(halfv); - c.strokeLine(p1.x, p1.y, p2.x, p2.y); - } - public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) { return new TrajectoryActionBuilder( TurnAction::new, 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 index ea7618dbc1d4..803ea158c950 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java @@ -1,17 +1,22 @@ package org.firstinspires.ftc.teamcode.tuning; +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; 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.Drawing; import org.firstinspires.ftc.teamcode.MecanumDrive; import org.firstinspires.ftc.teamcode.TankDrive; public class LocalizationTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); @@ -32,6 +37,11 @@ public void runOpMode() throws InterruptedException { telemetry.addData("y", drive.pose.position.y); telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble())); telemetry.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), drive.pose); + FtcDashboard.getInstance().sendTelemetryPacket(packet); } } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); @@ -53,6 +63,11 @@ public void runOpMode() throws InterruptedException { telemetry.addData("y", drive.pose.position.y); telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble())); telemetry.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), drive.pose); + FtcDashboard.getInstance().sendTelemetryPacket(packet); } } else { throw new RuntimeException();