saltylab-firmware/ui/vesc_panel.js

/**
 * vesc_panel.js — Saltybot VESC Motor Dashboard (Issue #653)
 *
 * Subscribes via rosbridge WebSocket to:
 *   /vesc/left/state    std_msgs/String (JSON) — left VESC telemetry
 *   /vesc/right/state   std_msgs/String (JSON) — right VESC telemetry
 *   /vesc/combined      std_msgs/String (JSON) — battery voltage + totals
 *
 * JSON fields per motor state:
 *   can_id, rpm, current_a, current_in_a, duty_cycle, voltage_v,
 *   temp_fet_c, temp_motor_c, fault_code, fault_name, alive, stamp
 *
 * JSON fields for combined:
 *   voltage_v, total_current_a, left_rpm, right_rpm,
 *   left_alive, right_alive, stamp
 *
 * E-stop publishes:
 *   /saltybot/emergency  std_msgs/String  — JSON estop event
 *   /cmd_vel             geometry_msgs/Twist — zero velocity
 */

'use strict';

// ── Thresholds ─────────────────────────────────────────────────────────────────

const RPM_MAX        = 8000;   // full-scale for arc gauge
const RPM_WARN       = 5600;   // 70%
const RPM_CRIT       = 7200;   // 90%

const CUR_MAX        = 60.0;   // A — overcurrent threshold from node params
const CUR_WARN       = 40.0;
const CUR_CRIT       = 54.0;

const TFET_WARN      = 70.0;   // °C
const TFET_CRIT      = 80.0;

const TMOT_WARN      = 85.0;   // °C
const TMOT_CRIT      = 100.0;

const VBATT_MIN      = 12.0;   // V — 4S LiPo empty
const VBATT_MAX      = 16.8;   // V — 4S LiPo full
const VBATT_WARN     = 13.2;   // ~15% SOC
const VBATT_CRIT     = 12.4;   // ~5% SOC

const DUTY_WARN      = 0.85;
const DUTY_CRIT      = 0.95;

// Sparkline: sample every 500ms, keep 120 points = 60s history
const SPARK_INTERVAL = 500;    // ms
const SPARK_MAX_PTS  = 120;

// Stale threshold: if no update in 2s, mark offline
const STALE_MS       = 2000;

// ── Colors ────────────────────────────────────────────────────────────────────

const C_GREEN  = '#22c55e';
const C_AMBER  = '#f59e0b';
const C_RED    = '#ef4444';
const C_CYAN   = '#06b6d4';
const C_DIM    = '#374151';
const C_MID    = '#6b7280';
const C_HI     = '#d1d5db';

function healthColor(val, warn, crit) {
  if (val >= crit) return C_RED;
  if (val >= warn) return C_AMBER;
  return C_GREEN;
}

// ── State ─────────────────────────────────────────────────────────────────────

const motors = {
  left:  { state: null, lastMs: 0, rpmHist: [], curHist: [], sparkTs: 0 },
  right: { state: null, lastMs: 0, rpmHist: [], curHist: [], sparkTs: 0 },
};
let combined      = null;
let combinedLastMs = 0;

let ros        = null;
let subLeft    = null;
let subRight   = null;
let subComb    = null;
let pubEmerg   = null;
let pubCmdVel  = null;

let msgCount   = 0;
let hzTs       = Date.now();
let hzCounter  = 0;
let staleTimer = null;

// ── DOM helpers ───────────────────────────────────────────────────────────────

function $(id)  { return document.getElementById(id); }

function setBar(id, pct, color) {
  const el = $(id);
  if (!el) return;
  el.style.width      = Math.min(100, Math.max(0, pct * 100)).toFixed(1) + '%';
  el.style.background = color;
}

function setText(id, text) {
  const el = $(id);
  if (el) el.textContent = text;
}

function setColor(id, color) {
  const el = $(id);
  if (el) el.style.color = color;
}

// ── Badge helpers ─────────────────────────────────────────────────────────────

function setBadge(id, text, cls) {
  const el = $(id);
  if (!el) return;
  el.textContent  = text;
  el.className    = 'sys-badge ' + cls;
}

function setFaultBadge(side, text, cls) {
  const el = $('fault-' + side);
  if (!el) return;
  el.textContent = text;
  el.className   = 'fault-badge ' + cls;
}

