From 14100bed50c363caacd15888effb3dea813715c0 Mon Sep 17 00:00:00 2001 From: missing Date: Tue, 24 Jan 2023 09:42:13 -0600 Subject: [PATCH] change some stuff about dashboard drawing --- .../teamcode/DashboardTelemetryWrapper.java | 41 ++++++++++++++----- .../firstinspires/ftc/teamcode/Hardware.java | 6 +-- .../ftc/teamcode/kinematics/Poser.java | 4 +- 3 files changed, 35 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java index 1bb7226..481940a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java @@ -22,16 +22,33 @@ public class DashboardTelemetryWrapper implements Telemetry { } private void rawDrawRobot(FieldVector pos, double yaw) { - FieldVector topLeft = new FieldVector(Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH).div(2).rot(yaw).add(pos); - FieldVector topRight = new FieldVector(Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH).div(2).rot(yaw).add(pos); - FieldVector bottomLeft = new FieldVector(Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH.neg()).div(2).rot(yaw).add(pos); - FieldVector bottomRight = new FieldVector(Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH.neg()).div(2).rot(yaw).add(pos); + // 2--3 6--7 + // | | | | + // | 4----5 | + // | | + // | | + // 1----------8 + FieldVector[] ps = new FieldVector[]{ + new FieldVector( Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH.neg() ).div(2), + new FieldVector( Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH ).div(2), + new FieldVector( Distance.ROBOT_WIDTH.neg().div(2), Distance.ROBOT_LENGTH ).div(2), + new FieldVector( Distance.ROBOT_WIDTH.neg().div(2), Distance.ROBOT_LENGTH.div(3) ).div(2), + new FieldVector( Distance.ROBOT_WIDTH.div(2), Distance.ROBOT_LENGTH.div(3) ).div(2), + new FieldVector( Distance.ROBOT_WIDTH.div(2), Distance.ROBOT_LENGTH ).div(2), + new FieldVector( Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH ).div(2), + new FieldVector( Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH.neg() ).div(2) + }; + + double[] xs = new double[ps.length]; + double[] ys = new double[ps.length]; + for (int i = 0; i < ps.length; i ++) { + FieldVector p = ps[i].rot(yaw).add(pos); + xs[i] = p.x.valInInches(); + ys[i] = p.y.valInInches(); + } Canvas canvas = this.packet.fieldOverlay(); - canvas.fillPolygon( - new double[]{topLeft.x.valInInches(), topRight.x.valInInches(), bottomRight.x.valInInches(), bottomLeft.x.valInInches()}, - new double[]{topLeft.y.valInInches(), topRight.y.valInInches(), bottomRight.y.valInInches(), bottomLeft.y.valInInches()} - ); + canvas.fillPolygon(xs, ys); } public void drawRobot(FieldVector pos, double yaw) { @@ -43,19 +60,21 @@ public class DashboardTelemetryWrapper implements Telemetry { this.rawDrawRobot(pos, yaw); } - public void drawTargetOnDashboard(Pose target, Pose current) { + public void drawTarget(Pose target, Pose current) { Canvas canvas = this.packet.fieldOverlay(); canvas.setStrokeWidth(1); - canvas.setFill("transparent"); + canvas.setFill("green"); canvas.setStroke("green"); - canvas.fillCircle(target.pos.x.valInInches(), target.pos.y.valInInches(), 1); + canvas.fillCircle(target.pos.x.valInInches(), target.pos.y.valInInches(), 2); canvas.strokeLine( current.pos.x.valInInches(), current.pos.y.valInInches(), target.pos.x.valInInches(), target.pos.y.valInInches() ); + + canvas.setFill("transparent"); this.rawDrawRobot(target.pos, target.yaw); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java index b2adf1c..3706e98 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java @@ -17,7 +17,7 @@ import java.util.List; public class Hardware { public final OpMode opMode; - public final DashboardTelemetryWrapper dashboardTelemetryWrapper; + public final DashboardTelemetryWrapper dashboardTelemetry; public final DcMotor fl; public final DcMotor fr; public final DcMotor bl; @@ -32,8 +32,8 @@ public class Hardware { public Hardware(OpMode opMode) { HardwareMap hm = opMode.hardwareMap; this.opMode = opMode; - this.dashboardTelemetryWrapper = new DashboardTelemetryWrapper(FtcDashboard.getInstance()); - opMode.telemetry = new MultipleTelemetry(opMode.telemetry, this.dashboardTelemetryWrapper); + this.dashboardTelemetry = new DashboardTelemetryWrapper(FtcDashboard.getInstance()); + opMode.telemetry = new MultipleTelemetry(opMode.telemetry, this.dashboardTelemetry); this.fl = hm.get(DcMotor.class, "fl"); this.fr = hm.get(DcMotor.class, "fr"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java index a23f8d4..48bce90 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java @@ -101,8 +101,8 @@ public class Poser { telemetry.addData("yawPower", yawPower); telemetry.addData("liftPower", liftPower); telemetry.addData("loopsCorrect", loopsCorrect); - hardware.dashboardTelemetryWrapper.drawTargetOnDashboard(targetPose, currentPose); - hardware.dashboardTelemetryWrapper.drawRobot(currentPose.pos, currentPose.yaw); + hardware.dashboardTelemetry.drawTarget(targetPose, currentPose); + hardware.dashboardTelemetry.drawRobot(currentPose.pos, currentPose.yaw); telemetry.update(); posPower = posPower.rot(-currentPose.yaw);