missing/power-play
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

change some stuff about dashboard drawing

Browse source
This commit is contained in:
missing 2023-01-24 09:42:13 -06:00
parent c4e2385d0a
commit 14100bed50
3 changed files with 35 additions and 16 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

41
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java
View file

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

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java
View file

@ -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");

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java
View file

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