// ── Arc gauge (canvas) ────────────────────────────────────────────────────────
//
//  Standard dashboard semicircle: 225° → 315° going the "long way" (270° sweep)
//  In canvas coords (0 = right, clockwise):
//    startAngle = 0.75π  (135° → bottom-left area)
//    endAngle   = 2.25π  (405° = bottom-right area)

function drawArcGauge(canvasId, value, maxVal, color) {
  const canvas = $(canvasId);
  if (!canvas) return;

  // Match pixel size to CSS size
  const rect = canvas.getBoundingClientRect();
  if (rect.width  > 0) canvas.width  = rect.width  * devicePixelRatio;
  if (rect.height > 0) canvas.height = rect.height * devicePixelRatio;

  const ctx  = canvas.getContext('2d');
  const dpr  = devicePixelRatio || 1;
  const W    = canvas.width;
  const H    = canvas.height;
  const cx   = W / 2;
  const cy   = H * 0.62;
  const r    = Math.min(W, H) * 0.37;
  const lw   = Math.max(6, r * 0.18);

  const SA   = Math.PI * 0.75;  // start: 135°
  const EA   = Math.PI * 2.25;  // end:   405° (≡ 45°)
  const pct  = Math.min(1, Math.max(0, Math.abs(value) / maxVal));
  const VA   = SA + pct * (EA - SA);

  ctx.clearRect(0, 0, W, H);

  // Background arc
  ctx.beginPath();
  ctx.arc(cx, cy, r, SA, EA);
  ctx.strokeStyle = '#0c2a3a';
  ctx.lineWidth   = lw;
  ctx.lineCap     = 'butt';
  ctx.stroke();

  // Value arc
  if (pct > 0.003) {
    ctx.beginPath();
    ctx.arc(cx, cy, r, SA, VA);
    ctx.strokeStyle = color;
    ctx.lineWidth   = lw;
    ctx.lineCap     = 'round';
    ctx.stroke();
  }

  // Center RPM value
  ctx.fillStyle    = C_HI;
  ctx.font         = `bold ${Math.round(r * 0.44)}px "Courier New"`;
  ctx.textAlign    = 'center';
  ctx.textBaseline = 'middle';
  ctx.fillText(Math.abs(value).toLocaleString(), cx, cy);

  // "RPM" label
  ctx.fillStyle    = C_MID;
  ctx.font         = `${Math.round(r * 0.22)}px "Courier New"`;
  ctx.fillText('RPM', cx, cy + r * 0.48);
}

// ── Sparkline (canvas) ────────────────────────────────────────────────────────

function drawSparkline(canvasId, history, color, maxVal) {
  const canvas = $(canvasId);
  if (!canvas || history.length < 2) {
    if (canvas) {
      const ctx = canvas.getContext('2d');
      ctx.clearRect(0, 0, canvas.width, canvas.height);
    }
    return;
  }

  const rect = canvas.getBoundingClientRect();
  if (rect.width > 0) {
    canvas.width  = rect.width * devicePixelRatio;
    canvas.height = canvas.offsetHeight * devicePixelRatio;
  }

  const ctx  = canvas.getContext('2d');
  const W    = canvas.width;
  const H    = canvas.height;
  const pad  = 4;

  ctx.clearRect(0, 0, W, H);

  const n     = history.length;
  const scale = maxVal > 0 ? maxVal : 1;

  function ptX(i) { return pad + (i / (n - 1)) * (W - pad * 2); }
  function ptY(v) { return H - pad - (Math.max(0, Math.min(scale, v)) / scale) * (H - pad * 2); }

  // Fill
  ctx.beginPath();
  ctx.moveTo(ptX(0), H);
  for (let i = 0; i < n; i++) ctx.lineTo(ptX(i), ptY(history[i]));
  ctx.lineTo(ptX(n - 1), H);
  ctx.closePath();
  ctx.fillStyle = color + '28';
  ctx.fill();

  // Line
  ctx.beginPath();
  ctx.moveTo(ptX(0), ptY(history[0]));
  for (let i = 1; i < n; i++) ctx.lineTo(ptX(i), ptY(history[i]));
  ctx.strokeStyle = color;
  ctx.lineWidth   = 1.5 * devicePixelRatio;
  ctx.lineJoin    = 'round';
  ctx.stroke();
}

// ── History helpers ───────────────────────────────────────────────────────────

function pushHistory(arr, val) {
  arr.push(val);
  if (arr.length > SPARK_MAX_PTS) arr.shift();
}

// ── Motor rendering ───────────────────────────────────────────────────────────

