Show the superstructure sensor positions
Make a table of them.
Change-Id: I9481b6ab3f8ecd14763656c8083d5ab3f3e1c571
diff --git a/y2019/vision/server/BUILD b/y2019/vision/server/BUILD
index b194cde..35449a5 100644
--- a/y2019/vision/server/BUILD
+++ b/y2019/vision/server/BUILD
@@ -56,5 +56,6 @@
"//third_party/seasocks",
"//y2019:constants",
"//y2019/control_loops/drivetrain:camera_queue",
+ "//y2019/control_loops/superstructure:superstructure_queue",
],
)
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index e531f51..05a53e2 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -20,6 +20,7 @@
#include "seasocks/WebSocket.h"
#include "y2019/constants.h"
#include "y2019/control_loops/drivetrain/camera.q.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
#include "y2019/vision/server/server_data.pb.h"
namespace y2019 {
@@ -97,6 +98,8 @@
void DataThread(seasocks::Server *server, WebsocketHandler *websocket_handler) {
auto &camera_frames = y2019::control_loops::drivetrain::camera_frames;
auto &drivetrain_status = frc971::control_loops::drivetrain_queue.status;
+ auto &superstructure_status =
+ ::y2019::control_loops::superstructure::superstructure_queue.status;
std::array<LocalCameraFrame, 5> latest_frames;
std::array<aos::monotonic_clock::time_point, 5> last_target_time;
@@ -106,7 +109,8 @@
while (true) {
camera_frames.FetchNextBlocking();
drivetrain_status.FetchLatest();
- if (!drivetrain_status.get()) {
+ superstructure_status.FetchLatest();
+ if (!drivetrain_status.get() || !superstructure_status.get()) {
// Try again if we don't have any drivetrain statuses.
continue;
}
@@ -185,6 +189,13 @@
pose->set_theta(target_pose.abs_theta());
}
}
+
+ Sensors *sensors = debug_data.mutable_sensors();
+ sensors->set_wrist(superstructure_status->wrist.position);
+ sensors->set_elevator(superstructure_status->elevator.position);
+ sensors->set_intake(superstructure_status->intake.position);
+ sensors->set_stilts(superstructure_status->stilts.position);
+
::std::string json;
google::protobuf::util::MessageToJsonString(debug_data, &json);
server->execute(
diff --git a/y2019/vision/server/server_data.proto b/y2019/vision/server/server_data.proto
index d3e5665..96a5859 100644
--- a/y2019/vision/server/server_data.proto
+++ b/y2019/vision/server/server_data.proto
@@ -40,9 +40,19 @@
repeated Pose targets = 3;
}
+// Data for the current sensor values.
+message Sensors {
+ // Superstructure calibrated positions.
+ optional float wrist = 1;
+ optional float elevator = 2;
+ optional float intake = 3;
+ optional float stilts = 4;
+}
+
// The overall package of data that we send to the webpage.
message DebugData {
optional Pose robot_pose = 1;
optional LineFollowDebug line_follow_debug = 2;
repeated CameraDebug camera_debug = 3;
+ optional Sensors sensors = 4;
}
diff --git a/y2019/vision/server/www/index.html b/y2019/vision/server/www/index.html
index 4e6e316..c970ce1 100644
--- a/y2019/vision/server/www/index.html
+++ b/y2019/vision/server/www/index.html
@@ -2,9 +2,69 @@
<html>
<head>
<title>Vision Debug Server</title>
+ <style type="text/css">
+ * {margin: 0; padding: 0;}
+ #field {
+ float: left;
+ display: inline-block;
+ border: 1px solid;
+ }
+ #debugdata {
+ float: right;
+ display: inline-block;
+ }
+ .dof_container {
+ padding: 5px;
+ overflow: hidden;
+ }
+ .dof_name {
+ float: left;
+ padding-right: 15px;
+ display: inline-block;
+ }
+ .dof {
+ float: right;
+ display: inline-block;
+ }
+ </style>
</head>
<body style="overflow:hidden">
- <canvas id="field" style="border: 1px solid"></canvas>
+ <div style="overflow:hidden">
+ <canvas id="field"></canvas>
+ <div id="debugdata">
+ <div class="dof_container">
+ <div class="dof_name">
+ wrist
+ </div>
+ <div id="wrist" class="dof">
+ </div>
+ </div>
+
+ <div class="dof_container">
+ <div class="dof_name">
+ elevator
+ </div>
+ <div id="elevator" class="dof">
+ </div>
+ </div>
+
+ <div class="dof_container">
+ <div class="dof_name">
+ intake
+ </div>
+ <div id="intake" class="dof">
+ </div>
+ </div>
+
+ <div class="dof_container">
+ <div class="dof_name">
+ stilts
+ </div>
+ <div id="stilts" class="dof">
+ </div>
+ </div>
+ </div>
+ </div>
</body>
<script src="visualizer_bundle.min.js"></script>
</html>
diff --git a/y2019/vision/server/www/main.ts b/y2019/vision/server/www/main.ts
index ed06bc1..1f90527 100644
--- a/y2019/vision/server/www/main.ts
+++ b/y2019/vision/server/www/main.ts
@@ -18,8 +18,23 @@
private targetTheta = 0;
private cameraFrames : Frame[];
+ private wrist: number = -1;
+ private elevator: number = -1;
+ private intake: number = -1;
+ private stilts: number = -1;
+
+ private wrist_div: HTMLDivElement;
+ private elevator_div: HTMLDivElement;
+ private intake_div: HTMLDivElement;
+ private stilts_div: HTMLDivElement;
+
constructor() {
const canvas = <HTMLCanvasElement>document.getElementById('field');
+ this.wrist_div = <HTMLDivElement>document.getElementById('wrist');
+ this.elevator_div = <HTMLDivElement>document.getElementById('elevator');
+ this.intake_div = <HTMLDivElement>document.getElementById('intake');
+ this.stilts_div = <HTMLDivElement>document.getElementById('stilts');
+
const ctx = canvas.getContext('2d');
const server = location.host;
@@ -45,6 +60,11 @@
this.targetTheta = j.lineFollowDebug.goalTarget.theta;
}
this.cameraFrames = j.cameraDebug;
+
+ this.wrist = j.sensors.wrist;
+ this.elevator = j.sensors.elevator;
+ this.intake = j.sensors.intake;
+ this.stilts = j.sensors.stilts;
});
socket.addEventListener('close', (event) => {
setTimeout(() => {
@@ -66,6 +86,11 @@
const M_TO_PX = size / FIELD_WIDTH
ctx.scale(M_TO_PX, M_TO_PX);
ctx.lineWidth = 1 / M_TO_PX;
+
+ this.wrist_div.textContent = "";
+ this.elevator_div.textContent = "";
+ this.intake_div.textContent = "";
+ this.stilts_div.textContent = "";
}
draw(ctx: CanvasRenderingContext2D): void {
@@ -82,6 +107,13 @@
}
drawTarget(ctx, this.targetX, this.targetY, this.targetTheta);
ctx.restore();
+
+ // Now update the superstructure positions.
+ this.wrist_div.textContent = this.wrist.toFixed(3);
+ this.elevator_div.textContent = this.elevator.toFixed(3);
+ this.intake_div.textContent = this.intake.toFixed(3);
+ this.stilts_div.textContent = this.stilts.toFixed(3);
+
window.requestAnimationFrame(() => this.draw(ctx));
}
}