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

Draw robot in LocalizationTest (fixes #360)

Browse Source
This commit is contained in:
Ryan Brott
2024-01-30 22:10:22 -08:00
parent c6ef075c48
commit 01d55d76f4
3 changed files with 24 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View File

@ -21,7 +21,6 @@ import com.acmerobotics.roadrunner.TimeTurn;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TurnConstraints; import com.acmerobotics.roadrunner.TurnConstraints;
import com.acmerobotics.roadrunner.Twist2dDual; import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.VelConstraint; import com.acmerobotics.roadrunner.VelConstraint;
import com.acmerobotics.roadrunner.ftc.DownsampledWriter; import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
import com.acmerobotics.roadrunner.ftc.Encoder; import com.acmerobotics.roadrunner.ftc.Encoder;
@ -325,10 +324,10 @@ public final class MecanumDrive {
drawPoseHistory(c); drawPoseHistory(c);
c.setStroke("#4CAF50"); c.setStroke("#4CAF50");
drawRobot(c, txWorldTarget.value()); Drawing.drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5"); c.setStroke("#3F51B5");
drawRobot(c, pose); Drawing.drawRobot(c, pose);
c.setStroke("#4CAF50FF"); c.setStroke("#4CAF50FF");
c.setStrokeWidth(1); c.setStrokeWidth(1);
@ -406,10 +405,10 @@ public final class MecanumDrive {
drawPoseHistory(c); drawPoseHistory(c);
c.setStroke("#4CAF50"); c.setStroke("#4CAF50");
drawRobot(c, txWorldTarget.value()); Drawing.drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5"); c.setStroke("#3F51B5");
drawRobot(c, pose); Drawing.drawRobot(c, pose);
c.setStroke("#7C4DFFFF"); c.setStroke("#7C4DFFFF");
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2); c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
@ -455,18 +454,6 @@ public final class MecanumDrive {
c.strokePolyline(xPoints, yPoints); 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) { public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
return new TrajectoryActionBuilder( return new TrajectoryActionBuilder(
TurnAction::new, TurnAction::new,

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
View File

@ -343,10 +343,10 @@ public final class TankDrive {
drawPoseHistory(c); drawPoseHistory(c);
c.setStroke("#4CAF50"); c.setStroke("#4CAF50");
drawRobot(c, txWorldTarget.value()); Drawing.drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5"); c.setStroke("#3F51B5");
drawRobot(c, pose); Drawing.drawRobot(c, pose);
c.setStroke("#4CAF50FF"); c.setStroke("#4CAF50FF");
c.setStrokeWidth(1); c.setStrokeWidth(1);
@ -426,10 +426,10 @@ public final class TankDrive {
drawPoseHistory(c); drawPoseHistory(c);
c.setStroke("#4CAF50"); c.setStroke("#4CAF50");
drawRobot(c, txWorldTarget.value()); Drawing.drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5"); c.setStroke("#3F51B5");
drawRobot(c, pose); Drawing.drawRobot(c, pose);
c.setStroke("#7C4DFFFF"); c.setStroke("#7C4DFFFF");
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2); c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
@ -475,18 +475,6 @@ public final class TankDrive {
c.strokePolyline(xPoints, yPoints); 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) { public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
return new TrajectoryActionBuilder( return new TrajectoryActionBuilder(
TurnAction::new, TurnAction::new,

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
View File

@ -1,17 +1,22 @@
package org.firstinspires.ftc.teamcode.tuning; 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.Pose2d;
import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.Drawing;
import org.firstinspires.ftc.teamcode.MecanumDrive; import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive; import org.firstinspires.ftc.teamcode.TankDrive;
public class LocalizationTest extends LinearOpMode { public class LocalizationTest extends LinearOpMode {
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
@ -32,6 +37,11 @@ public class LocalizationTest extends LinearOpMode {
telemetry.addData("y", drive.pose.position.y); telemetry.addData("y", drive.pose.position.y);
telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble())); telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble()));
telemetry.update(); 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)) { } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
@ -53,6 +63,11 @@ public class LocalizationTest extends LinearOpMode {
telemetry.addData("y", drive.pose.position.y); telemetry.addData("y", drive.pose.position.y);
telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble())); telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble()));
telemetry.update(); telemetry.update();
TelemetryPacket packet = new TelemetryPacket();
packet.fieldOverlay().setStroke("#3F51B5");
Drawing.drawRobot(packet.fieldOverlay(), drive.pose);
FtcDashboard.getInstance().sendTelemetryPacket(packet);
} }
} else { } else {
throw new RuntimeException(); throw new RuntimeException();