function renderMotor(side, s) {
  const isAlive  = s && s.alive;
  const hasFault = isAlive && s.fault_code !== 0;

  const card = $('card-' + side);

  if (!isAlive) {
    setFaultBadge(side, 'OFFLINE', 'offline');
    setBadge('badge-' + side, 'OFFLINE', 'badge-stale');
    card.className = 'card motor-card state-offline';
    drawArcGauge('rpm-arc-' + side, 0, RPM_MAX, C_DIM);
    setText('dir-' + side, '—');
    setText('cur-' + side,   '—');
    setText('curin-' + side, '—');
    setText('duty-' + side,  '—');
    setBar('cur-bar-' + side,   0, C_DIM);
    setBar('curin-bar-' + side, 0, C_DIM);
    setBar('duty-bar-' + side,  0, C_DIM);
    setText('tfet-' + side,  '—');
    setText('tmot-' + side,  '—');
    setBar('tfet-bar-' + side, 0, C_DIM);
    setBar('tmot-bar-' + side, 0, C_DIM);
    $('tbox-fet-' + side).className = 'temp-box';
    $('tbox-mot-' + side).className = 'temp-box';
    return;
  }

  const rpm      = s.rpm;
  const cur      = Math.abs(s.current_a);
  const curIn    = Math.abs(s.current_in_a);
  const duty     = Math.abs(s.duty_cycle);
  const tfet     = s.temp_fet_c;
  const tmot     = s.temp_motor_c;
  const faultStr = hasFault ? s.fault_name.replace('FAULT_CODE_', '') : 'OK';

  // Overall card health
  const isWarn  = hasFault || cur >= CUR_WARN  || duty >= DUTY_WARN ||
                  tfet >= TFET_WARN || tmot >= TMOT_WARN;
  const isFault = hasFault || cur >= CUR_CRIT  || duty >= DUTY_CRIT ||
                  tfet >= TFET_CRIT || tmot >= TMOT_CRIT;

  if (isFault) {
    card.className = 'card motor-card state-fault';
    setBadge('badge-' + side, hasFault ? faultStr : 'CRIT', 'badge-error');
    setFaultBadge(side, faultStr, 'fault');
  } else if (isWarn) {
    card.className = 'card motor-card state-warn';
    setBadge('badge-' + side, 'WARN', 'badge-warn');
    setFaultBadge(side, 'WARN', 'warn');
  } else {
    card.className = 'card motor-card';
    setBadge('badge-' + side, 'OK', 'badge-ok');
    setFaultBadge(side, 'OK', '');
  }

  // RPM arc gauge
  const rpmColor = healthColor(Math.abs(rpm), RPM_WARN, RPM_CRIT);
  drawArcGauge('rpm-arc-' + side, rpm, RPM_MAX, rpmColor);

  // Direction indicator
  const dirEl = $('dir-' + side);
  if (Math.abs(rpm) < 30) {
    dirEl.textContent = 'STOP';
    dirEl.style.color = C_MID;
  } else if (rpm > 0) {
    dirEl.textContent = 'FWD ▲';
    dirEl.style.color = C_GREEN;
  } else {
    dirEl.textContent = '▼ REV';
    dirEl.style.color = C_AMBER;
  }

  // Current (motor)
  const curColor = healthColor(cur, CUR_WARN, CUR_CRIT);
  setText('cur-' + side, cur.toFixed(1) + ' A');
  setColor('cur-' + side, curColor);
  setBar('cur-bar-' + side, cur / CUR_MAX, curColor);

  // Current (input)
  const curInColor = healthColor(curIn, CUR_WARN * 0.8, CUR_CRIT * 0.8);
  setText('curin-' + side, curIn.toFixed(1) + ' A');
  setColor('curin-' + side, curInColor);
  setBar('curin-bar-' + side, curIn / CUR_MAX, curInColor);

  // Duty cycle
  const dutyColor = healthColor(duty, DUTY_WARN, DUTY_CRIT);
  setText('duty-' + side, (duty * 100).toFixed(1) + '%');
  setColor('duty-' + side, dutyColor);
  setBar('duty-bar-' + side, duty, dutyColor);

  // FET temperature
  const tfetColor = healthColor(tfet, TFET_WARN, TFET_CRIT);
  setText('tfet-' + side, tfet.toFixed(1) + '°');
  setColor('tfet-' + side, tfetColor);
  setBar('tfet-bar-' + side, tfet / TFET_CRIT, tfetColor);
  $('tbox-fet-' + side).className = 'temp-box' + (tfet >= TFET_CRIT ? ' crit' : tfet >= TFET_WARN ? ' warn' : '');

  // Motor temperature
  const tmotColor = healthColor(tmot, TMOT_WARN, TMOT_CRIT);
  setText('tmot-' + side, tmot.toFixed(1) + '°');
  setColor('tmot-' + side, tmotColor);
  setBar('tmot-bar-' + side, tmot / TMOT_CRIT, tmotColor);
  $('tbox-mot-' + side).className = 'temp-box' + (tmot >= TMOT_CRIT ? ' crit' : tmot >= TMOT_WARN ? ' warn' : '');

  // Sparklines
  const now = Date.now();
  const m   = motors[side];
  if (now - m.sparkTs >= SPARK_INTERVAL) {
    pushHistory(m.rpmHist, Math.abs(rpm));
    pushHistory(m.curHist, cur);
    m.sparkTs = now;
  }

  drawSparkline('spark-rpm-' + side, m.rpmHist, rpmColor, RPM_MAX);
  drawSparkline('spark-cur-' + side, m.curHist, curColor, CUR_MAX);
}

