change some stuff about dashboard drawing
This commit is contained in:
parent
c4e2385d0a
commit
14100bed50
|
@ -22,16 +22,33 @@ public class DashboardTelemetryWrapper implements Telemetry {
|
||||||
}
|
}
|
||||||
|
|
||||||
private void rawDrawRobot(FieldVector pos, double yaw) {
|
private void rawDrawRobot(FieldVector pos, double yaw) {
|
||||||
FieldVector topLeft = new FieldVector(Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH).div(2).rot(yaw).add(pos);
|
// 2--3 6--7
|
||||||
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);
|
// | 4----5 |
|
||||||
FieldVector bottomRight = new FieldVector(Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH.neg()).div(2).rot(yaw).add(pos);
|
// | |
|
||||||
|
// | |
|
||||||
|
// 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 canvas = this.packet.fieldOverlay();
|
||||||
canvas.fillPolygon(
|
canvas.fillPolygon(xs, ys);
|
||||||
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()}
|
|
||||||
);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public void drawRobot(FieldVector pos, double yaw) {
|
public void drawRobot(FieldVector pos, double yaw) {
|
||||||
|
@ -43,19 +60,21 @@ public class DashboardTelemetryWrapper implements Telemetry {
|
||||||
this.rawDrawRobot(pos, yaw);
|
this.rawDrawRobot(pos, yaw);
|
||||||
}
|
}
|
||||||
|
|
||||||
public void drawTargetOnDashboard(Pose target, Pose current) {
|
public void drawTarget(Pose target, Pose current) {
|
||||||
Canvas canvas = this.packet.fieldOverlay();
|
Canvas canvas = this.packet.fieldOverlay();
|
||||||
canvas.setStrokeWidth(1);
|
canvas.setStrokeWidth(1);
|
||||||
canvas.setFill("transparent");
|
canvas.setFill("green");
|
||||||
canvas.setStroke("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(
|
canvas.strokeLine(
|
||||||
current.pos.x.valInInches(),
|
current.pos.x.valInInches(),
|
||||||
current.pos.y.valInInches(),
|
current.pos.y.valInInches(),
|
||||||
target.pos.x.valInInches(),
|
target.pos.x.valInInches(),
|
||||||
target.pos.y.valInInches()
|
target.pos.y.valInInches()
|
||||||
);
|
);
|
||||||
|
|
||||||
|
canvas.setFill("transparent");
|
||||||
this.rawDrawRobot(target.pos, target.yaw);
|
this.rawDrawRobot(target.pos, target.yaw);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -17,7 +17,7 @@ import java.util.List;
|
||||||
|
|
||||||
public class Hardware {
|
public class Hardware {
|
||||||
public final OpMode opMode;
|
public final OpMode opMode;
|
||||||
public final DashboardTelemetryWrapper dashboardTelemetryWrapper;
|
public final DashboardTelemetryWrapper dashboardTelemetry;
|
||||||
public final DcMotor fl;
|
public final DcMotor fl;
|
||||||
public final DcMotor fr;
|
public final DcMotor fr;
|
||||||
public final DcMotor bl;
|
public final DcMotor bl;
|
||||||
|
@ -32,8 +32,8 @@ public class Hardware {
|
||||||
public Hardware(OpMode opMode) {
|
public Hardware(OpMode opMode) {
|
||||||
HardwareMap hm = opMode.hardwareMap;
|
HardwareMap hm = opMode.hardwareMap;
|
||||||
this.opMode = opMode;
|
this.opMode = opMode;
|
||||||
this.dashboardTelemetryWrapper = new DashboardTelemetryWrapper(FtcDashboard.getInstance());
|
this.dashboardTelemetry = new DashboardTelemetryWrapper(FtcDashboard.getInstance());
|
||||||
opMode.telemetry = new MultipleTelemetry(opMode.telemetry, this.dashboardTelemetryWrapper);
|
opMode.telemetry = new MultipleTelemetry(opMode.telemetry, this.dashboardTelemetry);
|
||||||
|
|
||||||
this.fl = hm.get(DcMotor.class, "fl");
|
this.fl = hm.get(DcMotor.class, "fl");
|
||||||
this.fr = hm.get(DcMotor.class, "fr");
|
this.fr = hm.get(DcMotor.class, "fr");
|
||||||
|
|
|
@ -101,8 +101,8 @@ public class Poser {
|
||||||
telemetry.addData("yawPower", yawPower);
|
telemetry.addData("yawPower", yawPower);
|
||||||
telemetry.addData("liftPower", liftPower);
|
telemetry.addData("liftPower", liftPower);
|
||||||
telemetry.addData("loopsCorrect", loopsCorrect);
|
telemetry.addData("loopsCorrect", loopsCorrect);
|
||||||
hardware.dashboardTelemetryWrapper.drawTargetOnDashboard(targetPose, currentPose);
|
hardware.dashboardTelemetry.drawTarget(targetPose, currentPose);
|
||||||
hardware.dashboardTelemetryWrapper.drawRobot(currentPose.pos, currentPose.yaw);
|
hardware.dashboardTelemetry.drawRobot(currentPose.pos, currentPose.yaw);
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
|
|
||||||
posPower = posPower.rot(-currentPose.yaw);
|
posPower = posPower.rot(-currentPose.yaw);
|
||||||
|
|
Loading…
Reference in a new issue