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) {
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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");
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in a new issue