// ── Combined / battery rendering ──────────────────────────────────────────────

function renderCombined(c) {
  if (!c) return;

  const volt       = c.voltage_v;
  const totalCur   = c.total_current_a;
  const leftRpm    = c.left_rpm;
  const rightRpm   = c.right_rpm;

  // Voltage health
  const voltColor = volt <= VBATT_CRIT ? C_RED :
                    volt <= VBATT_WARN  ? C_AMBER : C_GREEN;
  const voltPct   = Math.max(0, Math.min(1, (volt - VBATT_MIN) / (VBATT_MAX - VBATT_MIN)));

  setText('batt-voltage', volt.toFixed(2));
  setColor('batt-voltage', voltColor);
  setText('batt-volt-pct', (voltPct * 100).toFixed(0) + '%');
  setBar('batt-volt-bar', voltPct, voltColor);

  // Total current
  const curColor = healthColor(totalCur, CUR_MAX * 0.8, CUR_MAX * 1.1);
  setText('batt-total-cur', totalCur.toFixed(1));
  setColor('batt-total-cur', curColor);
  setText('batt-cur-pct', totalCur.toFixed(1) + ' A');
  setBar('batt-cur-bar', totalCur / (CUR_MAX * 2), curColor);

  // RPM summary
  setText('batt-rpm-l', Math.abs(leftRpm).toLocaleString());
  setText('batt-rpm-r', Math.abs(rightRpm).toLocaleString());

  // Battery status badge
  if (volt <= VBATT_CRIT) {
    setBadge('badge-batt', volt.toFixed(2) + ' V', 'badge-error');
  } else if (volt <= VBATT_WARN) {
    setBadge('badge-batt', volt.toFixed(2) + ' V', 'badge-warn');
  } else {
    setBadge('badge-batt', volt.toFixed(2) + ' V', 'badge-ok');
  }

  setBadge('badge-total', totalCur.toFixed(1) + ' A', curColor === C_RED ? 'badge-error' : curColor === C_AMBER ? 'badge-warn' : 'badge-ok');

  const d = new Date(c.stamp * 1000);
  setText('batt-stamp', d.toLocaleTimeString('en-US', { hour12: false }));
}

// ── ROS connection ────────────────────────────────────────────────────────────

function connect() {
  const url = $('ws-input').value.trim();
  if (!url) return;
  if (ros) { try { ros.close(); } catch (_) {} }

  ros = new ROSLIB.Ros({ url });

  ros.on('connection', () => {
    $('conn-dot').className   = 'connected';
    $('conn-label').textContent = url;
    $('btn-connect').textContent = 'RECONNECT';
    setupSubs();
    setupPubs();
  });

  ros.on('error', (err) => {
    $('conn-dot').className   = 'error';
    $('conn-label').textContent = 'ERROR: ' + (err?.message || err);
    teardown();
  });

  ros.on('close', () => {
    $('conn-dot').className   = '';
    $('conn-label').textContent = 'Disconnected';
    teardown();
  });
}

function setupSubs() {
  subLeft = new ROSLIB.Topic({
    ros, name: '/vesc/left/state',
    messageType: 'std_msgs/String',
    throttle_rate: 100,
  });
  subLeft.subscribe((msg) => {
    try {
      const s = JSON.parse(msg.data);
      motors.left.state  = s;
      motors.left.lastMs = Date.now();
      renderMotor('left', s);
      tickHz();
    } catch (_) {}
  });

  subRight = new ROSLIB.Topic({
    ros, name: '/vesc/right/state',
    messageType: 'std_msgs/String',
    throttle_rate: 100,
  });
  subRight.subscribe((msg) => {
    try {
      const s = JSON.parse(msg.data);
      motors.right.state  = s;
      motors.right.lastMs = Date.now();
      renderMotor('right', s);
      tickHz();
    } catch (_) {}
  });

  subComb = new ROSLIB.Topic({
    ros, name: '/vesc/combined',
    messageType: 'std_msgs/String',
    throttle_rate: 100,
  });
  subComb.subscribe((msg) => {
    try {
      combined       = JSON.parse(msg.data);
      combinedLastMs = Date.now();
      renderCombined(combined);
    } catch (_) {}
  });

  // Stale check every second
  if (staleTimer) clearInterval(staleTimer);
  staleTimer = setInterval(checkStale, 1000);
}

function setupPubs() {
  pubEmerg = new ROSLIB.Topic({
    ros, name: '/saltybot/emergency',
    messageType: 'std_msgs/String',
  });
  pubCmdVel = new ROSLIB.Topic({
    ros, name: '/cmd_vel',
    messageType: 'geometry_msgs/Twist',
  });
}

function teardown() {
  if (subLeft)  { subLeft.unsubscribe();  subLeft  = null; }
  if (subRight) { subRight.unsubscribe(); subRight = null; }
  if (subComb)  { subComb.unsubscribe();  subComb  = null; }
  if (staleTimer) { clearInterval(staleTimer); staleTimer = null; }
  pubEmerg  = null;
  pubCmdVel = null;
  motors.left.state  = null;
  motors.right.state = null;
  renderMotor('left',  null);
  renderMotor('right', null);
}

// ── Stale detection ───────────────────────────────────────────────────────────

function checkStale() {
  const now = Date.now();
  if (motors.left.lastMs  && now - motors.left.lastMs  > STALE_MS) {
    motors.left.state  = null;
    renderMotor('left', null);
  }
  if (motors.right.lastMs && now - motors.right.lastMs > STALE_MS) {
    motors.right.state = null;
    renderMotor('right', null);
  }
}

// ── Hz counter ────────────────────────────────────────────────────────────────

function tickHz() {
  hzCounter++;
  const now    = Date.now();
  const elapsedSec = (now - hzTs) / 1000;
  if (elapsedSec >= 1.0) {
    const hz = (hzCounter / elapsedSec).toFixed(1);
    setText('hz-label', hz + ' Hz');
    setText('stamp-label', new Date().toLocaleTimeString('en-US', { hour12: false }));
    hzCounter = 0;
    hzTs      = now;
  }
}

// ── E-stop ────────────────────────────────────────────────────────────────────

function fireEstop() {
  const btn = $('btn-estop');
  btn.classList.add('fired');
  setTimeout(() => btn.classList.remove('fired'), 2000);

  // Zero velocity
  if (pubCmdVel) {
    pubCmdVel.publish(new ROSLIB.Message({
      linear:  { x: 0.0, y: 0.0, z: 0.0 },
      angular: { x: 0.0, y: 0.0, z: 0.0 },
    }));
  }

  // Emergency event
  if (pubEmerg) {
    pubEmerg.publish(new ROSLIB.Message({
      data: JSON.stringify({
        type:      'ESTOP',
        source:    'vesc_panel',
        timestamp: Date.now() / 1000,
        msg:       'E-stop triggered from VESC motor dashboard (issue #653)',
      }),
    }));
  }
}

// ── Event wiring ──────────────────────────────────────────────────────────────

$('btn-connect').addEventListener('click', connect);
$('ws-input').addEventListener('keydown', (e) => { if (e.key === 'Enter') connect(); });
$('btn-estop').addEventListener('click', fireEstop);

// ── Init ──────────────────────────────────────────────────────────────────────

const stored = localStorage.getItem('vesc_panel_ws');
if (stored) $('ws-input').value = stored;
$('ws-input').addEventListener('change', (e) => {
  localStorage.setItem('vesc_panel_ws', e.target.value);
});

// Initial empty state render
renderMotor('left',  null);
renderMotor('right', null);