Calibration supercharged

.gitignore

@@ -5,3 +5,4 @@ sim_results/
 sim_results_multi/
 tuningTrials/
 # RL_Trials/
+venv/

A0Calibration/A0CalibrationSketch/A0CalibrationSketch.ino

@@ -0,0 +1,152 @@
+// A0CalibrationSketch
+// ────────────────────────────────────────────────────────────
+// Trimmed-down calibration streamer. Continuously outputs
+// "<mm>, <a0_raw>" lines over serial, where <mm> is the
+// position measured by one of the two known sensors (A2, A3)
+// and <a0_raw> is the raw ADC count from the unknown sensor
+// on A0. A companion Python notebook buckets these points
+// into 0.05mm intervals and exports an Excel calibration.
+//
+// Handshake protocol (restartable without re-uploading):
+//   Wake byte : 'S'  Python -> Arduino  (start/restart streaming)
+//   Stop byte : 'X'  Python -> Arduino  (stop streaming, return to idle)
+//
+//   Idle state  : Arduino waits for 'S', ignores all other bytes.
+//   Stream state: Arduino emits data lines, watches for 'X'.
+//                 On 'X' it returns to idle state.
+//
+//   Python connect sequence (works regardless of current Arduino state):
+//     1. Send 'X'             (stops streaming if running; no-op if idle)
+//     2. sleep 200 ms + flush (drain any in-flight data lines)
+//     3. Send 'S'             (Arduino: 1 s settle, then prints #READY)
+//     4. Wait for '#READY'
+//
+//   Data lines: <float_mm>, <int_a0_raw>\n  — no other output ever.
+// ────────────────────────────────────────────────────────────
+
+#include <Arduino.h>
+#include <util/atomic.h>
+
+// ── ADC Interrupt-driven 3-channel read (A2, A3, A0) ─────────
+// Channel index: 0 → A2 (sensor 0), 1 → A3 (sensor 1), 2 → A0 (unknown)
+static const uint8_t adc_mux[3] = {2, 3, 1};
+
+volatile uint16_t adc_result[3] = {0, 0, 0};
+volatile bool     adc_ready[3]  = {false, false, false};
+volatile uint8_t  adc_channel   = 0;
+
+void setupADC() {
+  ADMUX = (1 << REFS0) | adc_mux[0];   // AVCC ref, start on A2
+  ADCSRA = (1 << ADEN) | (1 << ADIE) | (1 << ADPS2);  // /16 prescaler
+  ADCSRA |= (1 << ADSC);
+}
+
+// ── OOR digital inputs ───────────────────────────────────────
+#define OOR_PIN_0  12    // HIGH = out of range, sensor 0 (A2)
+#define OOR_PIN_1  13    // HIGH = out of range, sensor 1 (A3)
+
+volatile bool OOR[2];
+
+ISR(ADC_vect) {
+  uint16_t sample = ADC;
+  uint8_t ch   = adc_channel;
+  uint8_t next = (ch + 1) % 3;
+
+  if (ch < 2) {
+    OOR[ch] = digitalRead(ch == 0 ? OOR_PIN_0 : OOR_PIN_1);
+    if (!OOR[ch]) {
+      adc_result[ch] = sample;
+      adc_ready[ch]  = true;
+    }
+  } else {
+    // A0: no OOR, always store
+    adc_result[2] = sample;
+    adc_ready[2]  = true;
+  }
+
+  ADMUX = (ADMUX & 0xF0) | adc_mux[next];
+  adc_channel = next;
+  ADCSRA |= (1 << ADSC);
+}
+
+// ── ADC → mm linear mappings (raw range: 16–26 mm) ──────────
+// Kept identical to AltSensorTesting.ino so calibration is
+// performed in the same position frame.
+#define adcToMM0(adc) ((float)map(adc, 178, 895, 1600, 2600) / 100.0f)
+#define adcToMM1(adc) ((float)map(adc, 176, 885, 1600, 2600) / 100.0f)
+
+// Mounting offsets so sensor 0 → 0–10 mm, sensor 1 → 10–20 mm
+#define OFFSET_MM0  15.6f
+#define OFFSET_MM1   6.2f
+
+// ── Streaming state ──────────────────────────────────────────
+bool streaming = false;
+
+// Enter idle: wait for the 'S' wake byte (all other bytes ignored).
+// Then settle, clear stale ADC flags, announce #READY, and set streaming.
+// Can be called from both setup() and loop() for restartable sessions.
+void waitForWake() {
+  streaming = false;
+  while (true) {
+    if (Serial.available()) {
+      char c = Serial.read();
+      if (c == 'S') break;
+      // Any other byte (e.g. stray 'X') is silently discarded.
+    }
+  }
+  delay(1000);  // ADC reference settle
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    adc_ready[0] = adc_ready[1] = adc_ready[2] = false;
+  }
+  Serial.println(F("#READY"));
+  streaming = true;
+}
+
+// ═════════════════════════════════════════════════════════════
+void setup() {
+  Serial.begin(2000000);
+  pinMode(OOR_PIN_0, INPUT);
+  pinMode(OOR_PIN_1, INPUT);
+  setupADC();      // ADC runs continuously; emission is gated by streaming flag
+  waitForWake();
+}
+
+void loop() {
+  // Check for stop byte before doing any work.
+  if (Serial.available()) {
+    char c = Serial.read();
+    if (c == 'X') {
+      waitForWake();  // returns only after next 'S' + #READY
+      return;
+    }
+  }
+
+  if (!streaming) return;
+
+  uint16_t val[3];
+  bool ready[3];
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    for (uint8_t i = 0; i < 3; i++) {
+      ready[i]     = adc_ready[i];
+      val[i]       = adc_result[i];
+      adc_ready[i] = false;
+    }
+  }
+
+  if (!ready[0] && !ready[1]) return;
+
+  // Emit one line per in-range sensor sample, paired with the
+  // most recent A0 raw ADC count (val[2] is always fresh).
+  if (ready[0]) {
+    float mm = adcToMM0(val[0]) - OFFSET_MM0;
+    Serial.print(mm, 3);
+    Serial.print(F(", "));
+    Serial.println(val[2]);
+  }
+  if (ready[1]) {
+    float mm = adcToMM1(val[1]) - OFFSET_MM1;
+    Serial.print(mm, 3);
+    Serial.print(F(", "));
+    Serial.println(val[2]);
+  }
+}

A0Calibration/calibrate_a0.ipynb

@@ -0,0 +1,483 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# A0 Sensor Calibration\n",
+    "\n",
+    "Streams `mm, a0_raw` lines from the `A0CalibrationSketch` Arduino sketch, buckets them into 0.05 mm intervals (±0.02 mm acceptance window), and exports a calibration table to `data/` as a timestamped Excel file.\n",
+    "\n",
+    "**Workflow**\n",
+    "1. Upload `A0CalibrationSketch/A0CalibrationSketch.ino` to the Arduino.\n",
+    "2. Run the **Config** and **Connect** cells below.\n",
+    "3. Run the **Collect** cell and sweep the target slowly through the full 0–20 mm range. The cell terminates automatically once every bucket has ≥50 samples, or press **Stop Collection** to halt early.\n",
+    "4. Run the **Export** cell.\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 4,
+   "id": "83a5e59e",
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "401 buckets from 0.0 to 20.0 mm (step 0.05 mm)\n",
+      "Acceptance window: +/-0.02 mm from each bucket center\n",
+      "Target: >= 50 samples per bucket\n"
+     ]
+    }
+   ],
+   "source": [
+    "# === Config ===\n",
+    "SERIAL_PORT   = None       # e.g. '/dev/cu.usbmodem14101'; None -> auto-detect\n",
+    "BAUD_RATE     = 115200\n",
+    "\n",
+    "MM_MIN        = 0.0\n",
+    "MM_MAX        = 20.0\n",
+    "BUCKET_STEP   = 0.05       # bucket spacing (mm)\n",
+    "BUCKET_WINDOW = 0.02       # accept samples within +/- WINDOW of bucket center\n",
+    "MIN_SAMPLES   = 50         # each bucket must reach this count to auto-stop\n",
+    "\n",
+    "from pathlib import Path\n",
+    "DATA_DIR = Path('data')\n",
+    "DATA_DIR.mkdir(exist_ok=True)\n",
+    "\n",
+    "import numpy as np\n",
+    "\n",
+    "N_BUCKETS      = int(round((MM_MAX - MM_MIN) / BUCKET_STEP)) + 1\n",
+    "bucket_centers = np.round(MM_MIN + np.arange(N_BUCKETS) * BUCKET_STEP, 4)\n",
+    "\n",
+    "print(f\"{N_BUCKETS} buckets from {MM_MIN} to {MM_MAX} mm (step {BUCKET_STEP} mm)\")\n",
+    "print(f\"Acceptance window: +/-{BUCKET_WINDOW} mm from each bucket center\")\n",
+    "print(f\"Target: >= {MIN_SAMPLES} samples per bucket\")\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "id": "7b88a837",
+   "metadata": {},
+   "source": [
+    "## 1. Connect to Arduino"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 5,
+   "id": "bee575ca",
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Opening /dev/cu.usbmodem11301 @ 115200...\n",
+      "Sending wake signal...\n",
+      "Waiting for #READY marker...\n",
+      "Arduino ready - streaming.\n"
+     ]
+    }
+   ],
+   "source": [
+    "import serial\n",
+    "import serial.tools.list_ports\n",
+    "import time\n",
+    "\n",
+    "def find_port():\n",
+    "    if SERIAL_PORT is not None:\n",
+    "        return SERIAL_PORT\n",
+    "    ports = list(serial.tools.list_ports.comports())\n",
+    "    for p in ports:\n",
+    "        dev  = p.device\n",
+    "        desc = (p.description or '').lower()\n",
+    "        if ('usbmodem' in dev or 'usbserial' in dev.lower()\n",
+    "                or 'arduino' in desc or 'wch' in desc):\n",
+    "            return dev\n",
+    "    if ports:\n",
+    "        return ports[0].device\n",
+    "    raise RuntimeError(\"No serial ports found. Set SERIAL_PORT manually.\")\n",
+    "\n",
+    "port = find_port()\n",
+    "print(f\"Opening {port} @ {BAUD_RATE}...\")\n",
+    "\n",
+    "# Close any previously-held handle if this cell is re-run.\n",
+    "try:\n",
+    "    if ser.is_open:\n",
+    "        ser.close()\n",
+    "except NameError:\n",
+    "    pass\n",
+    "\n",
+    "ser = serial.Serial(port, BAUD_RATE, timeout=0.1)\n",
+    "\n",
+    "# Opening the port may or may not reset the board.\n",
+    "# Either way, the Arduino is in idle state (waiting for 'S') after boot.\n",
+    "# Wait for reset/boot to finish, then flush anything printed before we opened.\n",
+    "time.sleep(3.0)\n",
+    "ser.reset_input_buffer()\n",
+    "\n",
+    "# Send 'X' first: stops any ongoing stream if the Arduino was already running.\n",
+    "# If it's in idle state, 'X' is silently ignored.\n",
+    "ser.write(b'X')\n",
+    "time.sleep(0.2)\n",
+    "ser.reset_input_buffer()\n",
+    "\n",
+    "# Send the wake byte. Arduino will settle for 1 s then print #READY.\n",
+    "print(\"Sending wake signal...\")\n",
+    "ser.write(b'S')\n",
+    "\n",
+    "print(\"Waiting for #READY marker...\")\n",
+    "deadline   = time.time() + 15.0\n",
+    "seen_ready = False\n",
+    "while time.time() < deadline:\n",
+    "    raw = ser.readline()\n",
+    "    if not raw:\n",
+    "        continue\n",
+    "    if raw.strip() == b'#READY':\n",
+    "        seen_ready = True\n",
+    "        break\n",
+    "\n",
+    "if not seen_ready:\n",
+    "    raise RuntimeError(\n",
+    "        \"Did not receive #READY within 15s. \"\n",
+    "        \"Check sketch is uploaded and SERIAL_PORT is correct.\"\n",
+    "    )\n",
+    "\n",
+    "# Discard one line to ensure we start reading on a clean boundary.\n",
+    "_ = ser.readline()\n",
+    "print(\"Arduino ready - streaming.\")\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "id": "f4955b1c",
+   "metadata": {},
+   "source": [
+    "## 2. Collect calibration data\n",
+    "\n",
+    "Run this cell and slowly sweep the target through the full 0–20 mm range. The cell drains the serial stream in a background thread, buckets each valid sample, and updates the plot every ~0.3 s.\n",
+    "\n",
+    "Press **Stop Collection** to halt early, or let it run until every bucket has reached the target.\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 6,
+   "id": "1207881e",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "application/vnd.jupyter.widget-view+json": {
+       "model_id": "0aa7a77335254ed68cf8461be35468c7",
+       "version_major": 2,
+       "version_minor": 0
+      },
+      "text/plain": [
+       "VBox(children=(HBox(children=(Button(button_style='danger', description='Stop Collection', icon='stop', style=…"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    },
+    {
+     "ename": "CancelledError",
+     "evalue": "",
+     "output_type": "error",
+     "traceback": [
+      "\u001b[31m---------------------------------------------------------------------------\u001b[39m",
+      "\u001b[31mCancelledError\u001b[39m                            Traceback (most recent call last)",
+      "\u001b[36mCell\u001b[39m\u001b[36m \u001b[39m\u001b[32mIn[6]\u001b[39m\u001b[32m, line 174\u001b[39m\n\u001b[32m    170\u001b[39m             \u001b[38;5;28;01mbreak\u001b[39;00m\n\u001b[32m    171\u001b[39m \n\u001b[32m    172\u001b[39m         \u001b[38;5;28;01mawait\u001b[39;00m asyncio.sleep(\u001b[32m0.05\u001b[39m)\n\u001b[32m    173\u001b[39m \n\u001b[32m--> \u001b[39m\u001b[32m174\u001b[39m \u001b[38;5;28;01mawait\u001b[39;00m collect()\n\u001b[32m    175\u001b[39m \n\u001b[32m    176\u001b[39m \u001b[38;5;66;03m# Tell the Arduino to stop streaming and return to idle,\u001b[39;00m\n\u001b[32m    177\u001b[39m \u001b[38;5;66;03m# so the connect cell can restart without re-uploading the sketch.\u001b[39;00m\n",
+      "\u001b[36mCell\u001b[39m\u001b[36m \u001b[39m\u001b[32mIn[6]\u001b[39m\u001b[32m, line 172\u001b[39m, in \u001b[36mcollect\u001b[39m\u001b[34m()\u001b[39m\n\u001b[32m    168\u001b[39m         \u001b[38;5;28;01mif\u001b[39;00m filled == N_BUCKETS:\n\u001b[32m    169\u001b[39m             status_label.value += \u001b[33m'  -- target reached'\u001b[39m\n\u001b[32m    170\u001b[39m             \u001b[38;5;28;01mbreak\u001b[39;00m\n\u001b[32m    171\u001b[39m \n\u001b[32m--> \u001b[39m\u001b[32m172\u001b[39m         \u001b[38;5;28;01mawait\u001b[39;00m asyncio.sleep(\u001b[32m0.05\u001b[39m)\n",
+      "\u001b[36mFile \u001b[39m\u001b[32m/Library/Frameworks/Python.framework/Versions/3.13/lib/python3.13/asyncio/tasks.py:718\u001b[39m, in \u001b[36msleep\u001b[39m\u001b[34m(delay, result)\u001b[39m\n\u001b[32m    714\u001b[39m h = loop.call_later(delay,\n\u001b[32m    715\u001b[39m                     futures._set_result_unless_cancelled,\n\u001b[32m    716\u001b[39m                     future, result)\n\u001b[32m    717\u001b[39m \u001b[38;5;28;01mtry\u001b[39;00m:\n\u001b[32m--> \u001b[39m\u001b[32m718\u001b[39m     \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28;01mawait\u001b[39;00m future\n\u001b[32m    719\u001b[39m \u001b[38;5;28;01mfinally\u001b[39;00m:\n\u001b[32m    720\u001b[39m     h.cancel()\n",
+      "\u001b[31mCancelledError\u001b[39m: "
+     ]
+    }
+   ],
+   "source": [
+    "import re\n",
+    "import queue\n",
+    "import threading\n",
+    "import asyncio\n",
+    "import time\n",
+    "import matplotlib.pyplot as plt\n",
+    "import ipywidgets as widgets\n",
+    "from IPython.display import display, clear_output\n",
+    "\n",
+    "# === Bucket state (reset on each run of this cell) ===\n",
+    "counts = np.zeros(N_BUCKETS, dtype=np.int64)\n",
+    "means  = np.zeros(N_BUCKETS, dtype=np.float64)\n",
+    "\n",
+    "# Matches \"<float>, <int>\" with optional leading sign and whitespace.\n",
+    "LINE_RE = re.compile(r'^\\s*(-?\\d+\\.?\\d*)\\s*,\\s*(\\d+)\\s*$')\n",
+    "\n",
+    "def update_bucket(mm, adc):\n",
+    "    idx = int(round((mm - MM_MIN) / BUCKET_STEP))\n",
+    "    if idx < 0 or idx >= N_BUCKETS:\n",
+    "        return False\n",
+    "    if abs(mm - bucket_centers[idx]) > BUCKET_WINDOW + 1e-9:\n",
+    "        return False  # falls into the gap between bucket windows\n",
+    "    counts[idx] += 1\n",
+    "    # Welford-style incremental mean (numerically stable, no accumulator overflow)\n",
+    "    means[idx] += (adc - means[idx]) / counts[idx]\n",
+    "    return True\n",
+    "\n",
+    "# === Serial reader thread ===\n",
+    "# If this cell is re-run, stop the previous reader first so we don't end up\n",
+    "# with two threads racing on ser.readline().\n",
+    "try:\n",
+    "    _reader_stop.set()\n",
+    "    _reader_thread.join(timeout=1.0)\n",
+    "except NameError:\n",
+    "    pass\n",
+    "\n",
+    "_reader_stop = threading.Event()\n",
+    "line_queue   = queue.Queue()\n",
+    "\n",
+    "def _reader_loop(stop_ev, q, s):\n",
+    "    while not stop_ev.is_set():\n",
+    "        try:\n",
+    "            raw = s.readline()   # returns after timeout=0.1s if no data\n",
+    "        except Exception:\n",
+    "            break\n",
+    "        if raw:\n",
+    "            q.put(raw)\n",
+    "\n",
+    "_reader_thread = threading.Thread(\n",
+    "    target=_reader_loop, args=(_reader_stop, line_queue, ser), daemon=True,\n",
+    ")\n",
+    "_reader_thread.start()\n",
+    "\n",
+    "# === UI widgets ===\n",
+    "stop_button = widgets.Button(\n",
+    "    description='Stop Collection', button_style='danger', icon='stop',\n",
+    ")\n",
+    "progress = widgets.IntProgress(\n",
+    "    min=0, max=N_BUCKETS, value=0,\n",
+    "    description='Buckets @target:', bar_style='info',\n",
+    "    style={'description_width': 'initial'},\n",
+    "    layout=widgets.Layout(width='500px'),\n",
+    ")\n",
+    "status_label = widgets.Label(value='Starting...')\n",
+    "plot_out     = widgets.Output()\n",
+    "\n",
+    "stop_requested = False\n",
+    "def _on_stop(_b):\n",
+    "    global stop_requested\n",
+    "    stop_requested = True\n",
+    "    stop_button.description = 'Stopping...'\n",
+    "    stop_button.disabled    = True\n",
+    "stop_button.on_click(_on_stop)\n",
+    "\n",
+    "display(widgets.VBox([\n",
+    "    widgets.HBox([stop_button, progress]),\n",
+    "    status_label,\n",
+    "    plot_out,\n",
+    "]))\n",
+    "\n",
+    "# === Live plot ===\n",
+    "plt.ioff()\n",
+    "fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6), sharex=True)\n",
+    "plt.close(fig)  # prevent implicit display; we manage display manually\n",
+    "\n",
+    "def _draw():\n",
+    "    ax1.clear(); ax2.clear()\n",
+    "    filled = counts > 0\n",
+    "    if filled.any():\n",
+    "        ax1.plot(bucket_centers[filled], means[filled], 'b.-', markersize=3)\n",
+    "    ax1.set_ylabel('Avg A0 ADC')\n",
+    "    ax1.set_title('Calibration curve (filled buckets)')\n",
+    "    ax1.grid(True, alpha=0.3)\n",
+    "\n",
+    "    colors = [\n",
+    "        '#2ecc71' if c >= MIN_SAMPLES\n",
+    "        else '#f39c12' if c > 0\n",
+    "        else '#e74c3c'\n",
+    "        for c in counts\n",
+    "    ]\n",
+    "    ax2.bar(bucket_centers, counts, width=BUCKET_STEP * 0.9, color=colors)\n",
+    "    ax2.axhline(\n",
+    "        MIN_SAMPLES, color='red', linestyle='--', linewidth=1,\n",
+    "        label=f'target = {MIN_SAMPLES}',\n",
+    "    )\n",
+    "    ax2.set_xlabel('Position (mm)')\n",
+    "    ax2.set_ylabel('Sample count')\n",
+    "    ax2.set_title('Bucket fill progress  (green = done, orange = partial, red = empty)')\n",
+    "    ax2.legend(loc='upper right', fontsize=8)\n",
+    "    ax2.grid(True, alpha=0.3)\n",
+    "\n",
+    "    fig.tight_layout()\n",
+    "    with plot_out:\n",
+    "        clear_output(wait=True)\n",
+    "        display(fig)\n",
+    "\n",
+    "_draw()\n",
+    "\n",
+    "# === Collection loop ===\n",
+    "total_parsed    = 0\n",
+    "total_bucketed  = 0\n",
+    "total_malformed = 0\n",
+    "last_draw       = time.monotonic()\n",
+    "DRAW_INTERVAL   = 0.3  # seconds\n",
+    "\n",
+    "async def collect():\n",
+    "    global total_parsed, total_bucketed, total_malformed, last_draw\n",
+    "\n",
+    "    while not stop_requested:\n",
+    "        # Drain whatever came in since last tick.\n",
+    "        try:\n",
+    "            while True:\n",
+    "                raw = line_queue.get_nowait()\n",
+    "                try:\n",
+    "                    txt = raw.decode('ascii', errors='replace').strip()\n",
+    "                except Exception:\n",
+    "                    total_malformed += 1\n",
+    "                    continue\n",
+    "                if not txt or txt.startswith('#'):\n",
+    "                    continue  # stray marker or blank line\n",
+    "                m = LINE_RE.match(txt)\n",
+    "                if not m:\n",
+    "                    total_malformed += 1\n",
+    "                    continue\n",
+    "                total_parsed += 1\n",
+    "                mm  = float(m.group(1))\n",
+    "                adc = int(m.group(2))\n",
+    "                if update_bucket(mm, adc):\n",
+    "                    total_bucketed += 1\n",
+    "        except queue.Empty:\n",
+    "            pass\n",
+    "\n",
+    "        filled = int(np.sum(counts >= MIN_SAMPLES))\n",
+    "\n",
+    "        now = time.monotonic()\n",
+    "        if now - last_draw >= DRAW_INTERVAL:\n",
+    "            last_draw = now\n",
+    "            progress.value = filled\n",
+    "            status_label.value = (\n",
+    "                f\"filled={filled}/{N_BUCKETS}  \"\n",
+    "                f\"min_count={int(counts.min())}  \"\n",
+    "                f\"parsed={total_parsed}  \"\n",
+    "                f\"bucketed={total_bucketed}  \"\n",
+    "                f\"malformed={total_malformed}\"\n",
+    "            )\n",
+    "            _draw()\n",
+    "\n",
+    "        if filled == N_BUCKETS:\n",
+    "            status_label.value += '  -- target reached'\n",
+    "            break\n",
+    "\n",
+    "        await asyncio.sleep(0.05)\n",
+    "\n",
+    "await collect()\n",
+    "\n",
+    "# Tell the Arduino to stop streaming and return to idle,\n",
+    "# so the connect cell can restart without re-uploading the sketch.\n",
+    "try:\n",
+    "    ser.write(b'X')\n",
+    "    time.sleep(0.1)\n",
+    "    ser.reset_input_buffer()\n",
+    "except Exception:\n",
+    "    pass\n",
+    "\n",
+    "# Clean shutdown of the reader thread.\n",
+    "_reader_stop.set()\n",
+    "\n",
+    "_draw()\n",
+    "filled_final = int(np.sum(counts >= MIN_SAMPLES))\n",
+    "status_label.value = (\n",
+    "    f\"DONE. filled={filled_final}/{N_BUCKETS}  \"\n",
+    "    f\"parsded={total_parsed}  bucketed={total_bucketed}  malformed={total_malformed}\"\n",
+    ")\n",
+    "stop_button.description = 'Stopped'\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## 3. Export to Excel"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from datetime import datetime\n",
+    "import pandas as pd\n",
+    "\n",
+    "ts       = datetime.now().strftime('%Y%m%d_%H%M%S')\n",
+    "out_path = DATA_DIR / f'a0_calibration_{ts}.xlsx'\n",
+    "\n",
+    "has_data = counts > 0\n",
+    "df = pd.DataFrame({\n",
+    "    'mm_val':  bucket_centers[has_data],\n",
+    "    'avg_adc': np.round(means[has_data], 3),\n",
+    "})\n",
+    "\n",
+    "df.to_excel(out_path, index=False)\n",
+    "print(f\"Exported {len(df)} rows -> {out_path.resolve()}\")\n",
+    "\n",
+    "short = (counts > 0) & (counts < MIN_SAMPLES)\n",
+    "empty = counts == 0\n",
+    "if short.any():\n",
+    "    print(f\"  Note: {int(short.sum())} buckets have data but < {MIN_SAMPLES} samples.\")\n",
+    "if empty.any():\n",
+    "    print(f\"  Note: {int(empty.sum())} buckets had no data and are omitted from the export.\")\n",
+    "\n",
+    "df.head()\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## 4. Cleanup  (optional — run when done)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "try:\n",
+    "    _reader_stop.set()\n",
+    "    _reader_thread.join(timeout=1.0)\n",
+    "except NameError:\n",
+    "    pass\n",
+    "\n",
+    "try:\n",
+    "    if ser.is_open:\n",
+    "        ser.close()\n",
+    "        print(\"Serial port closed.\")\n",
+    "except NameError:\n",
+    "    pass\n"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "venv",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.13.7"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}

A0Calibration/calibrate_a0.py

@@ -0,0 +1,447 @@
+#!/usr/bin/env python3
+"""
+A0 Calibration — standalone tkinter GUI
+Usage:  python calibrate_a0.py
+Deps:   pyserial  numpy  pandas  openpyxl  matplotlib
+"""
+
+import re
+import time
+import threading
+from datetime import datetime
+from pathlib import Path
+
+import numpy as np
+import tkinter as tk
+from tkinter import ttk, messagebox
+import matplotlib
+matplotlib.use('TkAgg')
+from matplotlib.figure import Figure
+from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
+import serial
+import serial.tools.list_ports
+import pandas as pd
+
+# ── Configuration ─────────────────────────────────────────────
+BAUD_RATE        = 2_000_000
+MM_MIN           = 0.0
+MM_MAX           = 20.0
+BUCKET_STEP      = 0.05
+BUCKET_WINDOW    = 0.02
+MIN_SAMPLES      = 50
+DATA_DIR         = Path(__file__).parent / 'data'
+DRAW_INTERVAL_MS = 300      # GUI redraw period — worker is never throttled by this
+
+DATA_DIR.mkdir(exist_ok=True)
+N_BUCKETS      = int(round((MM_MAX - MM_MIN) / BUCKET_STEP)) + 1
+bucket_centers = np.round(MM_MIN + np.arange(N_BUCKETS) * BUCKET_STEP, 4)
+
+# Bytes regex: avoids decode overhead on every line in the hot path
+LINE_RE = re.compile(rb'^\s*(-?\d+\.?\d*)\s*,\s*(\d+)\s*$')
+
+
+class CalibrationApp:
+
+    def __init__(self, root: tk.Tk):
+        self.root = root
+        self.root.title('A0 Sensor Calibration')
+        self.root.minsize(900, 650)
+
+        # ── Shared bucket state (worker writes, GUI reads snapshots) ──
+        self.counts    = np.zeros(N_BUCKETS, dtype=np.int64)
+        self.means     = np.zeros(N_BUCKETS, dtype=np.float64)
+        self.data_lock = threading.Lock()
+
+        # ── Display stats: written by worker, read by GUI ──
+        self._stat_parsed    = 0
+        self._stat_bucketed  = 0
+        self._stat_malformed = 0
+        self._stats_lock     = threading.Lock()
+
+        # ── Thread / connection state ──
+        self.ser           = None
+        self.worker_thread = None
+        self.worker_stop   = threading.Event()
+        self.collecting    = False
+
+        self._build_ui()
+        self._refresh_ports()
+        self._schedule_draw()
+
+    # ── UI construction ───────────────────────────────────────
+
+    def _build_ui(self):
+        # Connection bar
+        top = ttk.Frame(self.root, padding=(6, 6, 6, 2))
+        top.pack(fill='x')
+
+        ttk.Label(top, text='Port:').pack(side='left')
+        self.port_var = tk.StringVar()
+        self.port_cb  = ttk.Combobox(top, textvariable=self.port_var,
+                                     width=30, state='readonly')
+        self.port_cb.pack(side='left', padx=(4, 6))
+        ttk.Button(top, text='↺', width=3,
+                   command=self._refresh_ports).pack(side='left', padx=2)
+        self.conn_btn = ttk.Button(top, text='Connect',
+                                   command=self._toggle_connect)
+        self.conn_btn.pack(side='left', padx=2)
+        self.conn_label = ttk.Label(top, text='● Disconnected',
+                                    foreground='#888')
+        self.conn_label.pack(side='left', padx=8)
+
+        # Control bar
+        ctrl = ttk.Frame(self.root, padding=(6, 2, 6, 2))
+        ctrl.pack(fill='x')
+
+        self.start_btn = ttk.Button(ctrl, text='Start Collection',
+                                    command=self._start, state='disabled')
+        self.start_btn.pack(side='left', padx=2)
+        self.stop_btn = ttk.Button(ctrl, text='Stop Collection',
+                                   command=self._stop, state='disabled')
+        self.stop_btn.pack(side='left', padx=2)
+        ttk.Button(ctrl, text='Export Excel',
+                   command=self._export).pack(side='left', padx=(14, 2))
+
+        ttk.Separator(ctrl, orient='vertical').pack(side='left',
+                                                    fill='y', padx=8)
+
+        self.progress_var = tk.IntVar(value=0)
+        self.progress_bar = ttk.Progressbar(ctrl, variable=self.progress_var,
+                                            maximum=N_BUCKETS, length=260)
+        self.progress_bar.pack(side='left')
+        self.progress_lbl = ttk.Label(ctrl, text=f'  0 / {N_BUCKETS} buckets')
+        self.progress_lbl.pack(side='left')
+
+        # Stats bar
+        sbar = ttk.Frame(self.root, padding=(6, 0, 6, 2))
+        sbar.pack(fill='x')
+        self.stats_lbl = ttk.Label(sbar,
+                                   text='parsed=0   bucketed=0   malformed=0',
+                                   foreground='#555')
+        self.stats_lbl.pack(side='left')
+
+        # Matplotlib canvas
+        self.fig = Figure(tight_layout=True)
+        self.ax1 = self.fig.add_subplot(2, 1, 1)
+        self.ax2 = self.fig.add_subplot(2, 1, 2, sharex=self.ax1)
+        
+        self.ax1.set_ylabel('Avg A0 ADC')
+        self.ax1.set_title('Calibration curve')
+        self.ax1.grid(True, alpha=0.3)
+
+        self.ax2.set_xlabel('Position (mm)')
+        self.ax2.set_ylabel('Sample count')
+        self.ax2.set_title(
+            'Bucket fill progress (green = done, orange = partial, red = empty)')
+        self.ax2.axhline(MIN_SAMPLES, color='red', linestyle='--',
+                         linewidth=1, label=f'target = {MIN_SAMPLES}')
+        self.ax2.legend(fontsize=8, loc='upper right')
+        self.ax2.grid(True, alpha=0.3)
+
+        # Create empty line and bar artists to update later
+        self.line, = self.ax1.plot([], [], 'b.-', markersize=3)
+        self.bars = self.ax2.bar(bucket_centers, np.zeros(N_BUCKETS), width=BUCKET_STEP * 0.9)
+
+        self.canvas = FigureCanvasTkAgg(self.fig, master=self.root)
+        self.canvas.get_tk_widget().pack(fill='both', expand=True,
+                                         padx=6, pady=(2, 6))
+
+    # ── Port management ───────────────────────────────────────
+
+    def _refresh_ports(self):
+        ports = list(serial.tools.list_ports.comports())
+        self.port_cb['values'] = [p.device for p in ports]
+        if not ports:
+            return
+        for p in ports:
+            d = (p.description or '').lower()
+            if ('usbmodem' in p.device or 'usbserial' in p.device.lower()
+                    or 'arduino' in d or 'wch' in d):
+                self.port_var.set(p.device)
+                return
+        if not self.port_var.get():
+            self.port_var.set(ports[0].device)
+
+    # ── Connection ────────────────────────────────────────────
+
+    def _toggle_connect(self):
+        if self.ser and self.ser.is_open:
+            self._disconnect()
+        else:
+            self._connect()
+
+    def _connect(self):
+        port = self.port_var.get()
+        if not port:
+            messagebox.showerror('No port', 'Select a serial port first.')
+            return
+        try:
+            # timeout=0.05: readline() returns within 50 ms when no data,
+            # so the worker's stop-event check is always responsive.
+            self.ser = serial.Serial(port, BAUD_RATE, timeout=0.05)
+        except serial.SerialException as e:
+            messagebox.showerror('Connect failed', str(e))
+            return
+        self.conn_btn.config(state='disabled')
+        self.conn_label.config(text='⏳ Connecting…', foreground='orange')
+        threading.Thread(target=self._handshake, daemon=True).start()
+
+    def _handshake(self):
+        """Runs in a daemon thread so the GUI stays responsive during waits."""
+        try:
+            time.sleep(3.0)             # cover Arduino reset/boot
+            self.ser.reset_input_buffer()
+            self.ser.write(b'X')        # stop stream if already running
+            time.sleep(0.2)
+            self.ser.reset_input_buffer()
+            self.ser.write(b'S')        # send wake byte
+
+            deadline = time.time() + 15.0
+            while time.time() < deadline:
+                raw = self.ser.readline()
+                if raw.strip() == b'#READY':
+                    self.ser.readline()  # discard potential partial first line
+                    self.root.after(0, self._on_connected)
+                    return
+            self.root.after(0, lambda: self._on_connect_failed(
+                'Did not receive #READY within 15 s.\n'
+                'Check sketch upload and baud rate.'))
+        except Exception as e:
+            self.root.after(0, lambda err=str(e): self._on_connect_failed(err))
+
+    def _on_connected(self):
+        self.conn_label.config(text='● Connected', foreground='green')
+        self.conn_btn.config(text='Disconnect', state='normal')
+        self.start_btn.config(state='normal')
+
+    def _on_connect_failed(self, msg):
+        messagebox.showerror('Connection failed', msg)
+        try:
+            self.ser.close()
+        except Exception:
+            pass
+        self.ser = None
+        self.conn_label.config(text='● Disconnected', foreground='#888')
+        self.conn_btn.config(text='Connect', state='normal')
+
+    def _disconnect(self):
+        self._stop()
+        try:
+            if self.ser and self.ser.is_open:
+                self.ser.write(b'X')
+                self.ser.close()
+        except Exception:
+            pass
+        self.ser = None
+        self.conn_label.config(text='● Disconnected', foreground='#888')
+        self.conn_btn.config(text='Connect', state='normal')
+        self.start_btn.config(state='disabled')
+        self.stop_btn.config(state='disabled')
+
+    # ── Collection control ────────────────────────────────────
+
+    def _start(self):
+        if self.collecting:
+            return
+        with self.data_lock:
+            self.counts[:] = 0
+            self.means[:]  = 0.0
+        with self._stats_lock:
+            self._stat_parsed = self._stat_bucketed = self._stat_malformed = 0
+
+        self.collecting = True
+        self.worker_stop.clear()
+        self.worker_thread = threading.Thread(
+            target=self._worker, daemon=True, name='serial-worker')
+        self.worker_thread.start()
+
+        self.start_btn.config(state='disabled')
+        self.stop_btn.config(state='normal')
+
+    def _stop(self):
+        if not self.collecting:
+            return
+        self.collecting = False
+        self.worker_stop.set()
+        if self.worker_thread:
+            self.worker_thread.join(timeout=1.0)
+        self.start_btn.config(
+            state='normal' if (self.ser and self.ser.is_open) else 'disabled')
+        self.stop_btn.config(state='disabled')
+
+    def _auto_complete(self):
+        """Called from worker via root.after — runs on the GUI thread."""
+        self._stop()
+        messagebox.showinfo(
+            'Collection complete',
+            f'All {N_BUCKETS} buckets reached ≥{MIN_SAMPLES} samples.\n'
+            'Click Export Excel to save.')
+
+    # ── Serial worker ─────────────────────────────────────────
+    #
+    # Design priorities:
+    #   1. Never miss a byte — reads everything in the OS buffer each wake
+    #   2. Never wait on the GUI — lock hold-time is a single array update
+    #   3. Auto-complete in O(1) — tracks filled count incrementally
+    #
+    def _worker(self):
+        ser      = self.ser
+        stop     = self.worker_stop
+        counts   = self.counts
+        means    = self.means
+        lock     = self.data_lock
+        centers  = bucket_centers
+
+        parsed = bucketed = malformed = filled_count = 0
+        buf    = bytearray()
+        STAT_EVERY = 500    # flush display stats every N parsed lines
+
+        while not stop.is_set():
+            waiting = ser.in_waiting
+            if waiting:
+                try:
+                    buf += ser.read(waiting)    # single syscall for all bytes
+                except serial.SerialException:
+                    break
+
+                # Process every complete line accumulated in the buffer
+                while b'\n' in buf:
+                    line, buf = buf.split(b'\n', 1)
+                    line = line.strip()
+                    if not line or line[0:1] == b'#':
+                        continue
+
+                    m = LINE_RE.match(line)
+                    if not m:
+                        malformed += 1
+                        continue
+
+                    parsed += 1
+                    mm  = float(m.group(1))
+                    adc = int(m.group(2))
+                    idx = int(round((mm - MM_MIN) / BUCKET_STEP))
+
+                    if (0 <= idx < N_BUCKETS
+                            and abs(mm - centers[idx]) <= BUCKET_WINDOW + 1e-9):
+                        with lock:
+                            counts[idx] += 1
+                            means[idx] += (adc - means[idx]) / counts[idx]
+                            just_filled = (counts[idx] == MIN_SAMPLES)
+
+                        if just_filled:
+                            filled_count += 1
+                            if filled_count == N_BUCKETS:
+                                # Flush final stats then signal the GUI
+                                with self._stats_lock:
+                                    self._stat_parsed    = parsed
+                                    self._stat_bucketed  = bucketed + 1
+                                    self._stat_malformed = malformed
+                                self.root.after(0, self._auto_complete)
+                                return
+                        bucketed += 1
+
+                    if parsed % STAT_EVERY == 0:
+                        with self._stats_lock:
+                            self._stat_parsed    = parsed
+                            self._stat_bucketed  = bucketed
+                            self._stat_malformed = malformed
+            else:
+                # Serial quiet — yield briefly so stop_event can be noticed
+                time.sleep(0.0005)
+
+        # Final stats flush on manual stop
+        with self._stats_lock:
+            self._stat_parsed    = parsed
+            self._stat_bucketed  = bucketed
+            self._stat_malformed = malformed
+
+    # ── GUI draw (runs on main thread via root.after) ─────────
+
+    def _schedule_draw(self):
+        self._draw()
+        self.root.after(DRAW_INTERVAL_MS, self._schedule_draw)
+
+    def _draw(self):
+        with self.data_lock:
+            counts = self.counts.copy()
+            means  = self.means.copy()
+        with self._stats_lock:
+            parsed    = self._stat_parsed
+            bucketed  = self._stat_bucketed
+            malformed = self._stat_malformed
+
+        filled = int(np.sum(counts >= MIN_SAMPLES))
+        self.progress_var.set(filled)
+        self.progress_lbl.config(text=f'  {filled} / {N_BUCKETS} buckets')
+        self.stats_lbl.config(
+            text=f'parsed={parsed}   bucketed={bucketed}   malformed={malformed}')
+
+        # --- NEW: Update existing artists instead of clearing axes ---
+        has = counts > 0
+        if has.any():
+            self.line.set_data(bucket_centers[has], means[has])
+            self.ax1.relim()           # Recalculate limits
+            self.ax1.autoscale_view()  # Rescale axes
+
+        colors = [
+            '#2ecc71' if c >= MIN_SAMPLES
+            else '#f39c12' if c > 0
+            else '#e74c3c'
+            for c in counts
+        ]
+        
+        # Update each bar's height and color
+        for bar, count, color in zip(self.bars, counts, colors):
+            bar.set_height(count)
+            bar.set_facecolor(color)
+            
+        self.ax2.relim()
+        self.ax2.autoscale_view()
+
+        self.canvas.draw_idle()
+        
+    # ── Export ────────────────────────────────────────────────
+
+    def _export(self):
+        with self.data_lock:
+            counts = self.counts.copy()
+            means  = self.means.copy()
+        if not counts.any():
+            messagebox.showwarning('No data', 'No data collected yet.')
+            return
+
+        ts       = datetime.now().strftime('%Y%m%d_%H%M%S')
+        out_path = DATA_DIR / f'a0_calibration_{ts}.xlsx'
+        has      = counts > 0
+        pd.DataFrame({
+            'mm_val':  bucket_centers[has],
+            'avg_adc': np.round(means[has], 3),
+        }).to_excel(out_path, index=False)
+
+        short = int(np.sum((counts > 0) & (counts < MIN_SAMPLES)))
+        empty = int(np.sum(counts == 0))
+        msg   = f'Exported {int(has.sum())} rows to:\n{out_path}'
+        if short:
+            msg += f'\n\n{short} bucket(s) have data but < {MIN_SAMPLES} samples.'
+        if empty:
+            msg += f'\n{empty} empty bucket(s) omitted.'
+        messagebox.showinfo('Exported', msg)
+
+    # ── Cleanup ───────────────────────────────────────────────
+
+    def on_close(self):
+        self._stop()
+        try:
+            if self.ser and self.ser.is_open:
+                self.ser.write(b'X')
+                self.ser.close()
+        except Exception:
+            pass
+        self.root.destroy()
+
+
+if __name__ == '__main__':
+    root = tk.Tk()
+    app  = CalibrationApp(root)
+    root.protocol('WM_DELETE_WINDOW', app.on_close)
+    root.mainloop()

AdditiveControlCode/AdditiveControlCode.ino

@@ -1,176 +0,0 @@
-#include <Arduino.h>
-#include "IndSensorMap.hpp"
-#include "Controller.hpp"
-#include "ADC.hpp"
-#include "FastPWM.hpp"
-
-// K, Ki, Kd Constants
-Constants repelling = {250, 0, 1000};
-Constants attracting = {250, 0, 1000};
-
-Constants RollLeftUp = {0, 0, 100};
-Constants RollLeftDown = {0, 0, 100};
-
-Constants RollFrontUp = {0, 0, 500};
-Constants RollFrontDown = {0, 0, 500};
-
-// Reference values for average dist, 
-float avgRef = 11.0; // TBD: what is our equilibrium height with this testrig?
-float LRDiffRef = -2.0; // TBD: what is our left-right balance equilibrium? Positive -> left is above right
-float FBDiffRef = 0.0; // TBD: what is front-back balance equilibrium? Positive -> front above back.
-
-// Might be useful for things like jitter or lag.
-#define sampling_rate 1000 // Hz
-
-// EMA filter alpha value (all sensors use same alpha)
-#define alphaVal 0.3f
-
-// ABOVE THIS LINE IS TUNING VALUES ONLY, BELOW IS ACTUAL CODE.
-
-unsigned long tprior;
-unsigned int tDiffMicros;
-
-FullConsts fullConsts = {
-  {repelling, attracting},
-  {RollLeftDown, RollLeftUp},
-  {RollFrontDown, RollFrontUp}
-};
-
-FullController controller(indL, indR, indF, indB, fullConsts, avgRef, LRDiffRef, FBDiffRef);
-
-const int dt_micros = 1e6/sampling_rate;
-
-#define LEV_ON
-
-int ON = 0;
-
-void setup() {
-  Serial.begin(2000000);
-  setupADC();
-  setupFastPWM();
-
-  indL.alpha = alphaVal;
-  indR.alpha = alphaVal;
-  indF.alpha = alphaVal;
-  indB.alpha = alphaVal;
-
-  tprior = micros();
-
-  pinMode(dirFL, OUTPUT);
-  pinMode(pwmFL, OUTPUT);
-  pinMode(dirBL, OUTPUT);
-  pinMode(pwmBL, OUTPUT);
-  pinMode(dirFR, OUTPUT);
-  pinMode(pwmFR, OUTPUT);
-  pinMode(dirBR, OUTPUT);
-  pinMode(pwmBR, OUTPUT);
-}
-
-void loop() {
-  if (Serial.available() > 0) {
-    String cmd = Serial.readStringUntil('\n');
-    cmd.trim();
-    
-    // Check if it's a reference update command (format: REF,avgRef,lrDiffRef,fbDiffRef)
-    if (cmd.startsWith("REF,")) {
-      int firstComma = cmd.indexOf(',');
-      int secondComma = cmd.indexOf(',', firstComma + 1);
-      int thirdComma = cmd.indexOf(',', secondComma + 1);
-      
-      if (firstComma > 0 && secondComma > 0 && thirdComma > 0) {
-        float newAvgRef = cmd.substring(firstComma + 1, secondComma).toFloat();
-        float newLRDiffRef = cmd.substring(secondComma + 1, thirdComma).toFloat();
-        float newFBDiffRef = cmd.substring(thirdComma + 1).toFloat();
-        
-        avgRef = newAvgRef;
-        LRDiffRef = newLRDiffRef;
-        FBDiffRef = newFBDiffRef;
-        
-        controller.updateReferences(avgRef, LRDiffRef, FBDiffRef);
-        Serial.print("Updated References: Avg=");
-        Serial.print(avgRef);
-        Serial.print(", LR=");
-        Serial.print(LRDiffRef);
-        Serial.print(", FB=");
-        Serial.println(FBDiffRef);
-      }
-    }
-    // Check if it's a PID tuning command (format: PID,mode,kp,ki,kd)
-    else if (cmd.startsWith("PID,")) {
-      int firstComma = cmd.indexOf(',');
-      int secondComma = cmd.indexOf(',', firstComma + 1);
-      int thirdComma = cmd.indexOf(',', secondComma + 1);
-      int fourthComma = cmd.indexOf(',', thirdComma + 1);
-      
-      if (firstComma > 0 && secondComma > 0 && thirdComma > 0 && fourthComma > 0) {
-        int mode = cmd.substring(firstComma + 1, secondComma).toInt();
-        float kp = cmd.substring(secondComma + 1, thirdComma).toFloat();
-        float ki = cmd.substring(thirdComma + 1, fourthComma).toFloat();
-        float kd = cmd.substring(fourthComma + 1).toFloat();
-        
-        Constants newConst = {kp, ki, kd};
-        
-        // Mode mapping:
-        // 0: Repelling
-        // 1: Attracting
-        // 2: RollLeftDown
-        // 3: RollLeftUp
-        // 4: RollFrontDown
-        // 5: RollFrontUp
-        
-        switch(mode) {
-          case 0: // Repelling
-            repelling = newConst;
-            controller.updateAvgPID(repelling, attracting);
-            Serial.println("Updated Repelling PID");
-            break;
-          case 1: // Attracting
-            attracting = newConst;
-            controller.updateAvgPID(repelling, attracting);
-            Serial.println("Updated Attracting PID");
-            break;
-          case 2: // RollLeftDown
-            RollLeftDown = newConst;
-            controller.updateLRPID(RollLeftDown, RollLeftUp);
-            Serial.println("Updated RollLeftDown PID");
-            break;
-          case 3: // RollLeftUp
-            RollLeftUp = newConst;
-            controller.updateLRPID(RollLeftDown, RollLeftUp);
-            Serial.println("Updated RollLeftUp PID");
-            break;
-          case 4: // RollFrontDown
-            RollFrontDown = newConst;
-            controller.updateFBPID(RollFrontDown, RollFrontUp);
-            Serial.println("Updated RollFrontDown PID");
-            break;
-          case 5: // RollFrontUp
-            RollFrontUp = newConst;
-            controller.updateFBPID(RollFrontDown, RollFrontUp);
-            Serial.println("Updated RollFrontUp PID");
-            break;
-          default:
-            Serial.println("Invalid mode");
-            break;
-        }
-      }
-    } else {
-      // Original control on/off command
-      controller.outputOn = (cmd.charAt(0) != '0');
-    }
-  }
-  
-  tDiffMicros = micros() - tprior;
-
-  if (tDiffMicros >= dt_micros){
-    controller.update();
-    controller.report();
-    controller.sendOutputs(); 
-    // this and the previous line can be switched if you want the PWMs to display 0 when controller off.
-    
-    tprior = micros(); // maybe we have to move this line to before the update commands?
-    // since the floating point arithmetic may take a while...
-  }
-
-  //Serial.println(telapsed);
-}

AltSensorTesting/AltSensorTesting.ino

@@ -0,0 +1,196 @@
+#include <Arduino.h>
+#include <util/atomic.h>
+
+// ── ADC Interrupt-driven 3-channel read (A2, A3, A0) ─────────
+// Channel index: 0 → A2 (sensor 0), 1 → A3 (sensor 1), 2 → A0 (raw ref)
+static const uint8_t adc_mux[3] = {2, 3, 0};
+
+volatile uint16_t adc_result[3] = {0, 0, 0};
+volatile bool     adc_ready[3]  = {false, false, false};
+volatile uint8_t  adc_channel   = 0;
+
+void setupADC() {
+  ADMUX = (1 << REFS0) | adc_mux[0];  // AVCC ref, start on A2
+  ADCSRA = (1 << ADEN) | (1 << ADIE) | (1 << ADPS2);  // /16 prescaler
+  ADCSRA |= (1 << ADSC);
+}
+
+// ── OOR digital inputs ───────────────────────────────────────
+#define OOR_PIN_0  12    // HIGH = out of range, sensor 0 (A2)
+#define OOR_PIN_1  13    // HIGH = out of range, sensor 1 (A3)
+
+volatile bool OOR[2];
+
+ISR(ADC_vect) {
+  uint16_t sample = ADC;
+  uint8_t ch   = adc_channel;
+  uint8_t next = (ch + 1) % 3;
+
+  if (ch < 2) {
+    // Sensor channels: filter by OOR
+    OOR[ch] = digitalRead(ch == 0 ? OOR_PIN_0 : OOR_PIN_1);
+    if (!OOR[ch]) {
+      adc_result[ch] = sample;
+      adc_ready[ch]  = true;
+    }
+  } else {
+    // A0: no OOR, always store
+    adc_result[2] = sample;
+    adc_ready[2]  = true;
+  }
+
+  ADMUX = (ADMUX & 0xF0) | adc_mux[next];
+  adc_channel = next;
+  ADCSRA |= (1 << ADSC);
+}
+
+// ── ADC → mm linear mappings (raw range: 16–26 mm) ──────────
+#define adcToMM0(adc) ((float)map(adc, 178, 895, 1600, 2600) / 100.0f)
+#define adcToMM1(adc) ((float)map(adc, 176, 885, 1600, 2600) / 100.0f)
+
+// Subtract mounting offsets so both sensors share the same position frame:
+//   Sensor 0 raw 16–26 mm − 16 → 0–10 mm
+//   Sensor 1 raw 16–26 mm −  6 → 10–20 mm
+#define OFFSET_MM0  15.6f
+#define OFFSET_MM1   6.2f
+
+// ── Boundary tracking (in-range only, per sensor) ────────────
+#define TRACK_N 10
+uint16_t lowestVals[2][TRACK_N];
+uint16_t highestVals[2][TRACK_N];
+uint8_t  lowestCount[2]  = {0, 0};
+uint8_t  highestCount[2] = {0, 0};
+
+static void trackLowest(uint8_t s, uint16_t val) {
+  uint16_t *lv = lowestVals[s];
+  uint8_t  &lc = lowestCount[s];
+  if (lc < TRACK_N) {
+    uint8_t i = lc;
+    while (i > 0 && lv[i - 1] > val) { lv[i] = lv[i - 1]; i--; }
+    lv[i] = val;
+    lc++;
+  } else if (val < lv[TRACK_N - 1]) {
+    uint8_t i = TRACK_N - 1;
+    while (i > 0 && lv[i - 1] > val) { lv[i] = lv[i - 1]; i--; }
+    lv[i] = val;
+  }
+}
+
+static void trackHighest(uint8_t s, uint16_t val) {
+  uint16_t *hv = highestVals[s];
+  uint8_t  &hc = highestCount[s];
+  if (hc < TRACK_N) {
+    uint8_t i = hc;
+    while (i > 0 && hv[i - 1] < val) { hv[i] = hv[i - 1]; i--; }
+    hv[i] = val;
+    hc++;
+  } else if (val > hv[TRACK_N - 1]) {
+    uint8_t i = TRACK_N - 1;
+    while (i > 0 && hv[i - 1] < val) { hv[i] = hv[i - 1]; i--; }
+    hv[i] = val;
+  }
+}
+
+static void resetTracking() {
+  lowestCount[0] = highestCount[0] = 0;
+  lowestCount[1] = highestCount[1] = 0;
+}
+
+static void printBoundaries() {
+  for (uint8_t s = 0; s < 2; s++) {
+    Serial.print(F("--- Sensor "));
+    Serial.print(s);
+    Serial.println(F(": 10 Lowest In-Range ADC Values ---"));
+    for (uint8_t i = 0; i < lowestCount[s]; i++) Serial.println(lowestVals[s][i]);
+    Serial.print(F("--- Sensor "));
+    Serial.print(s);
+    Serial.println(F(": 10 Highest In-Range ADC Values ---"));
+    for (uint8_t i = 0; i < highestCount[s]; i++) Serial.println(highestVals[s][i]);
+  }
+}
+
+// ── State ────────────────────────────────────────────────────
+bool sampling = false;
+bool rawMode  = false;
+
+// ═════════════════════════════════════════════════════════════
+void setup() {
+  Serial.begin(115200);
+  pinMode(OOR_PIN_0, INPUT);
+  pinMode(OOR_PIN_1, INPUT);
+  setupADC();
+  Serial.println(F("Send '1' to start sampling, '0' to stop and print bounds, '2' for raw ADC output."));
+}
+
+void loop() {
+  // ── Serial command handling ──────────────────────────────
+  if (Serial.available() > 0) {
+    String cmd = Serial.readStringUntil('\n');
+    cmd.trim();
+
+    if (cmd.charAt(0) == '1') {
+      sampling = true;
+      rawMode  = false;
+      resetTracking();
+      Serial.println(F("Sampling started."));
+    } else if (cmd.charAt(0) == '2') {
+      sampling = true;
+      rawMode  = true;
+      Serial.println(F("Raw ADC output started."));
+    } else if (cmd.charAt(0) == '0') {
+      sampling = false;
+      rawMode  = false;
+      Serial.println(F("Sampling stopped."));
+      printBoundaries();
+    }
+  }
+
+  // ── Main sample path ────────────────────────────────────
+  if (!sampling) return;
+
+  uint16_t val[3];
+  bool ready[3];
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    for (uint8_t i = 0; i < 3; i++) {
+      ready[i]     = adc_ready[i];
+      val[i]       = adc_result[i];
+      adc_ready[i] = false;
+    }
+  }
+
+  if (!ready[0] && !ready[1]) return;
+
+  if (rawMode) {
+    if (ready[0]) {
+      Serial.print(adcToMM0(val[0]) - OFFSET_MM0);
+      Serial.print(F(", "));
+      Serial.println(val[2]);
+    }
+    if (ready[1]) {
+      Serial.print(adcToMM1(val[1]) - OFFSET_MM1);
+      Serial.print(F(", "));
+      Serial.println(val[2]);
+    }
+    return;
+  }
+
+  // Apply offset for whichever sensor(s) are in range
+  if (ready[0]) {
+    float mm = adcToMM0(val[0]) - OFFSET_MM0;
+    trackLowest(0, val[0]);
+    trackHighest(0, val[0]);
+    Serial.print(val[0]);
+    Serial.print(F(", "));
+    Serial.print(mm, 2);
+    Serial.println(F(" mm (s0)"));
+  }
+  if (ready[1]) {
+    float mm = adcToMM1(val[1]) - OFFSET_MM1;
+    trackLowest(1, val[1]);
+    trackHighest(1, val[1]);
+    Serial.print(val[1]);
+    Serial.print(F(", "));
+    Serial.print(mm, 2);
+    Serial.println(F(" mm (s1)"));
+  }
+}

RL Testing/PWM_Circuit_Model.py

@@ -1,47 +0,0 @@
-import numpy as np
-import matplotlib.pyplot as plt
-from scipy.signal import TransferFunction, lsim
-
-# ---- Parameters (edit these) ----
-R = 1.5          # ohms
-L = 0.0025       # henries
-V = 12.0         # volts
-
-# Time base
-t_end = 0.2
-dt = 1e-4
-t = np.arange(0, t_end, dt)
-
-# ---- Define a duty command D(t) ----
-# Example: start at 0, step to 0.2 at 20 ms, then to 0.6 at 80 ms, then to 1.0 at 140 ms (DC full on)
-D = np.zeros_like(t)
-D[t >= 0.020] = 0.2
-D[t >= 0.080] = 0.6
-D[t >= 0.140] = 1.0  # "straight DC" case (100% duty)
-
-# Clamp just in case
-D = np.clip(D, 0.0, 1.0)
-
-# ---- Transfer function I(s)/D(s) = V / (L s + R) ----
-# In scipy.signal.TransferFunction, numerator/denominator are polynomials in s
-G = TransferFunction([V], [L, R])
-
-# Simulate i(t) response to input D(t)
-tout, i, _ = lsim(G, U=D, T=t)
-
-# ---- Print steady-state expectations ----
-# For constant duty D0, steady-state i_ss = (V/R)*D0
-print("Expected steady-state currents (V/R * D):")
-for D0 in [0.0, 0.2, 0.6, 1.0]:
-    print(f"  D={D0:.1f} -> i_ss ~ {(V/R)*D0:.3f} A")
-
-
-# ---- Plot ----
-plt.figure()
-plt.plot(tout, D, label="Duty D(t)")
-plt.plot(tout, i, label="Current i(t) [A]")
-plt.xlabel("Time [s]")
-plt.grid(True)
-plt.legend()
-plt.show()
-

RL Testing/lev_PID.ipynb

@@ -1,787 +0,0 @@
-{
- "cells": [
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "# PID Control Testing for Maglev Pod\n",
-    "\n",
-    "This notebook provides a PID control interface for the `LevPodEnv` simulation environment.\n",
-    "\n",
-    "## Control Architecture\n",
-    "- **Height PID**: Controls average gap height → base PWM for all coils\n",
-    "- **Roll PID**: Corrects roll angle → differential left/right adjustment\n",
-    "- **Pitch PID**: Corrects pitch angle → differential front/back adjustment\n",
-    "\n",
-    "The outputs combine: `coil_pwm = height_output ± roll_adjustment ± pitch_adjustment`"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import numpy as np\n",
-    "import matplotlib.pyplot as plt\n",
-    "from lev_pod_env import LevPodEnv, TARGET_GAP\n",
-    "\n",
-    "# Set plot style for better aesthetics\n",
-    "plt.style.use('seaborn-v0_8-whitegrid')\n",
-    "plt.rcParams['figure.facecolor'] = 'white'\n",
-    "plt.rcParams['axes.facecolor'] = 'white'\n",
-    "plt.rcParams['font.size'] = 11\n",
-    "plt.rcParams['axes.titlesize'] = 14\n",
-    "plt.rcParams['axes.labelsize'] = 12\n",
-    "\n",
-    "print(f\"Target Gap Height: {TARGET_GAP * 1000:.2f} mm\")"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "---\n",
-    "## PID Controller Class"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "class PIDController:\n",
-    "    \"\"\"Simple PID controller with anti-windup.\"\"\"\n",
-    "    \n",
-    "    def __init__(self, kp: float, ki: float, kd: float, \n",
-    "                 output_min: float = -1.0, output_max: float = 1.0,\n",
-    "                 integral_limit: float = np.inf):\n",
-    "        self.kp = kp\n",
-    "        self.ki = ki\n",
-    "        self.kd = kd\n",
-    "        self.output_min = output_min\n",
-    "        self.output_max = output_max\n",
-    "        self.integral_limit = integral_limit if integral_limit else abs(output_max) * 2\n",
-    "        \n",
-    "        self.integral = 0.0\n",
-    "        self.prev_error = 0.0\n",
-    "        self.first_update = True\n",
-    "    \n",
-    "    def reset(self):\n",
-    "        \"\"\"Reset controller state.\"\"\"\n",
-    "        self.integral = 0.0\n",
-    "        self.prev_error = 0.0\n",
-    "        self.first_update = True\n",
-    "    \n",
-    "    def update(self, error: float, dt: float) -> float:\n",
-    "        \"\"\"Compute PID output.\n",
-    "        \n",
-    "        Args:\n",
-    "            error: Current error (setpoint - measurement)\n",
-    "            dt: Time step in seconds\n",
-    "        \n",
-    "        Returns:\n",
-    "            Control output (clamped to output limits)\n",
-    "        \"\"\"\n",
-    "        # Proportional term\n",
-    "        p_term = self.kp * error\n",
-    "        \n",
-    "        # Integral term with anti-windup\n",
-    "        self.integral += error * dt\n",
-    "        self.integral = np.clip(self.integral, -self.integral_limit, self.integral_limit)\n",
-    "        i_term = self.ki * self.integral\n",
-    "        \n",
-    "        # Derivative term (skip on first update to avoid spike)\n",
-    "        if self.first_update:\n",
-    "            d_term = 0.0\n",
-    "            self.first_update = False\n",
-    "        else:\n",
-    "            d_term = self.kd * (error - self.prev_error) / dt\n",
-    "        \n",
-    "        self.prev_error = error\n",
-    "        \n",
-    "        # Compute and clamp output\n",
-    "        output = p_term + i_term + d_term\n",
-    "        return np.clip(output, self.output_min, self.output_max)"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "---\n",
-    "## PID Gains Configuration\n",
-    "\n",
-    "**Modify these values to tune the controller.**\n",
-    "\n",
-    "- **Height PID**: Controls vertical position (gap height)\n",
-    "- **Roll PID**: Corrects side-to-side tilt\n",
-    "- **Pitch PID**: Corrects front-to-back tilt"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# ============================================================\n",
-    "#                    HEIGHT PID GAINS\n",
-    "# ============================================================\n",
-    "# Controls average gap height. Positive error = pod too low.\n",
-    "# Output: base PWM applied to all coils (positive = more lift)\n",
-    "\n",
-    "HEIGHT_KP = 50.0    # Proportional gain\n",
-    "HEIGHT_KI = 5.0     # Integral gain  \n",
-    "HEIGHT_KD = 10.0    # Derivative gain\n",
-    "\n",
-    "# ============================================================\n",
-    "#                     ROLL PID GAINS\n",
-    "# ============================================================\n",
-    "# Corrects roll angle (rotation about X-axis).\n",
-    "# Positive roll = right side lower → increase right coils\n",
-    "# Output: differential adjustment to left/right coils\n",
-    "\n",
-    "ROLL_KP = 2.0       # Proportional gain\n",
-    "ROLL_KI = 0.5       # Integral gain\n",
-    "ROLL_KD = 0.5       # Derivative gain\n",
-    "\n",
-    "# ============================================================\n",
-    "#                    PITCH PID GAINS  \n",
-    "# ============================================================\n",
-    "# Corrects pitch angle (rotation about Y-axis).\n",
-    "# Positive pitch = back lower → increase back coils\n",
-    "# Output: differential adjustment to front/back coils\n",
-    "\n",
-    "PITCH_KP = 2.0      # Proportional gain\n",
-    "PITCH_KI = 0.5      # Integral gain\n",
-    "PITCH_KD = 0.5      # Derivative gain\n",
-    "\n",
-    "print(\"PID Gains Configured:\")\n",
-    "print(f\"  Height: Kp={HEIGHT_KP}, Ki={HEIGHT_KI}, Kd={HEIGHT_KD}\")\n",
-    "print(f\"  Roll:   Kp={ROLL_KP}, Ki={ROLL_KI}, Kd={ROLL_KD}\")\n",
-    "print(f\"  Pitch:  Kp={PITCH_KP}, Ki={PITCH_KI}, Kd={PITCH_KD}\")"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "---\n",
-    "## Simulation Runner"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "def run_pid_simulation(\n",
-    "    initial_gap_mm: float = 14.0,\n",
-    "    max_steps: int = 2000,\n",
-    "    use_gui: bool = False,\n",
-    "    disturbance_step: int = None,\n",
-    "    disturbance_force: float = 0.0,\n",
-    "    disturbance_force_std: float = 0.0,\n",
-    "    verbose: bool = True\n",
-    ") -> dict:\n",
-    "    \"\"\"\n",
-    "    Run a PID-controlled simulation of the maglev pod.\n",
-    "\n",
-    "    Args:\n",
-    "        initial_gap_mm: Starting gap height in millimeters\n",
-    "        max_steps: Maximum simulation steps (each step is 1/240 second)\n",
-    "        use_gui: Whether to show PyBullet GUI visualization\n",
-    "        disturbance_step: Step at which to apply impulse disturbance (None = no impulse)\n",
-    "        disturbance_force: Impulse force in Newtons at disturbance_step (positive = upward)\n",
-    "        disturbance_force_std: Standard deviation of continuous stochastic noise (Newtons)\n",
-    "        verbose: Print progress updates\n",
-    "\n",
-    "    Returns:\n",
-    "        Dictionary containing time series data for plotting\n",
-    "    \"\"\"\n",
-    "    # Create environment with stochastic disturbance if specified\n",
-    "    env = LevPodEnv(\n",
-    "        use_gui=use_gui,\n",
-    "        initial_gap_mm=initial_gap_mm,\n",
-    "        max_steps=max_steps,\n",
-    "        disturbance_force_std=disturbance_force_std\n",
-    "    )\n",
-    "    dt = env.dt  # 1/240 seconds\n",
-    "\n",
-    "    # Initialize PID controllers\n",
-    "    height_pid = PIDController(HEIGHT_KP, HEIGHT_KI, HEIGHT_KD, output_min=-1.0, output_max=1.0)\n",
-    "    roll_pid = PIDController(ROLL_KP, ROLL_KI, ROLL_KD, output_min=-0.5, output_max=0.5)\n",
-    "    pitch_pid = PIDController(PITCH_KP, PITCH_KI, PITCH_KD, output_min=-0.5, output_max=0.5)\n",
-    "\n",
-    "    # Data storage\n",
-    "    data = {\n",
-    "        'time': [],\n",
-    "        'gap_front': [],\n",
-    "        'gap_back': [],\n",
-    "        'gap_avg': [],\n",
-    "        'roll_deg': [],\n",
-    "        'pitch_deg': [],\n",
-    "        'current_FL': [],\n",
-    "        'current_FR': [],\n",
-    "        'current_BL': [],\n",
-    "        'current_BR': [],\n",
-    "        'current_total': [],\n",
-    "        'pwm_FL': [],\n",
-    "        'pwm_FR': [],\n",
-    "        'pwm_BL': [],\n",
-    "        'pwm_BR': [],\n",
-    "    }\n",
-    "\n",
-    "    # Reset environment\n",
-    "    obs, _ = env.reset()\n",
-    "\n",
-    "    target_gap = TARGET_GAP  # meters\n",
-    "\n",
-    "    if verbose:\n",
-    "        print(f\"Starting simulation: initial_gap={initial_gap_mm}mm, target={target_gap*1000:.2f}mm\")\n",
-    "        if disturbance_step:\n",
-    "            print(f\"  Impulse disturbance: {disturbance_force}N at step {disturbance_step}\")\n",
-    "        if disturbance_force_std > 0:\n",
-    "            print(f\"  Stochastic noise: std={disturbance_force_std}N\")\n",
-    "\n",
-    "    for step in range(max_steps):\n",
-    "        t = step * dt\n",
-    "\n",
-    "        # Extract gap heights from observation (first 4 values are normalized gaps)\n",
-    "        # These are sensor readings (may include noise in future)\n",
-    "        gaps_normalized = obs[:4]  # [center_right, center_left, front, back]\n",
-    "        gaps = gaps_normalized * env.gap_scale + TARGET_GAP\n",
-    "\n",
-    "        gap_front = gaps[2]  # Front sensor\n",
-    "        gap_back = gaps[3]   # Back sensor\n",
-    "        gap_left = gaps[1]   # Center_Left sensor\n",
-    "        gap_right = gaps[0]  # Center_Right sensor\n",
-    "\n",
-    "        gap_avg = (gap_front + gap_back + gap_left + gap_right) / 4\n",
-    "\n",
-    "        # Calculate roll and pitch from sensor gaps (for PID control)\n",
-    "        y_distance = 0.1016  # Left to right distance (m)\n",
-    "        x_distance = 0.2518  # Front to back distance (m)\n",
-    "\n",
-    "        roll_angle = np.arcsin(np.clip((gap_left - gap_right) / y_distance, -1, 1))\n",
-    "        pitch_angle = np.arcsin(np.clip((gap_back - gap_front) / x_distance, -1, 1))\n",
-    "\n",
-    "        # Compute PID outputs\n",
-    "        height_error = target_gap - gap_avg  # Positive = too low, need more lift\n",
-    "        roll_error = -roll_angle  # Target is 0, negative feedback\n",
-    "        pitch_error = -pitch_angle  # Target is 0, negative feedback\n",
-    "\n",
-    "        height_output = height_pid.update(height_error, dt)\n",
-    "        roll_output = roll_pid.update(roll_error, dt)\n",
-    "        pitch_output = pitch_pid.update(pitch_error, dt)\n",
-    "\n",
-    "        # Combine outputs into 4 coil PWM commands\n",
-    "        # Coil layout: front_left (+x,+y), front_right (+x,-y), back_left (-x,+y), back_right (-x,-y)\n",
-    "        # Roll correction: positive roll_output → increase right side (FR, BR)\n",
-    "        # Pitch correction: positive pitch_output → increase back side (BL, BR)\n",
-    "\n",
-    "        pwm_FL = np.clip(height_output - roll_output - pitch_output, -1, 1)\n",
-    "        pwm_FR = np.clip(height_output + roll_output - pitch_output, -1, 1)\n",
-    "        pwm_BL = np.clip(height_output - roll_output + pitch_output, -1, 1)\n",
-    "        pwm_BR = np.clip(height_output + roll_output + pitch_output, -1, 1)\n",
-    "\n",
-    "        action = np.array([pwm_FL, pwm_FR, pwm_BL, pwm_BR], dtype=np.float32)\n",
-    "\n",
-    "        # Apply impulse disturbance if specified (uses environment method)\n",
-    "        if disturbance_step and step == disturbance_step:\n",
-    "            env.apply_impulse(disturbance_force)\n",
-    "            if verbose:\n",
-    "                print(f\"  Applied {disturbance_force}N impulse at step {step}\")\n",
-    "\n",
-    "        # Step environment (stochastic disturbance applied internally if enabled)\n",
-    "        obs, _, terminated, truncated, info = env.step(action)\n",
-    "\n",
-    "        # Store ground truth data from environment info (not sensor observations)\n",
-    "        data['time'].append(t)\n",
-    "        data['gap_front'].append(info['gap_front_yoke'] * 1000)  # Convert to mm\n",
-    "        data['gap_back'].append(info['gap_back_yoke'] * 1000)\n",
-    "        data['gap_avg'].append(info['gap_avg'] * 1000)  # Use ground truth from info\n",
-    "        data['roll_deg'].append(np.degrees(info['roll']))\n",
-    "        data['pitch_deg'].append(np.degrees(info['pitch']))  # Use ground truth from info\n",
-    "        data['current_FL'].append(info['curr_front_L'])\n",
-    "        data['current_FR'].append(info['curr_front_R'])\n",
-    "        data['current_BL'].append(info['curr_back_L'])\n",
-    "        data['current_BR'].append(info['curr_back_R'])\n",
-    "        data['current_total'].append(\n",
-    "            abs(info['curr_front_L']) + abs(info['curr_front_R']) +\n",
-    "            abs(info['curr_back_L']) + abs(info['curr_back_R'])\n",
-    "        )\n",
-    "        data['pwm_FL'].append(pwm_FL)\n",
-    "        data['pwm_FR'].append(pwm_FR)\n",
-    "        data['pwm_BL'].append(pwm_BL)\n",
-    "        data['pwm_BR'].append(pwm_BR)\n",
-    "\n",
-    "        if terminated or truncated:\n",
-    "            if verbose:\n",
-    "                print(f\"  Simulation ended at step {step} (terminated={terminated})\")\n",
-    "            break\n",
-    "\n",
-    "    env.close()\n",
-    "\n",
-    "    # Convert to numpy arrays\n",
-    "    for key in data:\n",
-    "        data[key] = np.array(data[key])\n",
-    "\n",
-    "    if verbose:\n",
-    "        print(f\"Simulation complete: {len(data['time'])} steps, {data['time'][-1]:.2f}s\")\n",
-    "        print(f\"  Final gap: {data['gap_avg'][-1]:.2f}mm (target: {target_gap*1000:.2f}mm)\")\n",
-    "        print(f\"  Final roll: {data['roll_deg'][-1]:.3f}°, pitch: {data['pitch_deg'][-1]:.3f}°\")\n",
-    "\n",
-    "    return data"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "---\n",
-    "## Plotting Functions"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "def plot_results(data: dict, title_suffix: str = \"\"):\n",
-    "    \"\"\"\n",
-    "    Create aesthetic plots of simulation results.\n",
-    "    \n",
-    "    Args:\n",
-    "        data: Dictionary from run_pid_simulation()\n",
-    "        title_suffix: Optional suffix for plot titles\n",
-    "    \"\"\"\n",
-    "    fig, axes = plt.subplots(2, 2, figsize=(14, 10))\n",
-    "    fig.suptitle(f'PID Control Performance{title_suffix}', fontsize=16, fontweight='bold', y=1.02)\n",
-    "    \n",
-    "    target_gap_mm = TARGET_GAP * 1000\n",
-    "    time = data['time']\n",
-    "    \n",
-    "    # Color palette\n",
-    "    colors = {\n",
-    "        'primary': '#2563eb',    # Blue\n",
-    "        'secondary': '#7c3aed',  # Purple\n",
-    "        'accent1': '#059669',    # Green\n",
-    "        'accent2': '#dc2626',    # Red\n",
-    "        'target': '#f59e0b',     # Orange\n",
-    "        'grid': '#e5e7eb'\n",
-    "    }\n",
-    "    \n",
-    "    # ========== Gap Height Plot ==========\n",
-    "    ax1 = axes[0, 0]\n",
-    "    ax1.plot(time, data['gap_avg'], color=colors['primary'], linewidth=2, label='Average Gap')\n",
-    "    ax1.plot(time, data['gap_front'], color=colors['secondary'], linewidth=1, alpha=0.6, label='Front Yoke')\n",
-    "    ax1.plot(time, data['gap_back'], color=colors['accent1'], linewidth=1, alpha=0.6, label='Back Yoke')\n",
-    "    ax1.axhline(y=target_gap_mm, color=colors['target'], linestyle='--', linewidth=2, label=f'Target ({target_gap_mm:.1f}mm)')\n",
-    "    \n",
-    "    ax1.set_xlabel('Time (s)')\n",
-    "    ax1.set_ylabel('Gap Height (mm)')\n",
-    "    ax1.set_title('Gap Height Over Time', fontweight='bold')\n",
-    "    ax1.legend(loc='best', framealpha=0.9)\n",
-    "    ax1.set_ylim([0, max(35, max(data['gap_avg']) * 1.1)])\n",
-    "    ax1.grid(True, alpha=0.3)\n",
-    "    \n",
-    "    # Add settling info\n",
-    "    final_error = abs(data['gap_avg'][-1] - target_gap_mm)\n",
-    "    ax1.text(0.98, 0.02, f'Final error: {final_error:.2f}mm', \n",
-    "             transform=ax1.transAxes, ha='right', va='bottom',\n",
-    "             fontsize=10, bbox=dict(boxstyle='round', facecolor='white', alpha=0.8))\n",
-    "    \n",
-    "    # ========== Roll Angle Plot ==========\n",
-    "    ax2 = axes[0, 1]\n",
-    "    ax2.plot(time, data['roll_deg'], color=colors['primary'], linewidth=2)\n",
-    "    ax2.axhline(y=0, color=colors['target'], linestyle='--', linewidth=1.5, alpha=0.7)\n",
-    "    ax2.fill_between(time, data['roll_deg'], 0, alpha=0.2, color=colors['primary'])\n",
-    "    \n",
-    "    ax2.set_xlabel('Time (s)')\n",
-    "    ax2.set_ylabel('Roll Angle (degrees)')\n",
-    "    ax2.set_title('Roll Angle Over Time', fontweight='bold')\n",
-    "    ax2.grid(True, alpha=0.3)\n",
-    "    \n",
-    "    max_roll = max(abs(data['roll_deg'].min()), abs(data['roll_deg'].max()))\n",
-    "    ax2.set_ylim([-max(1, max_roll * 1.2), max(1, max_roll * 1.2)])\n",
-    "    \n",
-    "    # ========== Pitch Angle Plot ==========\n",
-    "    ax3 = axes[1, 0]\n",
-    "    ax3.plot(time, data['pitch_deg'], color=colors['secondary'], linewidth=2)\n",
-    "    ax3.axhline(y=0, color=colors['target'], linestyle='--', linewidth=1.5, alpha=0.7)\n",
-    "    ax3.fill_between(time, data['pitch_deg'], 0, alpha=0.2, color=colors['secondary'])\n",
-    "    \n",
-    "    ax3.set_xlabel('Time (s)')\n",
-    "    ax3.set_ylabel('Pitch Angle (degrees)')\n",
-    "    ax3.set_title('Pitch Angle Over Time', fontweight='bold')\n",
-    "    ax3.grid(True, alpha=0.3)\n",
-    "    \n",
-    "    max_pitch = max(abs(data['pitch_deg'].min()), abs(data['pitch_deg'].max()))\n",
-    "    ax3.set_ylim([-max(1, max_pitch * 1.2), max(1, max_pitch * 1.2)])\n",
-    "    \n",
-    "    # ========== Current Draw Plot ==========\n",
-    "    ax4 = axes[1, 1]\n",
-    "    ax4.plot(time, data['current_FL'], linewidth=1.5, label='Front Left', alpha=0.8)\n",
-    "    ax4.plot(time, data['current_FR'], linewidth=1.5, label='Front Right', alpha=0.8)\n",
-    "    ax4.plot(time, data['current_BL'], linewidth=1.5, label='Back Left', alpha=0.8)\n",
-    "    ax4.plot(time, data['current_BR'], linewidth=1.5, label='Back Right', alpha=0.8)\n",
-    "    ax4.plot(time, data['current_total'], color='black', linewidth=2, label='Total', linestyle='--')\n",
-    "    \n",
-    "    ax4.set_xlabel('Time (s)')\n",
-    "    ax4.set_ylabel('Current (A)')\n",
-    "    ax4.set_title('Coil Current Draw Over Time', fontweight='bold')\n",
-    "    ax4.legend(loc='best', framealpha=0.9, ncol=2)\n",
-    "    ax4.grid(True, alpha=0.3)\n",
-    "    \n",
-    "    # Add average power info\n",
-    "    avg_total_current = np.mean(data['current_total'])\n",
-    "    ax4.text(0.98, 0.98, f'Avg total: {avg_total_current:.2f}A', \n",
-    "             transform=ax4.transAxes, ha='right', va='top',\n",
-    "             fontsize=10, bbox=dict(boxstyle='round', facecolor='white', alpha=0.8))\n",
-    "    \n",
-    "    plt.tight_layout()\n",
-    "    plt.show()\n",
-    "    \n",
-    "    return fig"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "def plot_control_signals(data: dict):\n",
-    "    \"\"\"\n",
-    "    Plot the PWM control signals sent to each coil.\n",
-    "    \n",
-    "    Args:\n",
-    "        data: Dictionary from run_pid_simulation()\n",
-    "    \"\"\"\n",
-    "    fig, ax = plt.subplots(figsize=(12, 5))\n",
-    "    \n",
-    "    time = data['time']\n",
-    "    \n",
-    "    ax.plot(time, data['pwm_FL'], label='Front Left', linewidth=1.5, alpha=0.8)\n",
-    "    ax.plot(time, data['pwm_FR'], label='Front Right', linewidth=1.5, alpha=0.8)\n",
-    "    ax.plot(time, data['pwm_BL'], label='Back Left', linewidth=1.5, alpha=0.8)\n",
-    "    ax.plot(time, data['pwm_BR'], label='Back Right', linewidth=1.5, alpha=0.8)\n",
-    "    \n",
-    "    ax.axhline(y=0, color='gray', linestyle='-', linewidth=0.5)\n",
-    "    ax.axhline(y=1, color='red', linestyle='--', linewidth=1, alpha=0.5, label='Saturation')\n",
-    "    ax.axhline(y=-1, color='red', linestyle='--', linewidth=1, alpha=0.5)\n",
-    "    \n",
-    "    ax.set_xlabel('Time (s)')\n",
-    "    ax.set_ylabel('PWM Duty Cycle')\n",
-    "    ax.set_title('Control Signals (PWM)', fontsize=14, fontweight='bold')\n",
-    "    ax.legend(loc='best', framealpha=0.9)\n",
-    "    ax.set_ylim([-1.2, 1.2])\n",
-    "    ax.grid(True, alpha=0.3)\n",
-    "    \n",
-    "    plt.tight_layout()\n",
-    "    plt.show()\n",
-    "    \n",
-    "    return fig"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "---\n",
-    "## Run Simulation\n",
-    "\n",
-    "**Configure the simulation parameters below and run the cell.**"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# ============================================================\n",
-    "#               SIMULATION PARAMETERS\n",
-    "# ============================================================\n",
-    "\n",
-    "INITIAL_GAP_MM = 14.0     # Starting gap height (mm). Target is ~16.49mm\n",
-    "MAX_STEPS = 2000          # Simulation steps (240 steps = 1 second)\n",
-    "USE_GUI = False           # Set to True to see PyBullet visualization\n",
-    "\n",
-    "# Impulse disturbance (one-time force at a specific step)\n",
-    "DISTURBANCE_STEP = None   # Step at which to apply impulse (e.g., 500). None = disabled\n",
-    "DISTURBANCE_FORCE = 0.0   # Impulse force in Newtons (positive = upward push)\n",
-    "\n",
-    "# Stochastic disturbance (continuous random noise each step)\n",
-    "DISTURBANCE_FORCE_STD = 0.0  # Std dev of random force noise (Newtons). 0 = disabled\n",
-    "\n",
-    "# ============================================================\n",
-    "\n",
-    "# Run simulation\n",
-    "results = run_pid_simulation(\n",
-    "    initial_gap_mm=INITIAL_GAP_MM,\n",
-    "    max_steps=MAX_STEPS,\n",
-    "    use_gui=USE_GUI,\n",
-    "    disturbance_step=DISTURBANCE_STEP,\n",
-    "    disturbance_force=DISTURBANCE_FORCE,\n",
-    "    disturbance_force_std=DISTURBANCE_FORCE_STD,\n",
-    "    verbose=True\n",
-    ")"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "---\n",
-    "## View Results"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Plot main performance metrics\n",
-    "plot_results(results)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Plot control signals (optional)\n",
-    "plot_control_signals(results)"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "---\n",
-    "## Compare Multiple Initial Conditions\n",
-    "\n",
-    "Run this cell to test the controller with different starting heights."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "def compare_initial_conditions(initial_gaps_mm: list, max_steps: int = 2000):\n",
-    "    \"\"\"\n",
-    "    Compare PID performance across different initial conditions.\n",
-    "    \n",
-    "    Args:\n",
-    "        initial_gaps_mm: List of starting gap heights to test\n",
-    "        max_steps: Maximum steps per simulation\n",
-    "    \"\"\"\n",
-    "    fig, axes = plt.subplots(2, 2, figsize=(14, 10))\n",
-    "    fig.suptitle('PID Performance Comparison: Different Initial Conditions', \n",
-    "                 fontsize=16, fontweight='bold', y=1.02)\n",
-    "    \n",
-    "    target_gap_mm = TARGET_GAP * 1000\n",
-    "    colors = plt.cm.viridis(np.linspace(0, 0.8, len(initial_gaps_mm)))\n",
-    "    \n",
-    "    all_results = []\n",
-    "    \n",
-    "    for i, gap in enumerate(initial_gaps_mm):\n",
-    "        print(f\"Running simulation {i+1}/{len(initial_gaps_mm)}: initial_gap={gap}mm\")\n",
-    "        data = run_pid_simulation(initial_gap_mm=gap, max_steps=max_steps, verbose=False)\n",
-    "        all_results.append((gap, data))\n",
-    "        \n",
-    "        label = f'{gap}mm'\n",
-    "        \n",
-    "        # Gap height\n",
-    "        axes[0, 0].plot(data['time'], data['gap_avg'], color=colors[i], \n",
-    "                        linewidth=2, label=label)\n",
-    "        \n",
-    "        # Roll\n",
-    "        axes[0, 1].plot(data['time'], data['roll_deg'], color=colors[i], \n",
-    "                        linewidth=2, label=label)\n",
-    "        \n",
-    "        # Pitch\n",
-    "        axes[1, 0].plot(data['time'], data['pitch_deg'], color=colors[i], \n",
-    "                        linewidth=2, label=label)\n",
-    "        \n",
-    "        # Total current\n",
-    "        axes[1, 1].plot(data['time'], data['current_total'], color=colors[i], \n",
-    "                        linewidth=2, label=label)\n",
-    "    \n",
-    "    # Add target line to gap plot\n",
-    "    axes[0, 0].axhline(y=target_gap_mm, color='red', linestyle='--', \n",
-    "                       linewidth=2, label=f'Target ({target_gap_mm:.1f}mm)')\n",
-    "    \n",
-    "    # Configure axes\n",
-    "    axes[0, 0].set_xlabel('Time (s)')\n",
-    "    axes[0, 0].set_ylabel('Gap Height (mm)')\n",
-    "    axes[0, 0].set_title('Average Gap Height', fontweight='bold')\n",
-    "    axes[0, 0].legend(loc='best')\n",
-    "    axes[0, 0].grid(True, alpha=0.3)\n",
-    "    \n",
-    "    axes[0, 1].axhline(y=0, color='gray', linestyle='--', linewidth=1)\n",
-    "    axes[0, 1].set_xlabel('Time (s)')\n",
-    "    axes[0, 1].set_ylabel('Roll Angle (degrees)')\n",
-    "    axes[0, 1].set_title('Roll Angle', fontweight='bold')\n",
-    "    axes[0, 1].legend(loc='best')\n",
-    "    axes[0, 1].grid(True, alpha=0.3)\n",
-    "    \n",
-    "    axes[1, 0].axhline(y=0, color='gray', linestyle='--', linewidth=1)\n",
-    "    axes[1, 0].set_xlabel('Time (s)')\n",
-    "    axes[1, 0].set_ylabel('Pitch Angle (degrees)')\n",
-    "    axes[1, 0].set_title('Pitch Angle', fontweight='bold')\n",
-    "    axes[1, 0].legend(loc='best')\n",
-    "    axes[1, 0].grid(True, alpha=0.3)\n",
-    "    \n",
-    "    axes[1, 1].set_xlabel('Time (s)')\n",
-    "    axes[1, 1].set_ylabel('Total Current (A)')\n",
-    "    axes[1, 1].set_title('Total Current Draw', fontweight='bold')\n",
-    "    axes[1, 1].legend(loc='best')\n",
-    "    axes[1, 1].grid(True, alpha=0.3)\n",
-    "    \n",
-    "    plt.tight_layout()\n",
-    "    plt.show()\n",
-    "    \n",
-    "    return all_results"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Compare different starting heights\n",
-    "# Uncomment and run to test multiple initial conditions\n",
-    "\n",
-    "# comparison_results = compare_initial_conditions(\n",
-    "#     initial_gaps_mm=[10.0, 14.0, 18.0, 22.0],\n",
-    "#     max_steps=2000\n",
-    "# )"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "---\n",
-    "## Test Disturbance Rejection\n",
-    "\n",
-    "Apply a sudden force disturbance and observe recovery."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Test disturbance rejection\n",
-    "# Uncomment and run to test disturbance response\n",
-    "\n",
-    "# Example 1: Impulse disturbance (one-time force)\n",
-    "# disturbance_results = run_pid_simulation(\n",
-    "#     initial_gap_mm=16.5,         # Start near target\n",
-    "#     max_steps=3000,\n",
-    "#     use_gui=False,\n",
-    "#     disturbance_step=720,        # Apply at 3 seconds\n",
-    "#     disturbance_force=-20.0,     # 20N downward push\n",
-    "#     verbose=True\n",
-    "# )\n",
-    "# plot_results(disturbance_results, title_suffix=' (with 20N impulse at t=3s)')\n",
-    "\n",
-    "# Example 2: Continuous stochastic noise\n",
-    "# noisy_results = run_pid_simulation(\n",
-    "#     initial_gap_mm=16.5,\n",
-    "#     max_steps=3000,\n",
-    "#     use_gui=False,\n",
-    "#     disturbance_force_std=2.0,   # 2N standard deviation continuous noise\n",
-    "#     verbose=True\n",
-    "# )\n",
-    "# plot_results(noisy_results, title_suffix=' (with 2N std continuous noise)')"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "---\n",
-    "## Summary Statistics"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "def print_summary(data: dict):\n",
-    "    \"\"\"Print summary statistics for a simulation run.\"\"\"\n",
-    "    target_gap_mm = TARGET_GAP * 1000\n",
-    "    \n",
-    "    # Calculate settling time (within 2% of target)\n",
-    "    tolerance = 0.02 * target_gap_mm\n",
-    "    settled_idx = None\n",
-    "    for i in range(len(data['gap_avg'])):\n",
-    "        if abs(data['gap_avg'][i] - target_gap_mm) < tolerance:\n",
-    "            # Check if it stays settled\n",
-    "            if all(abs(data['gap_avg'][j] - target_gap_mm) < tolerance \n",
-    "                   for j in range(i, min(i+100, len(data['gap_avg'])))):\n",
-    "                settled_idx = i\n",
-    "                break\n",
-    "    \n",
-    "    print(\"=\" * 50)\n",
-    "    print(\"SIMULATION SUMMARY\")\n",
-    "    print(\"=\" * 50)\n",
-    "    print(f\"Duration: {data['time'][-1]:.2f} seconds ({len(data['time'])} steps)\")\n",
-    "    print(f\"Target gap: {target_gap_mm:.2f} mm\")\n",
-    "    print()\n",
-    "    print(\"Gap Height:\")\n",
-    "    print(f\"  Initial:  {data['gap_avg'][0]:.2f} mm\")\n",
-    "    print(f\"  Final:    {data['gap_avg'][-1]:.2f} mm\")\n",
-    "    print(f\"  Error:    {abs(data['gap_avg'][-1] - target_gap_mm):.3f} mm\")\n",
-    "    print(f\"  Max:      {max(data['gap_avg']):.2f} mm\")\n",
-    "    print(f\"  Min:      {min(data['gap_avg']):.2f} mm\")\n",
-    "    if settled_idx:\n",
-    "        print(f\"  Settling time (2%): {data['time'][settled_idx]:.3f} s\")\n",
-    "    else:\n",
-    "        print(f\"  Settling time: Not settled within tolerance\")\n",
-    "    print()\n",
-    "    print(\"Orientation (final):\")\n",
-    "    print(f\"  Roll:  {data['roll_deg'][-1]:+.3f} degrees\")\n",
-    "    print(f\"  Pitch: {data['pitch_deg'][-1]:+.3f} degrees\")\n",
-    "    print()\n",
-    "    print(\"Current Draw:\")\n",
-    "    print(f\"  Average total: {np.mean(data['current_total']):.2f} A\")\n",
-    "    print(f\"  Peak total:    {max(data['current_total']):.2f} A\")\n",
-    "    print(\"=\" * 50)\n",
-    "\n",
-    "# Print summary for last simulation\n",
-    "print_summary(results)"
-   ]
-  }
- ],
- "metadata": {
-  "kernelspec": {
-   "display_name": "Python 3",
-   "language": "python",
-   "name": "python3"
-  },
-  "language_info": {
-   "name": "python",
-   "version": "3.11.0"
-  }
- },
- "nbformat": 4,
- "nbformat_minor": 4
-}

RL Testing/requirements.txt

@@ -1,156 +0,0 @@
-annotated-types==0.7.0
-anyio==4.10.0
-appnope==0.1.4
-argon2-cffi==25.1.0
-argon2-cffi-bindings==25.1.0
-arrow==1.3.0
-asttokens==3.0.0
-async-lru==2.0.5
-attrs==25.3.0
-babel==2.17.0
-beautifulsoup4==4.13.5
-bleach==6.2.0
-cachetools==6.2.1
-certifi==2025.8.3
-cffi==1.17.1
-charset-normalizer==3.4.3
-click==8.3.0
-comm==0.2.3
-contourpy==1.3.3
-cycler==0.12.1
-debugpy==1.8.16
-decorator==5.2.1
-defusedxml==0.7.1
-executing==2.2.1
-fastapi==0.115.0
-fastjsonschema==2.21.2
-fonttools==4.59.2
-fqdn==1.5.1
-future==1.0.0
-google-ai-generativelanguage==0.6.10
-google-api-core==2.26.0
-google-api-python-client==2.185.0
-google-auth==2.41.1
-google-auth-httplib2==0.2.0
-google-generativeai==0.8.3
-googleapis-common-protos==1.70.0
-grpcio==1.75.1
-grpcio-status==1.71.2
-h11==0.16.0
-httpcore==1.0.9
-httplib2==0.31.0
-httpx==0.27.2
-idna==3.10
-ipykernel==6.30.1
-ipython==9.5.0
-ipython_pygments_lexers==1.1.1
-ipywidgets==8.1.7
-iso8601==2.1.0
-isoduration==20.11.0
-jedi==0.19.2
-Jinja2==3.1.6
-json5==0.12.1
-jsonpointer==3.0.0
-jsonschema==4.25.1
-jsonschema-specifications==2025.4.1
-jupyter==1.1.1
-jupyter-console==6.6.3
-jupyter-events==0.12.0
-jupyter-lsp==2.3.0
-jupyter_client==8.6.3
-jupyter_core==5.8.1
-jupyter_server==2.17.0
-jupyter_server_terminals==0.5.3
-jupyterlab==4.4.7
-jupyterlab_pygments==0.3.0
-jupyterlab_server==2.27.3
-jupyterlab_widgets==3.0.15
-kiwisolver==1.4.9
-lark==1.2.2
-lazy_loader==0.4
-MarkupSafe==3.0.2
-matplotlib==3.10.6
-matplotlib-inline==0.1.7
-mistune==3.1.4
-mne==1.10.2
-mpmath==1.3.0
-nbclient==0.10.2
-nbconvert==7.16.6
-nbformat==5.10.4
-nest-asyncio==1.6.0
-notebook==7.4.5
-notebook_shim==0.2.4
-numpy==2.3.2
-packaging==25.0
-pandas==2.3.3
-pandocfilters==1.5.1
-parso==0.8.5
-pexpect==4.9.0
-pillow==11.3.0
-platformdirs==4.4.0
-pooch==1.8.2
-prometheus_client==0.22.1
-prompt_toolkit==3.0.52
-proto-plus==1.26.1
-protobuf==5.29.5
-psutil==7.0.0
-ptyprocess==0.7.0
-pure_eval==0.2.3
-pyasn1==0.6.1
-pyasn1_modules==0.4.2
-pycparser==2.22
-pydantic==2.9.2
-pydantic-settings==2.6.0
-pydantic_core==2.23.4
-Pygments==2.19.2
-pyparsing==3.2.3
-PyQt6==6.10.0
-PyQt6-Qt6==6.10.0
-PyQt6_sip==13.10.2
-pyqtgraph==0.13.7
-pyserial==3.5
-PySide6==6.10.0
-PySide6_Addons==6.10.0
-PySide6_Essentials==6.10.0
-python-dateutil==2.9.0.post0
-python-dotenv==1.0.1
-python-json-logger==3.3.0
-pytz==2025.2
-PyYAML==6.0.2
-pyzmq==27.0.2
-referencing==0.36.2
-requests==2.32.5
-rfc3339-validator==0.1.4
-rfc3986-validator==0.1.1
-rfc3987-syntax==1.1.0
-rpds-py==0.27.1
-rsa==4.9.1
-scipy==1.16.3
-Send2Trash==1.8.3
-serial==0.0.97
-setuptools==80.9.0
-shiboken6==6.10.0
-six==1.17.0
-sniffio==1.3.1
-soupsieve==2.8
-stack-data==0.6.3
-starlette==0.38.6
-sympy==1.14.0
-terminado==0.18.1
-tinycss2==1.4.0
-tornado==6.5.2
-tqdm==4.67.1
-traitlets==5.14.3
-types-python-dateutil==2.9.0.20250822
-typing_extensions==4.15.0
-tzdata==2025.2
-uri-template==1.3.0
-uritemplate==4.2.0
-urllib3==2.5.0
-uvicorn==0.32.0
-wcwidth==0.2.13
-webcolors==24.11.1
-webencodings==0.5.1
-websocket-client==1.8.0
-websockets==15.0.1
-widgetsnbextension==4.0.14

ReadMe.md

@@ -1,6 +1,115 @@
 # Levitation Control Repository
-This Repo contains the code and data from sensor calibration as well as arduino code for 4-point control on the 2-yoke test rig in the ```PIDTesting-2yoke4coil``` directory.
 
-This code is property of Texas Guadaloop, a student-led hyperloop team from the University of Texas at Austin.
+This repository contains embedded firmware, sensor characterization tooling, and a physics-based simulation stack as well as control algorithm trials for the Texas Guadaloop maglev system.
 
-To be able to run all python files in the repo, run ```pip install -r requirements.txt```
+This code is property of **Texas Guadaloop**, a student-led hyperloop team from the University of Texas at Austin.
+
+---
+
+## Repository Structure
+
+```
+.
+├── AltSensorTesting/       # Arduino firmware for Baumer inductive sensor characterization
+├── sensor/                 # Python scripts to collect and fit sensor calibration data
+├── loadCellCode/           # Arduino firmware for HX711 load-cell calibration
+├── TwoCellMagChar/         # Two-cell magnetic characterization rig (validates Ansys data)
+├── lev_sim/                # PyBullet simulation driven by Ansys sweep data
+├── MAGLEV_DIGITALTWIN_PYTHON/  # Earlier single-axis analytical digital twin (reference)
+├── serial_plotter.py       # Live serial plotter utility
+├── equilibrium.py          # Equilibrium gap/current solver
+└── requirements.txt        # Python dependencies
+```
+
+---
+
+## Embedded Stack
+
+### `AltSensorTesting/`
+
+Arduino firmware for reading Baumer inductive analog distance sensors. Uses interrupt-driven ADC sampling at ~77 kHz (16 MHz / prescaler 16) for low-latency gap measurement. Tracks the 10 lowest and 10 highest in-range ADC values over a sampling window to help establish calibration bounds. An out-of-range (OOR) digital pin is monitored in the ISR to discard invalid readings.
+
+Key details:
+- ADC ISR at ~77 kHz; readings discarded automatically when OOR pin is HIGH
+- Serial commands: `1` to start sampling, `0` to stop and print boundary statistics
+- Baud rate: 2,000,000
+
+
+
+---
+
+## Sensor Characterization
+
+### `sensor/`
+
+Python pipeline for converting raw ADC readings from the inductive gap sensors into calibrated millimeter distances.
+
+- **`sensorCollector.py`** — Serial interface that reads live ADC values from Arduino and applies the calibration model in real time.
+- **`analogFitter-*.py`** — Curve-fitting scripts (polynomial, exponential, 3/4/5-parameter logistic) that fit calibration sweep data (`data*.csv`) to find the best sensor model. The 5-parameter generalized logistic form was found to give the best fit.
+- **`Sensor*Averages.csv` / `data*.csv`** — Raw and averaged calibration data for sensors 0–3. Sensor 3 required a different voltage divider (20 kΩ / 50 kΩ) because the induction sensor output exceeds 6 V.
+
+Calibration constants (A, K, B, C, v) for the generalized logistic model are embedded directly in `sensorCollector.py` and `sensor_simplerNew.py` for deployment.
+
+---
+
+## Magnetic Characterization (`TwoCellMagChar/`)
+
+A minimal bench rig used to validate Ansys Maxwell FEA force predictions experimentally.
+
+- **`TwoCellMagChar.ino`** — Drives two H-bridge coil channels across a sweep of PWM values (−250 to +250 in steps of 50) while reading two HX711 load cells simultaneously. Averages 10 measurements per PWM step and reports gram-force readings over serial.
+- **`MagCharTrial.xlsx`** — Recorded force-vs-PWM data from physical trials.
+- **`CalibConsts.hpp`** — Load-cell offset and scale constants shared with `loadCellCode/`.
+
+The measured force curves confirmed that the Ansys sweep data is in reasonable agreement with physical hardware, providing confidence for using the FEA model inside the simulation.
+
+---
+
+## Levitation Simulation (`lev_sim/`)
+
+A PyBullet-based simulation environment for 4-point active magnetic levitation of the pod. The simulation is driven by an Ansys Maxwell parametric sweep (coil currents × gap height × roll angle → force & torque), fitted to a polynomial regression model for fast inference.
+
+### Data pipeline
+
+1. **Ansys sweep** — `Ansys Results 12-9.csv / .xlsx` contains FEA results sweeping left/right coil currents, roll angle, and gap height.
+2. **Function Fitting** — `Function Fitting.ipynb` fits the Ansys data to a `PolynomialFeatures + LinearRegression` model (inputs: `currL`, `currR`, `roll`, `1/gap`). The trained model is saved to `maglev_model.pkl`.
+3. **Fast inference** — `maglev_predictor.py` (`MaglevPredictor`) loads the pickle and bypasses sklearn overhead by extracting raw weight matrices, running pure-NumPy polynomial expansion for ~100× faster per-sample prediction.
+
+### Simulation environment
+
+`lev_pod_env.py` implements a [Gymnasium](https://gymnasium.farama.org/) `Env` wrapping PyBullet:
+
+- **State**: 4 gap heights (normalized) + 4 gap-height velocities
+- **Action**: 4 PWM duty cycles ∈ [−1, 1] (front-left, front-right, back-left, back-right coils)
+- **Physics**: first-order RL circuit model per coil (`mag_lev_coil.py`); coil parameters: R = 1.1 Ω, L = 2.5 mH, V_supply = 12 V, I_max = 10.2 A
+- **Forces**: `MaglevPredictor.predict()` maps coil currents + gap + roll → force and torque applied to the pod body in PyBullet
+- **Noise**: configurable Gaussian sensor noise (default σ = 0.1 mm) and stochastic disturbance forces
+- Target equilibrium gap: 11.86 mm (9.4 kg pod)
+- Simulation timestep: 1/240 s
+
+The environment is controller-agnostic — any control algorithm can be plugged in:
+
+| File | Controller |
+|------|-----------|
+| `lev_PID.ipynb` | Interactive PID tuning notebook |
+| `lev_PPO.ipynb` | PPO reinforcement learning via Stable-Baselines3 |
+
+### Supplementary Files for PID:
+
+`pid_simulation.py`: Feedforward LUT + PID
+`optuna_pid_tune.py`: Optuna-based automated PID hyperparameter search
+
+Pre-tuned PID gain sets are saved in `pid_best_params*.json` (variants for 1500, 3000, and 6000 Optuna trials).
+
+### PWM circuit model
+
+`PWM_Circuit_Model.py` is a standalone electrical model of the H-bridge PWM drive circuit used for offline verification of the coil current dynamics.
+
+---
+
+## Setup
+
+```bash
+pip install -r requirements.txt
+```
+
+Key dependencies: `pybullet`, `gymnasium`, `stable-baselines3`, `scikit-learn`, `optuna`, `pyserial`, `numpy`, `scipy`, `pandas`, `matplotlib`.

embedded/AdditiveControlCode/AdditiveControlCode.ino

@@ -0,0 +1,139 @@
+#include <Arduino.h>
+#include "IndSensorMap.hpp"
+#include "Controller.hpp"
+#include "ADC.hpp"
+#include "FastPWM.hpp"
+
+// ── PID Gains (Kp, Ki, Kd) ──────────────────────────────────
+// Height loop: controls average gap → additive PWM on all coils
+PIDGains heightGains = { 100.0f, 0.0f, 8.0f };
+
+// Roll loop: corrects left/right tilt → differential L/R
+PIDGains rollGains   = { 0.6f, 0.0f, -0.1f };
+
+// Pitch loop: corrects front/back tilt → differential F/B
+PIDGains pitchGains  = { 50.0f, 0.0f, 1.9f };
+
+// ── Reference ────────────────────────────────────────────────
+float avgRef = 12.36f;  // Target gap height (mm) — 9.4 kg equilibrium
+
+// ── Feedforward ──────────────────────────────────────────────
+bool useFeedforward = true;  // Set false to disable feedforward LUT
+
+// ── Sampling ─────────────────────────────────────────────────
+#define SAMPLING_RATE 200  // Hz (controller tick rate)
+
+// ── EMA filter alpha (all sensors) ───────────────────────────
+#define ALPHA_VAL 1.0f
+
+// ═══════════════════════════════════════════════════════════════
+// ABOVE THIS LINE IS TUNING VALUES ONLY, BELOW IS ACTUAL CODE.
+// ═══════════════════════════════════════════════════════════════
+
+unsigned long tprior;
+unsigned int tDiffMicros;
+
+FullController controller(indL, indR, indF, indB,
+                          heightGains, rollGains, pitchGains,
+                          avgRef, useFeedforward);
+
+const int dt_micros = 1000000 / SAMPLING_RATE;
+
+int ON = 0;
+
+void setup() {
+  Serial.begin(2000000);
+  setupADC();
+  setupFastPWM();
+
+  indL.alpha = ALPHA_VAL;
+  indR.alpha = ALPHA_VAL;
+  indF.alpha = ALPHA_VAL;
+  indB.alpha = ALPHA_VAL;
+
+  tprior = micros();
+
+  pinMode(dirFL, OUTPUT);
+  pinMode(pwmFL, OUTPUT);
+  pinMode(dirBL, OUTPUT);
+  pinMode(pwmBL, OUTPUT);
+  pinMode(dirFR, OUTPUT);
+  pinMode(pwmFR, OUTPUT);
+  pinMode(dirBR, OUTPUT);
+  pinMode(pwmBR, OUTPUT);
+}
+
+void loop() {
+  if (Serial.available() > 0) {
+    String cmd = Serial.readStringUntil('\n');
+    cmd.trim();
+    
+    // REF,avgRef — update target gap height
+    if (cmd.startsWith("REF,")) {
+      float newRef = cmd.substring(4).toFloat();
+      avgRef = newRef;
+      controller.updateReference(avgRef);
+      Serial.print("Updated Ref: ");
+      Serial.println(avgRef);
+    }
+    // PID,loop,kp,ki,kd — update gains (loop: 0=height, 1=roll, 2=pitch)
+    else if (cmd.startsWith("PID,")) {
+      int c1 = cmd.indexOf(',');
+      int c2 = cmd.indexOf(',', c1 + 1);
+      int c3 = cmd.indexOf(',', c2 + 1);
+      int c4 = cmd.indexOf(',', c3 + 1);
+      
+      if (c1 > 0 && c2 > 0 && c3 > 0 && c4 > 0) {
+        int loop = cmd.substring(c1 + 1, c2).toInt();
+        float kp  = cmd.substring(c2 + 1, c3).toFloat();
+        float ki  = cmd.substring(c3 + 1, c4).toFloat();
+        float kd  = cmd.substring(c4 + 1).toFloat();
+        
+        PIDGains g = { kp, ki, kd };
+        
+        switch (loop) {
+          case 0:
+            heightGains = g;
+            controller.updateHeightPID(g);
+            Serial.println("Updated Height PID");
+            break;
+          case 1:
+            rollGains = g;
+            controller.updateRollPID(g);
+            Serial.println("Updated Roll PID");
+            break;
+          case 2:
+            pitchGains = g;
+            controller.updatePitchPID(g);
+            Serial.println("Updated Pitch PID");
+            break;
+          default:
+            Serial.println("Invalid loop (0=height, 1=roll, 2=pitch)");
+            break;
+        }
+      }
+    }
+    // FF,0 or FF,1 — toggle feedforward
+    else if (cmd.startsWith("FF,")) {
+      bool en = (cmd.charAt(3) != '0');
+      useFeedforward = en;
+      controller.setFeedforward(en);
+      Serial.print("Feedforward: ");
+      Serial.println(en ? "ON" : "OFF");
+    }
+    else {
+      // Original on/off command (any char except '0' turns on)
+      controller.outputOn = (cmd.charAt(0) != '0');
+    }
+  }
+  
+  tDiffMicros = micros() - tprior;
+
+  if (tDiffMicros >= dt_micros) {
+    controller.update();
+    controller.report();
+    controller.sendOutputs();
+    
+    tprior = micros();
+  }
+}

embedded/lib/Controller.cpp

@@ -0,0 +1,181 @@
+#include "Controller.hpp"
+#include <Arduino.h>
+#include <math.h>
+
+// ── Integral windup limit ────────────────────────────────────
+static const float MAX_INTEGRAL = 1e4f;
+
+// ── Feedforward LUT in PROGMEM ───────────────────────────────
+// Generated by gen_ff_lut.py  (pod 9.4 kg, R=1.1Ω, V=12V, 3–20 mm)
+// Positive = repelling, Negative = attracting
+static const int16_t FF_PWM_LUT[FF_LUT_SIZE] PROGMEM = {
+   238,  238,  238,  238,  238,  238,  238,  238,
+   238,  238,  238,  238,  238,  238,  238,  238,
+   238,  238,  234,  219,  204,  188,  172,  157,
+   141,  125,  109,   93,   77,   61,   45,   29,
+    13,   -3,  -19,  -35,  -51,  -67,  -84, -100,
+  -116, -133, -150, -166, -183, -200, -217, -234,
+  -250, -250, -250, -250, -250, -250, -250, -250,
+  -250, -250, -250, -250, -250, -250, -250, -250
+};
+
+// ── Constructor ──────────────────────────────────────────────
+FullController::FullController(
+    IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
+    PIDGains hGains, PIDGains rGains, PIDGains pGains,
+    float avgRef, bool useFeedforward)
+  : Left(l), Right(r), Front(f), Back(b),
+    heightGains(hGains), rollGains(rGains), pitchGains(pGains),
+    heightErr({0,0,0}), rollErr({0,0,0}), pitchErr({0,0,0}),
+    AvgRef(avgRef), avg(0), ffEnabled(useFeedforward),
+    oor(false), outputOn(false),
+    FLPWM(0), BLPWM(0), FRPWM(0), BRPWM(0)
+{}
+
+// ── Main update (called each control tick) ───────────────────
+void FullController::update() {
+  // 1. Read all sensors (updates mmVal, oor)
+  Left.readMM();
+  Right.readMM();
+  Front.readMM();
+  Back.readMM();
+
+  oor = Left.oor || Right.oor || Front.oor || Back.oor;
+
+  // 2. Compute average gap (mm)
+  avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f;
+
+  // 3. Feedforward: base PWM from equilibrium LUT
+  int16_t ffPWM = ffEnabled ? feedforward(avg) : 0;
+
+  // 4. Height PID: error = reference - average gap
+  float heightE = AvgRef - avg;
+  heightErr.eDiff = heightE - heightErr.e;
+  if (!oor) {
+    heightErr.eInt += heightE;
+    heightErr.eInt = constrain(heightErr.eInt, -MAX_INTEGRAL, MAX_INTEGRAL);
+  }
+  heightErr.e = heightE;
+  int16_t heightPWM = pidCompute(heightGains, heightErr, CAP);
+
+  // 5. Roll PID: angle-based error (matches simulation)
+  //    roll_angle = asin((left - right) / y_distance)
+  //    error = -roll_angle  (drive roll toward zero)
+  float rollRatio = (Left.mmVal - Right.mmVal) / Y_DISTANCE_MM;
+  rollRatio = constrain(rollRatio, -1.0f, 1.0f);
+  float rollAngle = asinf(rollRatio);
+  float rollE = -rollAngle;
+
+  rollErr.eDiff = rollE - rollErr.e;
+  if (!oor) {
+    rollErr.eInt += rollE;
+    rollErr.eInt = constrain(rollErr.eInt, -MAX_INTEGRAL, MAX_INTEGRAL);
+  }
+  rollErr.e = rollE;
+  int16_t rollPWM = pidCompute(rollGains, rollErr, CAP / 2);
+
+  // 6. Pitch PID: angle-based error (matches simulation)
+  //    pitch_angle = asin((back - front) / x_distance)
+  //    error = -pitch_angle  (drive pitch toward zero)
+  float pitchRatio = (Back.mmVal - Front.mmVal) / X_DISTANCE_MM;
+  pitchRatio = constrain(pitchRatio, -1.0f, 1.0f);
+  float pitchAngle = asinf(pitchRatio);
+  float pitchE = -pitchAngle;
+
+  pitchErr.eDiff = pitchE - pitchErr.e;
+  if (!oor) {
+    pitchErr.eInt += pitchE;
+    pitchErr.eInt = constrain(pitchErr.eInt, -MAX_INTEGRAL, MAX_INTEGRAL);
+  }
+  pitchErr.e = pitchE;
+  int16_t pitchPWM = pidCompute(pitchGains, pitchErr, CAP / 2);
+
+  // 7. Mix outputs (same sign convention as simulation)
+  //    FL: ff + height - roll - pitch
+  //    FR: ff + height + roll - pitch
+  //    BL: ff + height - roll + pitch
+  //    BR: ff + height + roll + pitch
+  FLPWM = constrain(ffPWM + heightPWM - rollPWM - pitchPWM, -CAP, CAP);
+  FRPWM = constrain(ffPWM + heightPWM + rollPWM - pitchPWM, -CAP, CAP);
+  BLPWM = constrain(ffPWM + heightPWM - rollPWM + pitchPWM, -CAP, CAP);
+  BRPWM = constrain(ffPWM + heightPWM + rollPWM + pitchPWM, -CAP, CAP);
+}
+
+// ── PID compute (single symmetric set of gains) ─────────────
+int16_t FullController::pidCompute(PIDGains& gains, PIDState& state, float maxOutput) {
+  if (oor) return 0;
+  float out = gains.kp * state.e
+            + gains.ki * state.eInt
+            + gains.kd * state.eDiff;
+  return (int16_t)constrain(out, -maxOutput, maxOutput);
+}
+
+// ── Feedforward: linearly interpolate PROGMEM LUT ────────────
+int16_t FullController::feedforward(float gapMM) {
+  if (gapMM <= FF_GAP_MIN) return (int16_t)pgm_read_word(&FF_PWM_LUT[0]);
+  if (gapMM >= FF_GAP_MAX) return (int16_t)pgm_read_word(&FF_PWM_LUT[FF_LUT_SIZE - 1]);
+
+  float idx_f = (gapMM - 1.0 - FF_GAP_MIN) / FF_GAP_STEP;
+  uint8_t idx = (uint8_t)idx_f;
+  if (idx >= FF_LUT_SIZE - 1) idx = FF_LUT_SIZE - 2;
+
+  int16_t v0 = (int16_t)pgm_read_word(&FF_PWM_LUT[idx]);
+  int16_t v1 = (int16_t)pgm_read_word(&FF_PWM_LUT[idx + 1]);
+  float frac = idx_f - (float)idx;
+
+  return (int16_t)(v0 + frac * (v1 - v0));
+}
+
+// ── Zero all PWMs ────────────────────────────────────────────
+void FullController::zeroPWMs() {
+  FLPWM = 0;
+  BLPWM = 0;
+  FRPWM = 0;
+  BRPWM = 0;
+}
+
+// ── Send PWM values to hardware ──────────────────────────────
+void FullController::sendOutputs() {
+  if (!outputOn) {
+    zeroPWMs();
+  }
+
+  // Direction: LOW = repelling (positive PWM), HIGH = attracting (negative PWM)
+  // Using direct register writes for fast PWM mode set by setupFastPWM()
+  digitalWrite(dirFL, FLPWM < 0);
+  OCR2A = abs(FLPWM); // Pin 11 → Timer 2A
+  digitalWrite(dirBL, BLPWM < 0);
+  OCR1A = abs(BLPWM); // Pin 9  → Timer 1A
+  digitalWrite(dirFR, FRPWM < 0);
+  OCR2B = abs(FRPWM); // Pin 3  → Timer 2B
+  digitalWrite(dirBR, BRPWM < 0);
+  OCR1B = abs(BRPWM); // Pin 10 → Timer 1B
+}
+
+// ── Serial report ────────────────────────────────────────────
+void FullController::report() {
+  // CSV: Left,Right,Front,Back,Avg,FLPWM,BLPWM,FRPWM,BRPWM,ControlOn
+  Serial.print(Left.mmVal);  Serial.print(',');
+  Serial.print(Right.mmVal); Serial.print(',');
+  Serial.print(Front.mmVal); Serial.print(',');
+  Serial.print(Back.mmVal);  Serial.print(',');
+  Serial.print(avg);         Serial.print(',');
+  Serial.print(FLPWM);      Serial.print(',');
+  Serial.print(BLPWM);      Serial.print(',');
+  Serial.print(FRPWM);      Serial.print(',');
+  Serial.print(BRPWM);      Serial.print(',');
+  Serial.println(outputOn);
+}
+
+// ── Runtime tuning methods ───────────────────────────────────
+void FullController::updateHeightPID(PIDGains gains) { heightGains = gains; }
+void FullController::updateRollPID(PIDGains gains)   { rollGains   = gains; }
+void FullController::updatePitchPID(PIDGains gains)  { pitchGains  = gains; }
+
+void FullController::updateReference(float avgReference) {
+  AvgRef = avgReference;
+}
+
+void FullController::setFeedforward(bool enabled) {
+  ffEnabled = enabled;
+}

embedded/lib/Controller.hpp

@@ -0,0 +1,99 @@
+#ifndef CONTROLLER_HPP
+#define CONTROLLER_HPP
+
+#include <stdint.h>
+#include <avr/pgmspace.h>
+#include "IndSensorMap.hpp"
+
+// ── Pin Mapping ──────────────────────────────────────────────
+#define dirBL 2
+#define pwmBL 3
+#define dirBR 4
+#define pwmBR 10
+#define pwmFL 11
+#define dirFL 7
+#define dirFR 8
+#define pwmFR 9
+
+// ── Output Cap ───────────────────────────────────────────────
+#define CAP 250   // Max PWM magnitude (0-255 Arduino range)
+
+// ── PID Gains (single set per loop — matches simulation) ────
+typedef struct PIDGains {
+  float kp;
+  float ki;
+  float kd;
+} PIDGains;
+
+// ── PID Error State ──────────────────────────────────────────
+typedef struct PIDState {
+  float e;        // Current error
+  float eDiff;    // Derivative of error (e[k] - e[k-1])
+  float eInt;     // Integral of error (accumulated)
+} PIDState;
+
+// ── Feedforward LUT (PROGMEM) ────────────────────────────────
+// Generated by gen_ff_lut.py from MaglevPredictor model
+// Pod mass: 9.4 kg, Coil R: 1.1Ω, V_supply: 12V
+// Positive = repelling, Negative = attracting
+// Beyond ~16mm: clamped to -CAP (no equilibrium exists)
+#define FF_LUT_SIZE 64
+#define FF_GAP_MIN  3.0f
+#define FF_GAP_MAX  20.0f
+#define FF_GAP_STEP 0.269841f
+
+// ── Geometry (mm, matching simulation) ───────────────────────
+// Sensor-to-sensor distances for angle computation
+#define Y_DISTANCE_MM 101.6f   // Left↔Right sensor spacing (mm)
+#define X_DISTANCE_MM 251.8f   // Front↔Back sensor spacing (mm)
+
+// ── Controller Class ─────────────────────────────────────────
+class FullController {
+public:
+  bool oor;        // Any sensor out-of-range
+  bool outputOn;   // Enable/disable output
+
+  FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
+                 PIDGains heightGains, PIDGains rollGains, PIDGains pitchGains,
+                 float avgRef, bool useFeedforward);
+
+  void update();
+  void zeroPWMs();
+  void sendOutputs();
+  void report();
+
+  // Runtime tuning
+  void updateHeightPID(PIDGains gains);
+  void updateRollPID(PIDGains gains);
+  void updatePitchPID(PIDGains gains);
+  void updateReference(float avgReference);
+  void setFeedforward(bool enabled);
+
+private:
+  int16_t pidCompute(PIDGains& gains, PIDState& state, float maxOutput);
+  int16_t feedforward(float gapMM);
+
+  IndSensor& Left;
+  IndSensor& Right;
+  IndSensor& Front;
+  IndSensor& Back;
+
+  PIDGains heightGains;
+  PIDGains rollGains;
+  PIDGains pitchGains;
+
+  PIDState heightErr;
+  PIDState rollErr;
+  PIDState pitchErr;
+
+  float AvgRef;     // Target gap height (mm)
+  float avg;        // Current average gap (mm)
+  bool  ffEnabled;  // Feedforward on/off
+
+  int16_t FLPWM;
+  int16_t BLPWM;
+  int16_t FRPWM;
+  int16_t BRPWM;
+};
+
+#endif // CONTROLLER_HPP

embedded/lib/IndSensorMap.cpp

@@ -58,7 +58,7 @@ float IndSensor::readMM() {
 }
 
 // Predefined sensor instances
-IndSensor indL(ind1Map, A0);
+IndSensor indF(ind1Map, A0);
-IndSensor indR(ind0Map, A1);
+IndSensor indB(ind0Map, A1);
-IndSensor indF(ind3Map, A5);
+IndSensor indR(ind3Map, A5);
-IndSensor indB(ind2Map, A4);
+IndSensor indL(ind2Map, A4);

lev_sim/PWM_Circuit_Model.py

@@ -0,0 +1,164 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.signal import TransferFunction, lsim
+
+# ============================================================
+# Circuit Parameters
+# ============================================================
+R = 1.5            # Resistance  [Ω]
+L = 0.0025         # Inductance  [H]  (2.5 mH)
+V_SUPPLY = 12.0    # Supply rail [V]
+tau = L / R        # RL time constant ≈ 1.667 ms
+
+# ============================================================
+# PWM Parameters
+# ============================================================
+F_PWM = 16e3                    # PWM frequency  [Hz]
+T_PWM = 1.0 / F_PWM            # PWM period     [s]  (62.5 µs)
+
+# ============================================================
+# Simulation Window
+# ============================================================
+T_END = 1e-3                    # 1 ms  →  16 full PWM cycles
+DT    = 1e-7                    # 100 ns resolution (625 pts / PWM cycle)
+t     = np.arange(0, T_END + DT / 2, DT)
+
+# ============================================================
+# Duty-Cycle Command  D(t)
+# ============================================================
+# Ramp from 20 % → 80 % over the window so every PWM cycle
+# has a visibly different pulse width.
+def duty_command(t_val):
+    """Continuous duty-cycle setpoint (from a controller)."""
+    return np.clip(0.2 + 0.6 * (np.asarray(t_val) / T_END), 0.0, 1.0)
+
+D_continuous = duty_command(t)
+
+# ============================================================
+# MODEL 1 — Abstracted (Average-Voltage) Approximation
+# ============================================================
+# Treats the coil voltage as the smooth signal  D(t)·V.
+# Transfer function:  I(s)/D(s) = V / (Ls + R)
+G = TransferFunction([V_SUPPLY], [L, R])
+_, i_avg, _ = lsim(G, U=D_continuous, T=t)
+
+# ============================================================
+# MODEL 2 — True PWM Waveform  (exact analytical solution)
+# ============================================================
+# Between every switching edge the RL circuit obeys:
+#
+#   di/dt = (V_seg − R·i) / L          (V_seg = V_SUPPLY or 0)
+#
+# Closed-form from initial condition i₀ at time t₀:
+#
+#   i(t) = V_seg/R  +  (i₀ − V_seg/R) · exp(−R·(t − t₀) / L)
+#
+# We propagate i analytically from edge to edge — zero
+# numerical-integration error.  The only error source is
+# IEEE-754 floating-point arithmetic (~1e-15 relative).
+
+# --- Step 1: build segment table and propagate boundary currents ---
+seg_t_start = []          # start time of each constant-V segment
+seg_V       = []          # voltage applied during segment
+seg_i0      = []          # current at segment start
+
+i_boundary = 0.0          # coil starts de-energised
+
+cycle = 0
+while cycle * T_PWM < T_END:
+    t_cycle = cycle * T_PWM
+    D = float(duty_command(t_cycle))
+
+    # ---- ON phase (V_SUPPLY applied) ----
+    t_on_start = t_cycle
+    t_on_end   = min(t_cycle + D * T_PWM, T_END)
+    if t_on_end > t_on_start:
+        seg_t_start.append(t_on_start)
+        seg_V.append(V_SUPPLY)
+        seg_i0.append(i_boundary)
+        dt_on = t_on_end - t_on_start
+        i_boundary = (V_SUPPLY / R) + (i_boundary - V_SUPPLY / R) * np.exp(-R * dt_on / L)
+
+    # ---- OFF phase (0 V applied, free-wheeling through diode) ----
+    t_off_start = t_on_end
+    t_off_end   = min((cycle + 1) * T_PWM, T_END)
+    if t_off_end > t_off_start:
+        seg_t_start.append(t_off_start)
+        seg_V.append(0.0)
+        seg_i0.append(i_boundary)
+        dt_off = t_off_end - t_off_start
+        i_boundary = i_boundary * np.exp(-R * dt_off / L)
+
+    cycle += 1
+
+seg_t_start = np.array(seg_t_start)
+seg_V       = np.array(seg_V)
+seg_i0      = np.array(seg_i0)
+
+# --- Step 2: evaluate on the dense time array (vectorised) ---
+idx = np.searchsorted(seg_t_start, t, side='right') - 1
+idx = np.clip(idx, 0, len(seg_t_start) - 1)
+
+dt_in_seg = t - seg_t_start[idx]
+V_at_t    = seg_V[idx]
+i0_at_t   = seg_i0[idx]
+
+i_pwm = (V_at_t / R) + (i0_at_t - V_at_t / R) * np.exp(-R * dt_in_seg / L)
+v_pwm = V_at_t                       # switching waveform for plotting
+v_avg = D_continuous * V_SUPPLY       # average-model voltage
+
+# ============================================================
+# Console Output — sanity-check steady-state values
+# ============================================================
+print(f"RL time constant  τ = L/R = {tau*1e3:.3f} ms")
+print(f"PWM period        T = 1/f = {T_PWM*1e6:.1f} µs")
+print(f"Sim resolution    Δt      = {DT*1e9:.0f} ns  ({int(T_PWM/DT)} pts/cycle)")
+print()
+print("Expected steady-state currents  i_ss = (V/R)·D :")
+for d in [0.2, 0.4, 0.6, 0.8]:
+    print(f"  D = {d:.1f}  →  i_ss = {V_SUPPLY / R * d:.3f} A")
+
+# ============================================================
+# Plotting — 4-panel comparison
+# ============================================================
+t_us = t * 1e6                        # time axis in µs
+
+fig, axes = plt.subplots(4, 1, figsize=(14, 10), sharex=True)
+fig.suptitle("PWM RL-Circuit Model Comparison  (16 kHz, 1 ms window)",
+             fontsize=13, fontweight='bold')
+
+# --- 1. Duty-cycle command ---
+ax = axes[0]
+ax.plot(t_us, D_continuous * 100, color='tab:purple', linewidth=1.2)
+ax.set_ylabel("Duty Cycle [%]")
+ax.set_ylim(0, 100)
+ax.grid(True, alpha=0.3)
+
+# --- 2. Voltage waveforms ---
+ax = axes[1]
+ax.plot(t_us, v_pwm, color='tab:orange', linewidth=0.6, label="True PWM voltage")
+ax.plot(t_us, v_avg, color='tab:blue',   linewidth=1.4, label="Average voltage D·V",
+        linestyle='--')
+ax.set_ylabel("Voltage [V]")
+ax.set_ylim(-0.5, V_SUPPLY + 1)
+ax.legend(loc='upper left', fontsize=9)
+ax.grid(True, alpha=0.3)
+
+# --- 3. Current comparison ---
+ax = axes[2]
+ax.plot(t_us, i_pwm, color='tab:red',  linewidth=0.7, label="True PWM current (exact)")
+ax.plot(t_us, i_avg, color='tab:blue',  linewidth=1.4, label="Averaged-model current",
+        linestyle='--')
+ax.set_ylabel("Current [A]")
+ax.legend(loc='upper left', fontsize=9)
+ax.grid(True, alpha=0.3)
+
+# --- 4. Difference / ripple ---
+ax = axes[3]
+ax.plot(t_us, (i_pwm - i_avg) * 1e3, color='tab:green', linewidth=0.7)
+ax.set_ylabel("Δi  (PWM − avg) [mA]")
+ax.set_xlabel("Time [µs]")
+ax.grid(True, alpha=0.3)
+
+plt.tight_layout()
+plt.show()

lev_sim/archive_not_used/maglev_linearizer.py

@@ -0,0 +1,362 @@
+"""
+Magnetic Levitation Jacobian Linearizer
+
+Computes the local linear (Jacobian) approximation of the degree-6 polynomial
+force/torque model at any operating point.  The result is an LTI gain matrix
+that relates small perturbations in (currL, currR, roll, gap_height) to
+perturbations in (Force, Torque):
+
+    [ΔF  ]       [∂F/∂currL  ∂F/∂currR  ∂F/∂roll  ∂F/∂gap] [ΔcurrL  ]
+    [ΔTau]  ≈  J [∂T/∂currL  ∂T/∂currR  ∂T/∂roll  ∂T/∂gap] [ΔcurrR  ]
+                                                              [Δroll   ]
+                                                              [Δgap    ]
+
+Since the polynomial is analytic, all derivatives are computed exactly
+(symbolic differentiation of the power-product terms), NOT by finite
+differences.
+
+The chain rule is applied automatically to convert the internal invGap
+(= 1/gap_height) variable back to physical gap_height [mm].
+
+Usage:
+    lin = MaglevLinearizer("maglev_model.pkl")
+    plant = lin.linearize(currL=-15, currR=-15, roll=0.0, gap_height=10.0)
+    print(plant)
+    print(plant.dF_dcurrL)         # single gain
+    print(plant.control_jacobian)  # 2×2 matrix mapping ΔcurrL/R → ΔF/ΔT
+    f, t = plant.predict(delta_currL=0.5)  # quick what-if
+"""
+
+import numpy as np
+import joblib
+import os
+
+
+class LinearizedPlant:
+    """
+    Holds the Jacobian linearization of the force/torque model at one
+    operating point.
+
+    Attributes
+    ----------
+    operating_point : dict
+        The (currL, currR, roll, gap_height) where linearization was computed.
+    f0 : float
+        Force [N] at the operating point.
+    tau0 : float
+        Torque [mN·m] at the operating point.
+    jacobian : ndarray, shape (2, 4)
+        Full Jacobian:
+            Row 0 = Force derivatives,  Row 1 = Torque derivatives.
+            Columns = [currL [A], currR [A], rollDeg [deg], gap_height [mm]]
+    """
+
+    INPUT_LABELS = ['currL [A]', 'currR [A]', 'rollDeg [deg]', 'gap_height [mm]']
+
+    def __init__(self, operating_point, f0, tau0, jacobian):
+        self.operating_point = operating_point
+        self.f0 = f0
+        self.tau0 = tau0
+        self.jacobian = jacobian
+
+    # ---- Individual gain accessors ----
+    @property
+    def dF_dcurrL(self):
+        """∂Force/∂currL [N/A] at operating point."""
+        return self.jacobian[0, 0]
+
+    @property
+    def dF_dcurrR(self):
+        """∂Force/∂currR [N/A] at operating point."""
+        return self.jacobian[0, 1]
+
+    @property
+    def dF_droll(self):
+        """∂Force/∂roll [N/deg] at operating point."""
+        return self.jacobian[0, 2]
+
+    @property
+    def dF_dgap(self):
+        """∂Force/∂gap_height [N/mm] at operating point.
+        Typically positive (unstable): force increases as gap shrinks.
+        """
+        return self.jacobian[0, 3]
+
+    @property
+    def dT_dcurrL(self):
+        """∂Torque/∂currL [mN·m/A] at operating point."""
+        return self.jacobian[1, 0]
+
+    @property
+    def dT_dcurrR(self):
+        """∂Torque/∂currR [mN·m/A] at operating point."""
+        return self.jacobian[1, 1]
+
+    @property
+    def dT_droll(self):
+        """∂Torque/∂roll [mN·m/deg] at operating point."""
+        return self.jacobian[1, 2]
+
+    @property
+    def dT_dgap(self):
+        """∂Torque/∂gap_height [mN·m/mm] at operating point."""
+        return self.jacobian[1, 3]
+
+    @property
+    def control_jacobian(self):
+        """2×2 sub-matrix mapping control inputs [ΔcurrL, ΔcurrR] → [ΔF, ΔT].
+
+        This is the "B" portion of the linearized plant that the PID
+        controller acts through.
+        """
+        return self.jacobian[:, :2]
+
+    @property
+    def state_jacobian(self):
+        """2×2 sub-matrix mapping state perturbations [Δroll, Δgap] → [ΔF, ΔT].
+
+        Contains the magnetic stiffness (∂F/∂gap) and roll coupling.
+        This feeds into the "A" matrix of the full mechanical state-space.
+        """
+        return self.jacobian[:, 2:]
+
+    def predict(self, delta_currL=0.0, delta_currR=0.0,
+                delta_roll=0.0, delta_gap=0.0):
+        """
+        Predict force and torque using the linear approximation.
+
+        Returns (force, torque) including the nominal operating-point values.
+        """
+        delta = np.array([delta_currL, delta_currR, delta_roll, delta_gap])
+        perturbation = self.jacobian @ delta
+        return self.f0 + perturbation[0], self.tau0 + perturbation[1]
+
+    def __repr__(self):
+        op = self.operating_point
+        lines = [
+            f"LinearizedPlant @ currL={op['currL']:.1f}A, "
+            f"currR={op['currR']:.1f}A, "
+            f"roll={op['roll']:.2f}°, "
+            f"gap={op['gap_height']:.2f}mm",
+            f"  F₀ = {self.f0:+.4f} N    τ₀ = {self.tau0:+.4f} mN·m",
+            f"  ∂F/∂currL = {self.dF_dcurrL:+.4f} N/A       "
+            f"∂T/∂currL = {self.dT_dcurrL:+.4f} mN·m/A",
+            f"  ∂F/∂currR = {self.dF_dcurrR:+.4f} N/A       "
+            f"∂T/∂currR = {self.dT_dcurrR:+.4f} mN·m/A",
+            f"  ∂F/∂roll  = {self.dF_droll:+.4f} N/deg     "
+            f"∂T/∂roll  = {self.dT_droll:+.4f} mN·m/deg",
+            f"  ∂F/∂gap   = {self.dF_dgap:+.4f} N/mm      "
+            f"∂T/∂gap   = {self.dT_dgap:+.4f} mN·m/mm",
+        ]
+        return '\n'.join(lines)
+
+
+class MaglevLinearizer:
+    """
+    Jacobian linearizer for the polynomial maglev force/torque model.
+
+    Loads the same .pkl model as MaglevPredictor, but instead of just
+    evaluating the polynomial, computes exact analytical partial derivatives
+    at any operating point.
+    """
+
+    def __init__(self, model_path='maglev_model.pkl'):
+        if not os.path.exists(model_path):
+            raise FileNotFoundError(
+                f"Model file '{model_path}' not found. "
+                "Train and save the model from Function Fitting.ipynb first."
+            )
+
+        data = joblib.load(model_path)
+        poly_transformer = data['poly_features']
+        linear_model = data['model']
+
+        # powers_: (n_terms, n_inputs) — exponent matrix from sklearn
+        # Transpose to (n_inputs, n_terms) for broadcasting with x[:, None]
+        self.powers = poly_transformer.powers_.T.astype(np.float64)
+
+        self.force_coef = linear_model.coef_[0]        # (n_terms,)
+        self.torque_coef = linear_model.coef_[1]       # (n_terms,)
+        self.force_intercept = linear_model.intercept_[0]
+        self.torque_intercept = linear_model.intercept_[1]
+
+        self.degree = data['degree']
+        self.n_terms = self.powers.shape[1]
+
+    def _to_internal(self, currL, currR, roll, gap_height):
+        """Convert physical inputs to the polynomial's internal variables."""
+        invGap = 1.0 / max(gap_height, 1e-6)
+        return np.array([currL, currR, roll, invGap], dtype=np.float64)
+
+    def evaluate(self, currL, currR, roll, gap_height):
+        """
+        Evaluate the full (nonlinear) polynomial at a single point.
+
+        Returns
+        -------
+        force : float  [N]
+        torque : float  [mN·m]
+        """
+        x = self._to_internal(currL, currR, roll, gap_height)
+        poly_features = np.prod(x[:, None] ** self.powers, axis=0)
+        force = np.dot(self.force_coef, poly_features) + self.force_intercept
+        torque = np.dot(self.torque_coef, poly_features) + self.torque_intercept
+        return float(force), float(torque)
+
+    def _jacobian_internal(self, x):
+        """
+        Compute the 2×4 Jacobian w.r.t. the internal polynomial variables
+        (currL, currR, rollDeg, invGap).
+
+        For each variable x_k, the partial derivative of a polynomial term
+            c · x₁^a₁ · x₂^a₂ · x₃^a₃ · x₄^a₄
+        is:
+            c · a_k · x_k^(a_k - 1) · ∏_{j≠k} x_j^a_j
+
+        This is computed vectorised over all terms simultaneously.
+        """
+        jac = np.zeros((2, 4))
+        for k in range(4):
+            # a_k for every term — this becomes the multiplicative scale
+            scale = self.powers[k, :]  # (n_terms,)
+
+            # Reduce the k-th exponent by 1 (floored at 0; the scale
+            # factor of 0 for constant-in-x_k terms zeros those out)
+            deriv_powers = self.powers.copy()
+            deriv_powers[k, :] = np.maximum(deriv_powers[k, :] - 1.0, 0.0)
+
+            # Evaluate the derivative polynomial
+            poly_terms = np.prod(x[:, None] ** deriv_powers, axis=0)
+            deriv_features = scale * poly_terms  # (n_terms,)
+
+            jac[0, k] = np.dot(self.force_coef, deriv_features)
+            jac[1, k] = np.dot(self.torque_coef, deriv_features)
+
+        return jac
+
+    def linearize(self, currL, currR, roll, gap_height):
+        """
+        Compute the Jacobian linearization at the given operating point.
+
+        Parameters
+        ----------
+        currL : float      Left coil current [A]
+        currR : float      Right coil current [A]
+        roll : float       Roll angle [deg]
+        gap_height : float Air gap [mm]
+
+        Returns
+        -------
+        LinearizedPlant
+            Contains the operating-point values (F₀, τ₀) and the 2×4
+            Jacobian with columns [currL, currR, roll, gap_height].
+        """
+        x = self._to_internal(currL, currR, roll, gap_height)
+        f0, tau0 = self.evaluate(currL, currR, roll, gap_height)
+
+        # Jacobian in internal coordinates (w.r.t. invGap in column 3)
+        jac_internal = self._jacobian_internal(x)
+
+        # Chain rule: ∂f/∂gap = ∂f/∂invGap · d(invGap)/d(gap)
+        #                     = ∂f/∂invGap · (−1 / gap²)
+        jac = jac_internal.copy()
+        jac[:, 3] *= -1.0 / (gap_height ** 2)
+
+        return LinearizedPlant(
+            operating_point={
+                'currL': currL, 'currR': currR,
+                'roll': roll, 'gap_height': gap_height,
+            },
+            f0=f0,
+            tau0=tau0,
+            jacobian=jac,
+        )
+
+    def gain_schedule(self, gap_heights, currL, currR, roll=0.0):
+        """
+        Precompute linearizations across a range of gap heights at fixed
+        current and roll.  Useful for visualising how plant gains vary
+        and for designing a gain-scheduled PID.
+
+        Parameters
+        ----------
+        gap_heights : array-like of float [mm]
+        currL, currR : float [A]
+        roll : float [deg], default 0
+
+        Returns
+        -------
+        list of LinearizedPlant, one per gap height
+        """
+        return [
+            self.linearize(currL, currR, roll, g)
+            for g in gap_heights
+        ]
+
+
+# ==========================================================================
+# Demo / quick validation
+# ==========================================================================
+if __name__ == '__main__':
+    import sys
+
+    model_path = os.path.join(os.path.dirname(__file__), 'maglev_model.pkl')
+    lin = MaglevLinearizer(model_path)
+
+    # --- Single-point linearization ---
+    print("=" * 70)
+    print("SINGLE-POINT LINEARIZATION")
+    print("=" * 70)
+    plant = lin.linearize(currL=-15, currR=-15, roll=0.0, gap_height=10.0)
+    print(plant)
+    print()
+
+    # Quick sanity check: compare linear prediction vs full polynomial
+    dc = 0.5  # small current perturbation
+    f_lin, t_lin = plant.predict(delta_currL=dc)
+    f_act, t_act = lin.evaluate(-15 + dc, -15, 0.0, 10.0)
+    print(f"Linearised  vs  Actual  (ΔcurrL = {dc:+.1f} A):")
+    print(f"  Force:  {f_lin:.4f}  vs  {f_act:.4f}  (err {abs(f_lin-f_act):.6f} N)")
+    print(f"  Torque: {t_lin:.4f}  vs  {t_act:.4f}  (err {abs(t_lin-t_act):.6f} mN·m)")
+    print()
+
+    # --- Gain schedule across gap heights ---
+    print("=" * 70)
+    print("GAIN SCHEDULE  (currL = currR = -15 A, roll = 0°)")
+    print("=" * 70)
+    gaps = [6, 8, 10, 12, 15, 20, 25]
+    plants = lin.gain_schedule(gaps, currL=-15, currR=-15, roll=0.0)
+
+    header = f"{'Gap [mm]':>10} {'F₀ [N]':>10} {'∂F/∂iL':>10} {'∂F/∂iR':>10} {'∂F/∂gap':>10} {'∂T/∂iL':>12} {'∂T/∂iR':>12}"
+    print(header)
+    print("-" * len(header))
+    for p in plants:
+        g = p.operating_point['gap_height']
+        print(
+            f"{g:10.1f} {p.f0:10.3f} {p.dF_dcurrL:10.4f} "
+            f"{p.dF_dcurrR:10.4f} {p.dF_dgap:10.4f} "
+            f"{p.dT_dcurrL:12.4f} {p.dT_dcurrR:12.4f}"
+        )
+    print()
+
+    # --- Note on PID usage ---
+    print("=" * 70)
+    print("NOTES FOR PID DESIGN")
+    print("=" * 70)
+    print("""
+At each operating point, the linearized electromagnetic plant is:
+
+    [ΔF  ]   [∂F/∂iL  ∂F/∂iR] [ΔiL]   [∂F/∂roll  ∂F/∂gap] [Δroll]
+    [ΔTau] = [∂T/∂iL  ∂T/∂iR] [ΔiR] + [∂T/∂roll  ∂T/∂gap] [Δgap ]
+              ^^^^^^^^^^^^^^^^           ^^^^^^^^^^^^^^^^^^^^
+              control_jacobian             state_jacobian
+
+The full mechanical dynamics (linearized) are:
+    m  · Δg̈ap  = ΔF  - m·g    (vertical — note ∂F/∂gap > 0 means unstable)
+    Iz · Δroll̈ = ΔTau          (roll)
+
+So the PID loop sees:
+    control_jacobian  →  the gain from current commands to force/torque
+    state_jacobian    →  the coupling from state perturbations (acts like
+                         a destabilising spring for gap, restoring for roll)
+""")

lev_sim/archive_not_used/maglev_state_space.py

@@ -0,0 +1,653 @@
+"""
+Full Linearized State-Space Model for the Guadaloop Maglev Pod
+==============================================================
+
+Combines three dynamic layers into a single LTI system  ẋ = Ax + Bu,  y = Cx:
+
+  Layer 1 — Coil RL dynamics (electrical):
+      di/dt = (V·pwm − R·i) / L
+      This is already linear.  A first-order lag from PWM command to current.
+
+  Layer 2 — Electromagnetic force/torque map (from Ansys polynomial):
+      (F, τ) = f(iL, iR, roll, gap)
+      Nonlinear, but the MaglevLinearizer gives us the Jacobian at any
+      operating point, making it locally linear.
+
+  Layer 3 — Rigid-body mechanics (Newton/Euler):
+      m·z̈  = F_front + F_back − m·g       (heave)
+      Iy·θ̈  = L_arm·(F_front − F_back)     (pitch from force differential)
+      Ix·φ̈  = τ_front + τ_back              (roll from magnetic torque)
+      These are linear once the force/torque are linearized.
+
+The key coupling: the pod is rigid, so front and back yoke gaps are NOT
+independent.  They are related to the average gap and pitch angle:
+
+      gap_front = gap_avg − L_arm · pitch
+      gap_back  = gap_avg + L_arm · pitch
+
+This means a pitch perturbation changes both yoke gaps, which changes both
+yoke forces, which feeds back into the heave and pitch dynamics.  The
+electromagnetic Jacobian captures how force/torque respond to these gap
+changes, creating the destabilizing "magnetic stiffness" that makes maglev
+inherently open-loop unstable.
+
+State vector (10 states):
+    x = [gap_avg, gap_vel, pitch, pitch_rate, roll, roll_rate,
+         i_FL, i_FR, i_BL, i_BR]
+
+    - gap_avg [m]:    average air gap (track-to-yoke distance)
+    - gap_vel [m/s]:  d(gap_avg)/dt
+    - pitch [rad]:    rotation about Y axis (positive = back hangs lower)
+    - pitch_rate [rad/s]
+    - roll [rad]:     rotation about X axis
+    - roll_rate [rad/s]
+    - i_FL..BR [A]:   the four coil currents
+
+Input vector (4 inputs):
+    u = [pwm_FL, pwm_FR, pwm_BL, pwm_BR]   (duty cycles, dimensionless)
+
+Output vector (3 outputs):
+    y = [gap_avg, pitch, roll]
+"""
+
+import numpy as np
+import os
+from maglev_linearizer import MaglevLinearizer
+
+# ---------------------------------------------------------------------------
+# Physical constants and unit conversions
+# ---------------------------------------------------------------------------
+GRAVITY = 9.81          # m/s²
+DEG2RAD = np.pi / 180.0
+RAD2DEG = 180.0 / np.pi
+
+# State indices (for readability)
+GAP, GAPV, PITCH, PITCHV, ROLL, ROLLV, I_FL, I_FR, I_BL, I_BR = range(10)
+
+
+# ===================================================================
+# StateSpaceResult — the output container
+# ===================================================================
+class StateSpaceResult:
+    """
+    Holds the A, B, C, D matrices of the linearized plant plus
+    operating-point metadata and stability analysis.
+    """
+
+    STATE_LABELS = [
+        'gap_avg [m]', 'gap_vel [m/s]',
+        'pitch [rad]', 'pitch_rate [rad/s]',
+        'roll [rad]', 'roll_rate [rad/s]',
+        'i_FL [A]', 'i_FR [A]', 'i_BL [A]', 'i_BR [A]',
+    ]
+    INPUT_LABELS = ['pwm_FL', 'pwm_FR', 'pwm_BL', 'pwm_BR']
+    OUTPUT_LABELS = ['gap_avg [m]', 'pitch [rad]', 'roll [rad]']
+
+    def __init__(self, A, B, C, D, operating_point,
+                 equilibrium_force_error, plant_front, plant_back):
+        self.A = A
+        self.B = B
+        self.C = C
+        self.D = D
+        self.operating_point = operating_point
+        self.equilibrium_force_error = equilibrium_force_error
+        self.plant_front = plant_front    # LinearizedPlant for front yoke
+        self.plant_back = plant_back      # LinearizedPlant for back yoke
+
+    @property
+    def eigenvalues(self):
+        """Eigenvalues of A, sorted by decreasing real part."""
+        eigs = np.linalg.eigvals(self.A)
+        return eigs[np.argsort(-np.real(eigs))]
+
+    @property
+    def is_open_loop_stable(self):
+        return bool(np.all(np.real(self.eigenvalues) < 0))
+
+    @property
+    def unstable_eigenvalues(self):
+        eigs = self.eigenvalues
+        return eigs[np.real(eigs) > 1e-8]
+
+    def to_scipy(self):
+        """Convert to scipy.signal.StateSpace for frequency-domain analysis."""
+        from scipy.signal import StateSpace
+        return StateSpace(self.A, self.B, self.C, self.D)
+
+    def print_A_structure(self):
+        """Print the A matrix with row/column labels for physical insight."""
+        labels_short = ['gap', 'ġap', 'θ', 'θ̇', 'φ', 'φ̇',
+                        'iFL', 'iFR', 'iBL', 'iBR']
+        print("\nA matrix (non-zero entries):")
+        print("-" * 65)
+        for i in range(10):
+            for j in range(10):
+                if abs(self.A[i, j]) > 1e-10:
+                    print(f"  A[{labels_short[i]:>3}, {labels_short[j]:>3}] "
+                          f"= {self.A[i,j]:+12.4f}")
+        print("-" * 65)
+
+    def print_B_structure(self):
+        """Print the B matrix with labels."""
+        labels_short = ['gap', 'ġap', 'θ', 'θ̇', 'φ', 'φ̇',
+                        'iFL', 'iFR', 'iBL', 'iBR']
+        u_labels = ['uFL', 'uFR', 'uBL', 'uBR']
+        print("\nB matrix (non-zero entries):")
+        print("-" * 50)
+        for i in range(10):
+            for j in range(4):
+                if abs(self.B[i, j]) > 1e-10:
+                    print(f"  B[{labels_short[i]:>3}, {u_labels[j]:>3}] "
+                          f"= {self.B[i,j]:+12.4f}")
+        print("-" * 50)
+
+    def __repr__(self):
+        op = self.operating_point
+        eigs = self.eigenvalues
+
+        at_eq = abs(self.equilibrium_force_error) < 0.5
+        eq_str = ('AT EQUILIBRIUM' if at_eq
+                  else f'NOT AT EQUILIBRIUM — {self.equilibrium_force_error:+.2f} N residual')
+
+        lines = [
+            "=" * 70,
+            "LINEARIZED MAGLEV STATE-SPACE  (ẋ = Ax + Bu,  y = Cx)",
+            "=" * 70,
+            f"Operating point:",
+            f"  gap = {op['gap_height']:.2f} mm,  "
+            f"currL = {op['currL']:.2f} A,  "
+            f"currR = {op['currR']:.2f} A,  "
+            f"roll = {op['roll']:.1f}°,  "
+            f"pitch = {op['pitch']:.1f}°",
+            f"  F_front = {self.plant_front.f0:.3f} N,  "
+            f"F_back = {self.plant_back.f0:.3f} N,  "
+            f"F_total = {self.plant_front.f0 + self.plant_back.f0:.3f} N,  "
+            f"Weight = {op['mass'] * GRAVITY:.3f} N",
+            f"  >> {eq_str}",
+            "",
+            f"System: {self.A.shape[0]} states × "
+            f"{self.B.shape[1]} inputs × "
+            f"{self.C.shape[0]} outputs",
+            f"Open-loop stable: {self.is_open_loop_stable}",
+            "",
+            "Eigenvalues of A:",
+        ]
+
+        # Group complex conjugate pairs
+        printed = set()
+        for i, ev in enumerate(eigs):
+            if i in printed:
+                continue
+            re_part = np.real(ev)
+            im_part = np.imag(ev)
+            stability = "UNSTABLE" if re_part > 1e-8 else "stable"
+
+            if abs(im_part) < 1e-6:
+                lines.append(
+                    f"  λ = {re_part:+12.4f}              "
+                    f"  τ = {abs(1/re_part)*1000 if abs(re_part) > 1e-8 else float('inf'):.2f} ms"
+                    f"  ({stability})"
+                )
+            else:
+                # Find conjugate pair
+                for j in range(i + 1, len(eigs)):
+                    if j not in printed and abs(eigs[j] - np.conj(ev)) < 1e-6:
+                        printed.add(j)
+                        break
+                omega_n = abs(ev)
+                lines.append(
+                    f"  λ = {re_part:+12.4f} ± {abs(im_part):.4f}j"
+                    f"  ω_n = {omega_n:.1f} rad/s"
+                    f"  ({stability})"
+                )
+
+        lines.extend(["", "=" * 70])
+        return '\n'.join(lines)
+
+
+# ===================================================================
+# MaglevStateSpace — the builder
+# ===================================================================
+class MaglevStateSpace:
+    """
+    Assembles the full 10-state linearized state-space from the
+    electromagnetic Jacobian + rigid body dynamics + coil dynamics.
+
+    Physical parameters come from the URDF (pod.xml) and MagLevCoil.
+    """
+
+    def __init__(self, linearizer,
+                 mass=9.4,
+                 I_roll=0.0192942414,     # Ixx from pod.xml [kg·m²]
+                 I_pitch=0.130582305,     # Iyy from pod.xml [kg·m²]
+                 coil_R=1.1,              # from MagLevCoil in lev_pod_env.py
+                 coil_L=0.0025,           # 2.5 mH
+                 V_supply=12.0,           # supply voltage [V]
+                 L_arm=0.1259):           # front/back yoke X-offset [m]
+        self.linearizer = linearizer
+        self.mass = mass
+        self.I_roll = I_roll
+        self.I_pitch = I_pitch
+        self.coil_R = coil_R
+        self.coil_L = coil_L
+        self.V_supply = V_supply
+        self.L_arm = L_arm
+
+    @staticmethod
+    def _convert_jacobian_to_si(jac):
+        """
+        Convert a linearizer Jacobian from mixed units to pure SI.
+
+        The linearizer returns:
+            Row 0: Force  [N]     per  [A, A, deg, mm]
+            Row 1: Torque [mN·m]  per  [A, A, deg, mm]
+
+        We need:
+            Row 0: Force  [N]     per  [A, A, rad, m]
+            Row 1: Torque [N·m]   per  [A, A, rad, m]
+
+        Conversion factors:
+            col 0,1 (current): ×1 for force,  ×(1/1000) for torque
+            col 2   (roll):    ×(180/π) for force,  ×(180/π)/1000 for torque
+            col 3   (gap):     ×1000 for force,  ×(1000/1000)=×1 for torque
+        """
+        si = np.zeros((2, 4))
+
+        # Force row — already in N
+        si[0, 0] = jac[0, 0]                     # N/A → N/A
+        si[0, 1] = jac[0, 1]                     # N/A → N/A
+        si[0, 2] = jac[0, 2] * RAD2DEG           # N/deg → N/rad
+        si[0, 3] = jac[0, 3] * 1000.0            # N/mm → N/m
+
+        # Torque row — from mN·m to N·m
+        si[1, 0] = jac[1, 0] / 1000.0            # mN·m/A → N·m/A
+        si[1, 1] = jac[1, 1] / 1000.0            # mN·m/A → N·m/A
+        si[1, 2] = jac[1, 2] / 1000.0 * RAD2DEG  # mN·m/deg → N·m/rad
+        si[1, 3] = jac[1, 3]                      # mN·m/mm → N·m/m (factors cancel)
+
+        return si
+
+    def build(self, gap_height, currL, currR, roll=0.0, pitch=0.0):
+        """
+        Build the A, B, C, D matrices at a given operating point.
+
+        Parameters
+        ----------
+        gap_height : float  Average gap [mm]
+        currL : float       Equilibrium left coil current [A] (same front & back)
+        currR : float       Equilibrium right coil current [A]
+        roll : float        Equilibrium roll angle [deg], default 0
+        pitch : float       Equilibrium pitch angle [deg], default 0
+                            Non-zero pitch means front/back gaps differ.
+
+        Returns
+        -------
+        StateSpaceResult
+        """
+        m = self.mass
+        Ix = self.I_roll
+        Iy = self.I_pitch
+        R = self.coil_R
+        Lc = self.coil_L
+        V = self.V_supply
+        La = self.L_arm
+
+        # ------------------------------------------------------------------
+        # Step 1: Compute individual yoke gaps from average gap + pitch
+        #
+        # The pod is rigid.  If it pitches, the front and back yoke ends
+        # are at different distances from the track:
+        #   gap_front = gap_avg − L_arm · sin(pitch) ≈ gap_avg − L_arm · pitch
+        #   gap_back  = gap_avg + L_arm · sin(pitch) ≈ gap_avg + L_arm · pitch
+        #
+        # Sign convention (from lev_pod_env.py lines 230-232):
+        #   positive pitch = back gap > front gap  (back hangs lower)
+        # ------------------------------------------------------------------
+        pitch_rad = pitch * DEG2RAD
+        # L_arm [m] * sin(pitch) [rad] → meters; convert to mm for linearizer
+        gap_front_mm = gap_height - La * np.sin(pitch_rad) * 1000.0
+        gap_back_mm = gap_height + La * np.sin(pitch_rad) * 1000.0
+
+        # ------------------------------------------------------------------
+        # Step 2: Linearize each yoke independently
+        #
+        # Each U-yoke has its own (iL, iR) pair and sees its own gap.
+        # Both yokes see the same roll angle (the pod is rigid).
+        # The linearizer returns the Jacobian in mixed units.
+        # ------------------------------------------------------------------
+        plant_f = self.linearizer.linearize(currL, currR, roll, gap_front_mm)
+        plant_b = self.linearizer.linearize(currL, currR, roll, gap_back_mm)
+
+        # ------------------------------------------------------------------
+        # Step 3: Convert Jacobians to SI
+        #
+        # After this, all gains are in [N or N·m] per [A, A, rad, m].
+        # Columns: [currL, currR, roll, gap_height]
+        # ------------------------------------------------------------------
+        Jf = self._convert_jacobian_to_si(plant_f.jacobian)
+        Jb = self._convert_jacobian_to_si(plant_b.jacobian)
+
+        # Unpack for clarity — subscript _f = front yoke, _b = back yoke
+        # Force gains
+        kFiL_f, kFiR_f, kFr_f, kFg_f = Jf[0]
+        kFiL_b, kFiR_b, kFr_b, kFg_b = Jb[0]
+        # Torque gains
+        kTiL_f, kTiR_f, kTr_f, kTg_f = Jf[1]
+        kTiL_b, kTiR_b, kTr_b, kTg_b = Jb[1]
+
+        # ------------------------------------------------------------------
+        # Step 4: Assemble the A matrix (10 × 10)
+        #
+        # The A matrix encodes three kinds of coupling:
+        #
+        #  (a) Kinematic identities:  gap_vel = d(gap)/dt, etc.
+        #      These are always 1.0 on the super-diagonal of the
+        #      position/velocity pairs.
+        #
+        #  (b) Electromagnetic coupling through current states:
+        #      Coil currents produce forces/torques.  The linearized
+        #      gains (∂F/∂i, ∂T/∂i) appear in the acceleration rows.
+        #      This is the path from current states to mechanical
+        #      acceleration — the "plant gain" that PID acts through.
+        #
+        #  (c) Electromagnetic coupling through mechanical states:
+        #      Gap and roll perturbations change the force/torque.
+        #      This creates feedback loops:
+        #
+        #      - ∂F/∂gap < 0  → gap perturbation changes force in a
+        #        direction that AMPLIFIES the gap error  → UNSTABLE
+        #        (magnetic stiffness is "negative spring")
+        #
+        #      - ∂T/∂roll     → roll perturbation changes torque;
+        #        sign determines whether roll is self-correcting or not
+        #
+        #      - Pitch couples through gap_front/gap_back dependence
+        #        on pitch angle, creating pitch instability too
+        # ------------------------------------------------------------------
+        A = np.zeros((10, 10))
+
+        # (a) Kinematic identities
+        A[GAP, GAPV] = 1.0
+        A[PITCH, PITCHV] = 1.0
+        A[ROLL, ROLLV] = 1.0
+
+        # ------------------------------------------------------------------
+        # HEAVE:  m · Δgap̈ = −(ΔF_front + ΔF_back)
+        #
+        # The negative sign is because force is upward (+Z) but gap
+        # is measured downward (gap shrinks when pod moves up).
+        # At equilibrium F₀ = mg; perturbation ΔF pushes pod up → gap shrinks.
+        #
+        # Expanding ΔF using the rigid-body gap coupling:
+        #   ΔF_front = kFg_f·(Δgap − La·Δpitch) + kFr_f·Δroll + kFiL_f·ΔiFL + kFiR_f·ΔiFR
+        #   ΔF_back  = kFg_b·(Δgap + La·Δpitch) + kFr_b·Δroll + kFiL_b·ΔiBL + kFiR_b·ΔiBR
+        # ------------------------------------------------------------------
+        # Gap → gap acceleration  (magnetic stiffness, UNSTABLE)
+        A[GAPV, GAP] = -(kFg_f + kFg_b) / m
+        # Pitch → gap acceleration  (cross-coupling through differential gap)
+        A[GAPV, PITCH] = -(-kFg_f + kFg_b) * La / m
+        # Roll → gap acceleration
+        A[GAPV, ROLL] = -(kFr_f + kFr_b) / m
+        # Current → gap acceleration  (the control path!)
+        A[GAPV, I_FL] = -kFiL_f / m
+        A[GAPV, I_FR] = -kFiR_f / m
+        A[GAPV, I_BL] = -kFiL_b / m
+        A[GAPV, I_BR] = -kFiR_b / m
+
+        # ------------------------------------------------------------------
+        # PITCH:  Iy · Δpitcḧ = La · (ΔF_front − ΔF_back)
+        #
+        # Pitch torque comes from DIFFERENTIAL FORCE, not from the
+        # electromagnetic torque (which acts on roll).  This is because
+        # the front yoke is at x = +La and the back at x = −La:
+        #   τ_pitch = F_front·La − F_back·La = La·(F_front − F_back)
+        #
+        # At symmetric equilibrium, F_front = F_back → zero pitch torque. ✓
+        # A pitch perturbation breaks this symmetry through the gap coupling.
+        # ------------------------------------------------------------------
+        # Gap → pitch acceleration  (zero at symmetric equilibrium)
+        A[PITCHV, GAP] = La * (kFg_f - kFg_b) / Iy
+        # Pitch → pitch acceleration  (pitch instability — UNSTABLE)
+        # = −La²·(kFg_f + kFg_b)/Iy.  Since kFg < 0 → positive → unstable.
+        A[PITCHV, PITCH] = -La**2 * (kFg_f + kFg_b) / Iy
+        # Roll → pitch acceleration
+        A[PITCHV, ROLL] = La * (kFr_f - kFr_b) / Iy
+        # Current → pitch acceleration
+        A[PITCHV, I_FL] = La * kFiL_f / Iy
+        A[PITCHV, I_FR] = La * kFiR_f / Iy
+        A[PITCHV, I_BL] = -La * kFiL_b / Iy
+        A[PITCHV, I_BR] = -La * kFiR_b / Iy
+
+        # ------------------------------------------------------------------
+        # ROLL:  Ix · Δroll̈ = Δτ_front + Δτ_back
+        #
+        # Unlike pitch (driven by force differential), roll is driven by
+        # the electromagnetic TORQUE directly.  In the Ansys model, torque
+        # is the moment about the X axis produced by the asymmetric flux
+        # in the left vs right legs of each U-yoke.
+        #
+        # The torque Jacobian entries determine stability:
+        #   - ∂T/∂roll:  if this causes torque that amplifies roll → unstable
+        #   - ∂T/∂iL, ∂T/∂iR:  how current asymmetry controls roll
+        # ------------------------------------------------------------------
+        # Gap → roll acceleration
+        A[ROLLV, GAP] = (kTg_f + kTg_b) / Ix
+        # Pitch → roll acceleration  (cross-coupling)
+        A[ROLLV, PITCH] = (-kTg_f + kTg_b) * La / Ix
+        # Roll → roll acceleration  (roll stiffness)
+        A[ROLLV, ROLL] = (kTr_f + kTr_b) / Ix
+        # Current → roll acceleration
+        A[ROLLV, I_FL] = kTiL_f / Ix
+        A[ROLLV, I_FR] = kTiR_f / Ix
+        A[ROLLV, I_BL] = kTiL_b / Ix
+        A[ROLLV, I_BR] = kTiR_b / Ix
+
+        # ------------------------------------------------------------------
+        # COIL DYNAMICS:  L·di/dt = V·pwm − R·i
+        #
+        # Rearranged:  di/dt = −(R/L)·i + (V/L)·pwm
+        #
+        # This is a simple first-order lag with:
+        #   - Time constant τ_coil = L/R = 2.5ms/1.1 = 2.27 ms
+        #   - Eigenvalue = −R/L = −440  (very fast, well-damped)
+        #
+        # The coil dynamics act as a low-pass filter between the PWM
+        # command and the actual current.  For PID frequencies below
+        # ~100 Hz, this lag is small but not negligible.
+        # ------------------------------------------------------------------
+        for k in range(I_FL, I_BR + 1):
+            A[k, k] = -R / Lc
+
+        # ------------------------------------------------------------------
+        # Step 5: B matrix (10 × 4)
+        #
+        # Only the coil states respond directly to the PWM inputs.
+        # The mechanical states are affected INDIRECTLY: pwm → current
+        # → force/torque → acceleration.  This indirect path shows up
+        # as the product A_mech_curr × B_curr_pwm in the transfer function.
+        #
+        # B[coil_k, pwm_k] = V_supply / L_coil
+        # ------------------------------------------------------------------
+        B = np.zeros((10, 4))
+        for k in range(4):
+            B[I_FL + k, k] = V / Lc
+
+        # ------------------------------------------------------------------
+        # Step 6: C matrix (3 × 10)
+        #
+        # Default outputs are the three controlled DOFs:
+        #   gap_avg, pitch, roll
+        # These are directly the position states.
+        # ------------------------------------------------------------------
+        C = np.zeros((3, 10))
+        C[0, GAP] = 1.0      # gap_avg
+        C[1, PITCH] = 1.0    # pitch
+        C[2, ROLL] = 1.0     # roll
+
+        # D = 0 (no direct feedthrough from PWM to position)
+        D = np.zeros((3, 4))
+
+        # ------------------------------------------------------------------
+        # Step 7: Equilibrium check
+        #
+        # At a valid operating point, the total magnetic force should
+        # equal the pod weight.  A large residual means the linearization
+        # is valid mathematically but not physically meaningful (the pod
+        # wouldn't hover at this point without acceleration).
+        # ------------------------------------------------------------------
+        F_total = plant_f.f0 + plant_b.f0
+        weight = m * GRAVITY
+        eq_error = F_total - weight
+
+        return StateSpaceResult(
+            A=A, B=B, C=C, D=D,
+            operating_point={
+                'gap_height': gap_height,
+                'currL': currL, 'currR': currR,
+                'roll': roll, 'pitch': pitch,
+                'mass': m,
+            },
+            equilibrium_force_error=eq_error,
+            plant_front=plant_f,
+            plant_back=plant_b,
+        )
+
+    def find_equilibrium_current(self, gap_height, roll=0.0, tol=0.01):
+        """
+        Find the symmetric current (currL = currR = I) that makes
+        total force = weight at the given gap height.
+
+        Uses bisection over the current range.  The search assumes
+        negative currents produce attractive (upward) force, which
+        matches the Ansys model convention.
+
+        Parameters
+        ----------
+        gap_height : float  Target gap [mm]
+        roll : float        Roll angle [deg], default 0
+        tol : float         Force tolerance [N]
+
+        Returns
+        -------
+        float : equilibrium current [A]
+        """
+        target_per_yoke = self.mass * GRAVITY / 2.0
+
+        def force_residual(curr):
+            f, _ = self.linearizer.evaluate(curr, curr, roll, gap_height)
+            return f - target_per_yoke
+
+        # Bisection search over negative current range
+        # (More negative = stronger attraction)
+        a, b = -20.0, 0.0
+        fa, fb = force_residual(a), force_residual(b)
+
+        if fa * fb > 0:
+            # Try positive range too
+            a, b = 0.0, 20.0
+            fa, fb = force_residual(a), force_residual(b)
+            if fa * fb > 0:
+                raise ValueError(
+                    f"Cannot find equilibrium current at gap={gap_height}mm. "
+                    f"Force at I=−20A: {target_per_yoke + force_residual(-20):.1f}N, "
+                    f"at I=0: {target_per_yoke + force_residual(0):.1f}N, "
+                    f"at I=+20A: {target_per_yoke + force_residual(20):.1f}N, "
+                    f"target per yoke: {target_per_yoke:.1f}N"
+                )
+
+        for _ in range(100):
+            mid = (a + b) / 2.0
+            fmid = force_residual(mid)
+            if abs(fmid) < tol:
+                return mid
+            if fa * fmid < 0:
+                b = mid
+            else:
+                a, fa = mid, fmid
+
+        return (a + b) / 2.0
+
+
+# ======================================================================
+# Demo
+# ======================================================================
+if __name__ == '__main__':
+    model_path = os.path.join(os.path.dirname(__file__), 'maglev_model.pkl')
+    lin = MaglevLinearizer(model_path)
+    ss = MaglevStateSpace(lin)
+
+    # ------------------------------------------------------------------
+    # Find the equilibrium current at the target gap
+    # ------------------------------------------------------------------
+    TARGET_GAP_MM = 16.491741  # from lev_pod_env.py
+    print("=" * 70)
+    print("FINDING EQUILIBRIUM CURRENT")
+    print("=" * 70)
+    I_eq = ss.find_equilibrium_current(TARGET_GAP_MM)
+    F_eq, T_eq = lin.evaluate(I_eq, I_eq, 0.0, TARGET_GAP_MM)
+    print(f"Target gap:    {TARGET_GAP_MM:.3f} mm")
+    print(f"Pod weight:    {ss.mass * GRAVITY:.3f} N  ({ss.mass} kg)")
+    print(f"Required per yoke: {ss.mass * GRAVITY / 2:.3f} N")
+    print(f"Equilibrium current: {I_eq:.4f} A  (symmetric, currL = currR)")
+    print(f"Force per yoke at equilibrium: {F_eq:.3f} N")
+    print(f"Equilibrium PWM duty cycle: {I_eq * ss.coil_R / ss.V_supply:.4f}")
+    print()
+
+    # ------------------------------------------------------------------
+    # Build the state-space at equilibrium
+    # ------------------------------------------------------------------
+    result = ss.build(
+        gap_height=TARGET_GAP_MM,
+        currL=I_eq,
+        currR=I_eq,
+        roll=0.0,
+        pitch=0.0,
+    )
+    print(result)
+    print()
+
+    # ------------------------------------------------------------------
+    # Show the coupling structure
+    # ------------------------------------------------------------------
+    result.print_A_structure()
+    result.print_B_structure()
+
+    # ------------------------------------------------------------------
+    # Physical interpretation of key eigenvalues
+    # ------------------------------------------------------------------
+    eigs = result.eigenvalues
+    unstable = result.unstable_eigenvalues
+    print(f"\nUnstable modes: {len(unstable)}")
+    for ev in unstable:
+        # Time to double = ln(2) / real_part
+        t_double = np.log(2) / np.real(ev) * 1000  # ms
+        print(f"  λ = {np.real(ev):+.4f}  →  amplitude doubles in {t_double:.1f} ms")
+    print()
+    print("The PID loop must have bandwidth FASTER than these unstable modes")
+    print("to stabilize the plant.")
+
+    # ------------------------------------------------------------------
+    # Gain schedule: how eigenvalues change with gap
+    # ------------------------------------------------------------------
+    print("\n" + "=" * 70)
+    print("GAIN SCHEDULE: unstable eigenvalues vs gap height")
+    print("=" * 70)
+    gaps = [8, 10, 12, 14, TARGET_GAP_MM, 18, 20, 25]
+    header = f"{'Gap [mm]':>10} {'I_eq [A]':>10} {'λ_heave':>12} {'t_dbl [ms]':>12} {'λ_pitch':>12} {'t_dbl [ms]':>12}"
+    print(header)
+    print("-" * len(header))
+    for g in gaps:
+        try:
+            I = ss.find_equilibrium_current(g)
+            r = ss.build(g, I, I, 0.0, 0.0)
+            ue = r.unstable_eigenvalues
+            real_ue = sorted(np.real(ue), reverse=True)
+            # Typically: largest = heave, second = pitch
+            lam_h = real_ue[0] if len(real_ue) > 0 else 0.0
+            lam_p = real_ue[1] if len(real_ue) > 1 else 0.0
+            t_h = np.log(2) / lam_h * 1000 if lam_h > 0 else float('inf')
+            t_p = np.log(2) / lam_p * 1000 if lam_p > 0 else float('inf')
+            print(f"{g:10.2f} {I:10.4f} {lam_h:+12.4f} {t_h:12.1f} "
+                  f"{lam_p:+12.4f} {t_p:12.1f}")
+        except ValueError as e:
+            print(f"{g:10.2f}  (no equilibrium found)")

lev_sim/lev_pod_env.py

@@ -4,13 +4,15 @@ import pybullet as p
 import pybullet_data
 import numpy as np
 import os
+from datetime import datetime
 from mag_lev_coil import MagLevCoil
 from maglev_predictor import MaglevPredictor
 
-TARGET_GAP = 16.491741 / 1000 # target gap height for 5.8 kg pod in meters
+TARGET_GAP = 11.86 / 1000 # target gap height for 9.4 kg pod in meters
 
 class LevPodEnv(gym.Env):
-    def __init__(self, use_gui=False, initial_gap_mm=10.0, max_steps=2000, disturbance_force_std=0.0):
+    def __init__(self, use_gui=False, initial_gap_mm=10.0, max_steps=2000, disturbance_force_std=0.0,
+                 record_video=False, record_telemetry=False, record_dir="recordings"):
         super(LevPodEnv, self).__init__()
 
         # Store initial gap height parameter
@@ -21,6 +23,17 @@ class LevPodEnv(gym.Env):
         # Stochastic disturbance parameter (standard deviation of random force in Newtons)
         self.disturbance_force_std = disturbance_force_std
 
+        # Recording parameters
+        self.record_video = record_video
+        self.record_telemetry = record_telemetry
+        self.record_dir = record_dir
+        self._frame_skip = 4  # Capture every 4th step → 60fps video from 240Hz sim
+        self._video_width = 640
+        self._video_height = 480
+        self._frames = []
+        self._telemetry = {}
+        self._recording_active = False
+        
         # The following was coded by AI - see [1]
         # --- 1. Define Action & Observation Spaces ---
         # Action: 4 PWM duty cycles between -1 and 1 (4 independent coils)
@@ -143,7 +156,8 @@ class LevPodEnv(gym.Env):
                     if abs(local_pos[0]) < 0.06 or abs(local_pos[1]) < 0.02:
                         self.sensor_indices.append(i)
                         if abs(local_pos[0]) < 0.001:  # Center sensors (X ≈ 0)
-                            label = "Center_Right" if local_pos[1] > 0 else "Center_Left"
+                            # +Y = Left, -Y = Right (consistent with yoke labeling)
+                            label = "Center_Left" if local_pos[1] > 0 else "Center_Right"
                         else:  # Front/back sensors (Y ≈ 0)
                             label = "Front" if local_pos[0] > 0 else "Back"
                         self.sensor_labels.append(label)
@@ -157,6 +171,48 @@ class LevPodEnv(gym.Env):
         obs = self._get_obs(initial_reset=True)
         self.current_step = 0
 
+        # --- Recording setup ---
+        # Finalize any previous episode's recording before starting new one
+        if self._recording_active:
+            self._finalize_recording()
+
+        if self.record_video or self.record_telemetry:
+            self._recording_active = True
+            os.makedirs(self.record_dir, exist_ok=True)
+
+        if self.record_video:
+            self._frames = []
+            # Pod body center ≈ start_z, yoke tops at ≈ start_z + 0.091
+            # Track bottom at Z=0. Focus camera on the pod body, looking from the side
+            # so both the pod and the track bottom (with gap between) are visible.
+            pod_center_z = start_z + 0.045  # Approximate visual center of pod
+            self._view_matrix = p.computeViewMatrixFromYawPitchRoll(
+                cameraTargetPosition=[0, 0, pod_center_z],
+                distance=0.55,
+                yaw=50,       # Front-right angle
+                pitch=-5,     # Nearly horizontal to see gap from the side
+                roll=0,
+                upAxisIndex=2,
+                physicsClientId=self.client
+            )
+            self._proj_matrix = p.computeProjectionMatrixFOV(
+                fov=35,       # Narrow FOV for less distortion at close range
+                aspect=self._video_width / self._video_height,
+                nearVal=0.01,
+                farVal=2.0,
+                physicsClientId=self.client
+            )
+
+        if self.record_telemetry:
+            self._telemetry = {
+                'time': [], 'gap_FL': [], 'gap_FR': [], 'gap_BL': [], 'gap_BR': [],
+                'gap_front_avg': [], 'gap_back_avg': [], 'gap_avg': [],
+                'roll_deg': [], 'pitch_deg': [],
+                'curr_FL': [], 'curr_FR': [], 'curr_BL': [], 'curr_BR': [],
+                'force_front': [], 'force_back': [],
+                'torque_front': [], 'torque_back': [],
+            }
+
         return obs, {}
 
     # The following was generated by AI - see [14]
@@ -295,7 +351,7 @@ class LevPodEnv(gym.Env):
             physicsClientId=self.client
         )
 
-        # --- 5b. Apply Stochastic Disturbance Force (if enabled) ---
+        # --- 5b. Apply Stochastic Disturbance Force and Torques (if enabled) ---
         if self.disturbance_force_std > 0:
             disturbance_force = np.random.normal(0, self.disturbance_force_std)
             p.applyExternalForce(
@@ -305,6 +361,17 @@ class LevPodEnv(gym.Env):
                 flags=p.LINK_FRAME,
                 physicsClientId=self.client
             )
+            # Roll and pitch disturbance torques, scaled from heave force (torque ~ force * moment_arm)
+            # Moment arm ~ 0.15 m so e.g. 1 N force -> 0.15 N·m torque std
+            disturbance_torque_std = self.disturbance_force_std * 0.15
+            roll_torque = np.random.normal(0, disturbance_torque_std)
+            pitch_torque = np.random.normal(0, disturbance_torque_std)
+            p.applyExternalTorque(
+                self.podId, -1,
+                torqueObj=[roll_torque, pitch_torque, 0],
+                flags=p.LINK_FRAME,
+                physicsClientId=self.client
+            )
 
         # --- 6. Step Simulation ---
         p.stepSimulation(physicsClientId=self.client)
@@ -318,8 +385,8 @@ class LevPodEnv(gym.Env):
         obs = self._get_obs()
         
         # --- 8. Calculate Reward ---
-        # Goal: Hover at target gap (16.5mm), minimize roll/pitch, minimize power
+        # Goal: Hover at target gap (11.8mm), minimize roll/pitch, minimize power
-        target_gap = TARGET_GAP  # 16.5mm in meters
+        target_gap = TARGET_GAP  # 11.8mm in meters
         avg_gap = (avg_gap_front + avg_gap_back) / 2
         
         gap_error = abs(avg_gap - target_gap)
@@ -391,6 +458,42 @@ class LevPodEnv(gym.Env):
             'torque_back': torque_back
         }
 
+        # --- Recording ---
+        if self.record_video and self.current_step % self._frame_skip == 0:
+            _, _, rgb, _, _ = p.getCameraImage(
+                width=self._video_width,
+                height=self._video_height,
+                viewMatrix=self._view_matrix,
+                projectionMatrix=self._proj_matrix,
+                physicsClientId=self.client
+            )
+            self._frames.append(np.array(rgb, dtype=np.uint8).reshape(
+                self._video_height, self._video_width, 4)[:, :, :3])  # RGBA → RGB
+
+        if self.record_telemetry:
+            t = self._telemetry
+            t['time'].append(self.current_step * self.dt)
+            t['gap_FL'].append(front_left_gap * 1000)
+            t['gap_FR'].append(front_right_gap * 1000)
+            t['gap_BL'].append(back_left_gap * 1000)
+            t['gap_BR'].append(back_right_gap * 1000)
+            t['gap_front_avg'].append(avg_gap_front * 1000)
+            t['gap_back_avg'].append(avg_gap_back * 1000)
+            t['gap_avg'].append(avg_gap * 1000)
+            t['roll_deg'].append(np.degrees(roll_angle))
+            t['pitch_deg'].append(np.degrees(pitch_angle))
+            t['curr_FL'].append(curr_front_L)
+            t['curr_FR'].append(curr_front_R)
+            t['curr_BL'].append(curr_back_L)
+            t['curr_BR'].append(curr_back_R)
+            t['force_front'].append(force_front)
+            t['force_back'].append(force_back)
+            t['torque_front'].append(torque_front)
+            t['torque_back'].append(torque_back)
+
+        if terminated or truncated:
+            self._finalize_recording()
+
         return obs, reward, terminated, truncated, info
 
     def apply_impulse(self, force_z: float, position: list = None):
@@ -411,6 +514,20 @@ class LevPodEnv(gym.Env):
             physicsClientId=self.client
         )
 
+    def apply_torque_impulse(self, torque_nm: list):
+        """
+        Apply a one-time impulse torque to the pod (body frame).
+
+        Args:
+            torque_nm: [Tx, Ty, Tz] in N·m (LINK_FRAME: X=roll, Y=pitch, Z=yaw)
+        """
+        p.applyExternalTorque(
+            self.podId, -1,
+            torqueObj=torque_nm,
+            flags=p.LINK_FRAME,
+            physicsClientId=self.client
+        )
+
     # The following was generated by AI - see [15]
     def _get_obs(self, initial_reset=False):
         """
@@ -511,7 +628,98 @@ class LevPodEnv(gym.Env):
         
         return temp_urdf_path
 
+    def _finalize_recording(self):
+        """Save recorded video and/or telemetry plot to disk."""
+        if not self._recording_active:
+            return
+        self._recording_active = False
+
+        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+
+        # --- Save video ---
+        if self.record_video and len(self._frames) > 0:
+            import imageio.v3 as iio
+            video_path = os.path.join(self.record_dir, f"sim_{timestamp}.mp4")
+            fps = int(round(1.0 / (self.dt * self._frame_skip)))  # 60fps
+            iio.imwrite(video_path, self._frames, fps=fps, codec="h264")
+            print(f"Video saved: {video_path} ({len(self._frames)} frames, {fps}fps)")
+            self._frames = []
+
+        # --- Save telemetry plot ---
+        if self.record_telemetry and len(self._telemetry.get('time', [])) > 0:
+            self._save_telemetry_plot(timestamp)
+
+    def _save_telemetry_plot(self, timestamp):
+        """Generate and save a 4-panel telemetry figure."""
+        import matplotlib.pyplot as plt
+
+        t = {k: np.array(v) for k, v in self._telemetry.items()}
+        time = t['time']
+        target_mm = TARGET_GAP * 1000
+        weight = 9.4 * 9.81  # Pod weight in N
+
+        fig, axes = plt.subplots(4, 1, figsize=(12, 14), sharex=True)
+        fig.suptitle(f'Simulation Telemetry  |  gap₀={self.initial_gap_mm}mm  target={target_mm:.1f}mm',
+                     fontsize=14, fontweight='bold')
+
+        # --- Panel 1: Gap heights ---
+        ax = axes[0]
+        ax.plot(time, t['gap_FL'], lw=1, alpha=0.6, label='FL')
+        ax.plot(time, t['gap_FR'], lw=1, alpha=0.6, label='FR')
+        ax.plot(time, t['gap_BL'], lw=1, alpha=0.6, label='BL')
+        ax.plot(time, t['gap_BR'], lw=1, alpha=0.6, label='BR')
+        ax.plot(time, t['gap_avg'], lw=2, color='black', label='Average')
+        ax.axhline(y=target_mm, color='orange', ls='--', lw=2, label=f'Target ({target_mm:.1f}mm)')
+        ax.set_ylabel('Gap Height (mm)')
+        ax.legend(loc='best', ncol=3, fontsize=9)
+        ax.grid(True, alpha=0.3)
+        final_err = abs(t['gap_avg'][-1] - target_mm)
+        ax.text(0.98, 0.02, f'Final error: {final_err:.3f}mm',
+                transform=ax.transAxes, ha='right', va='bottom', fontsize=10,
+                bbox=dict(boxstyle='round', facecolor='white', alpha=0.8))
+
+        # --- Panel 2: Roll & Pitch ---
+        ax = axes[1]
+        ax.plot(time, t['roll_deg'], lw=1.5, label='Roll')
+        ax.plot(time, t['pitch_deg'], lw=1.5, label='Pitch')
+        ax.axhline(y=0, color='gray', ls='--', lw=1)
+        ax.set_ylabel('Angle (degrees)')
+        ax.legend(loc='best', fontsize=9)
+        ax.grid(True, alpha=0.3)
+
+        # --- Panel 3: Coil currents ---
+        ax = axes[2]
+        ax.plot(time, t['curr_FL'], lw=1, alpha=0.8, label='FL')
+        ax.plot(time, t['curr_FR'], lw=1, alpha=0.8, label='FR')
+        ax.plot(time, t['curr_BL'], lw=1, alpha=0.8, label='BL')
+        ax.plot(time, t['curr_BR'], lw=1, alpha=0.8, label='BR')
+        total = np.abs(t['curr_FL']) + np.abs(t['curr_FR']) + np.abs(t['curr_BL']) + np.abs(t['curr_BR'])
+        ax.plot(time, total, lw=2, color='black', ls='--', label='Total |I|')
+        ax.set_ylabel('Current (A)')
+        ax.legend(loc='best', ncol=3, fontsize=9)
+        ax.grid(True, alpha=0.3)
+
+        # --- Panel 4: Forces ---
+        ax = axes[3]
+        ax.plot(time, t['force_front'], lw=1.5, label='Front yoke')
+        ax.plot(time, t['force_back'], lw=1.5, label='Back yoke')
+        f_total = t['force_front'] + t['force_back']
+        ax.plot(time, f_total, lw=2, color='black', label='Total')
+        ax.axhline(y=weight, color='red', ls='--', lw=1.5, label=f'Weight ({weight:.1f}N)')
+        ax.set_ylabel('Force (N)')
+        ax.set_xlabel('Time (s)')
+        ax.legend(loc='best', ncol=2, fontsize=9)
+        ax.grid(True, alpha=0.3)
+
+        plt.tight_layout()
+        plot_path = os.path.join(self.record_dir, f"sim_{timestamp}_telemetry.png")
+        fig.savefig(plot_path, dpi=200, bbox_inches='tight')
+        plt.close(fig)
+        print(f"Telemetry plot saved: {plot_path}")
+        self._telemetry = {}
+
     def close(self):
+        self._finalize_recording()
         try:
             p.disconnect(physicsClientId=self.client)
         except p.error:

lev_sim/maglev_predictor.py

@@ -14,17 +14,22 @@ import numpy as np
 import joblib
 import os
 
+# Default path: next to this module so it works regardless of cwd
+_DEFAULT_MODEL_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), "maglev_model.pkl")
+
+
 class MaglevPredictor:
-    def __init__(self, model_path='maglev_model.pkl'):
+    def __init__(self, model_path=None):
         """
         Initialize predictor by loading the pickle file and extracting
         raw matrices for fast inference.
         """
-        if not os.path.exists(model_path):
+        path = model_path if model_path is not None else _DEFAULT_MODEL_PATH
-            raise FileNotFoundError(f"Model file '{model_path}' not found. Please train and save the model first.")
+        if not os.path.exists(path):
+            raise FileNotFoundError(f"Model file '{path}' not found. Please train and save the model first.")
 
-        print(f"Loading maglev model from {model_path}...")
+        print(f"Loading maglev model from {path}...")
-        data = joblib.load(model_path)
+        data = joblib.load(path)
         
         # 1. Extract Scikit-Learn Objects
         poly_transformer = data['poly_features']

lev_sim/optuna_pid_tune.py

@@ -0,0 +1,337 @@
+"""
+Optuna (TPE / Bayesian-style) optimization for the three-PID maglev controller.
+Tunes all nine gains (height, roll, pitch × Kp, Ki, Kd) jointly so coupling is respected.
+
+Noisy optimization: set disturbance_force_std > 0 so the env applies random force/torque
+each step (system changes slightly each run). To keep Bayesian optimization effective,
+use n_objective_repeats > 1: each candidate is evaluated multiple times and the mean cost
+is returned, reducing variance so TPE can compare trials reliably.
+
+Run from the command line or import and call run_optimization() from a notebook.
+"""
+
+import json
+import os
+import sys
+
+import numpy as np
+import optuna
+from optuna.samplers import TPESampler
+
+from lev_pod_env import TARGET_GAP
+from pid_controller import DEFAULT_GAINS
+from pid_simulation import run_pid_simulation, build_feedforward_lut
+
+# Save pid_best_params.json next to this script so notebook and CLI find it regardless of cwd
+_SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
+
+# Default optimization config: long enough to see late instability (~8s+), not so long that optimizer goes ultra-conservative
+DEFAULT_MAX_STEPS = 1500
+DEFAULT_INITIAL_GAPS_MM = [8.0, 15.0]  # Two conditions for robustness (bracket 11.86mm target)
+DEFAULT_N_TRIALS = 200
+DEFAULT_TIMEOUT_S = 3600
+TARGET_GAP_MM = TARGET_GAP * 1000
+
+
+def _cost_from_data(
+    data: dict,
+    target_gap_mm: float,
+    penalty_early: float = 500.0,
+    weight_late_osc: float = 3.0,
+    weight_steady_state: float = 2.0,
+) -> float:
+    """
+    Single scalar cost from one run. Lower is better.
+
+    - ITAE for gap error and |roll|/|pitch| (tracking over time).
+    - Late-oscillation penalty: std(roll) and std(pitch) over the *last 50%* of the run,
+      plus max |roll|/|pitch| in that window, so gains that go unstable after ~half the run are penalized.
+    - Steady-state term: mean absolute gap error and mean |roll|/|pitch| over the *last 20%*,
+      so we reward settling at target with small angles.
+    - Penalty if episode ended early (crash/instability), regardless of run length.
+    - Small penalty on mean total current (efficiency).
+    """
+    t = np.asarray(data["time"])
+    dt = np.diff(t, prepend=0.0)
+    gap_err_mm = np.abs(np.asarray(data["gap_avg"]) - target_gap_mm)
+    itae_gap = np.sum(t * gap_err_mm * dt)
+
+    roll_deg = np.asarray(data["roll_deg"])
+    pitch_deg = np.asarray(data["pitch_deg"])
+    roll_abs = np.abs(roll_deg)
+    pitch_abs = np.abs(pitch_deg)
+    itae_roll = np.sum(t * roll_abs * dt)
+    itae_pitch = np.sum(t * pitch_abs * dt)
+
+    n = len(t)
+    early_penalty = penalty_early if data.get("terminated_early", False) else 0.0
+
+    # Late half: penalize oscillation (std) and large angles (max) so "good for 6s then violent roll" is expensive
+    half = max(1, n // 2)
+    roll_last = roll_deg[-half:]
+    pitch_last = pitch_deg[-half:]
+    late_osc_roll = np.std(roll_last) + np.max(np.abs(roll_last))
+    late_osc_pitch = np.std(pitch_last) + np.max(np.abs(pitch_last))
+    late_osc_penalty = weight_late_osc * (late_osc_roll + late_osc_pitch)
+
+    # Last 20%: steady-state error — want small gap error and small roll/pitch at end
+    tail = max(1, n // 5)
+    ss_gap = np.mean(np.abs(np.asarray(data["gap_avg"])[-tail:] - target_gap_mm))
+    ss_roll = np.mean(roll_abs[-tail:])
+    ss_pitch = np.mean(pitch_abs[-tail:])
+    steady_state_penalty = weight_steady_state * (ss_gap + ss_roll + ss_pitch)
+
+    mean_current = np.mean(data["current_total"])
+    current_penalty = 0.01 * mean_current
+
+    return (
+        itae_gap
+        + 2.0 * (itae_roll + itae_pitch)
+        + early_penalty
+        + late_osc_penalty
+        + steady_state_penalty
+        + current_penalty
+    )
+
+
+def objective(
+    trial: optuna.Trial,
+    initial_gaps_mm: list,
+    max_steps: int,
+    use_feedforward: bool,
+    penalty_early: float,
+    disturbance_force_std: float = 0.0,
+    n_objective_repeats: int = 1,
+) -> float:
+    """
+    Optuna objective: suggest 9 gains, run simulation(s), return cost.
+
+    With disturbance_force_std > 0, the env applies random force/torque disturbances each step,
+    so the same gains yield different costs across runs. For Bayesian optimization under noise,
+    set n_objective_repeats > 1 to evaluate each candidate multiple times and return the mean cost,
+    reducing variance so TPE can compare trials reliably.
+    """
+    # All three loops tuned together (recommended: they interact)
+    height_kp = trial.suggest_float("height_kp", 5.0, 200.0, log=True)
+    height_ki = trial.suggest_float("height_ki", 0.5, 50.0, log=True)
+    height_kd = trial.suggest_float("height_kd", 1.0, 50.0, log=True)
+    roll_kp = trial.suggest_float("roll_kp", 0.1, 20.0, log=True)
+    roll_ki = trial.suggest_float("roll_ki", 0.01, 5.0, log=True)
+    roll_kd = trial.suggest_float("roll_kd", 0.01, 5.0, log=True)
+    pitch_kp = trial.suggest_float("pitch_kp", 0.1, 20.0, log=True)
+    pitch_ki = trial.suggest_float("pitch_ki", 0.01, 5.0, log=True)
+    pitch_kd = trial.suggest_float("pitch_kd", 0.01, 5.0, log=True)
+
+    gains = {
+        "height_kp": height_kp, "height_ki": height_ki, "height_kd": height_kd,
+        "roll_kp": roll_kp, "roll_ki": roll_ki, "roll_kd": roll_kd,
+        "pitch_kp": pitch_kp, "pitch_ki": pitch_ki, "pitch_kd": pitch_kd,
+    }
+
+    cost_sum = 0.0
+    n_evals = 0
+    for _ in range(n_objective_repeats):
+        for initial_gap in initial_gaps_mm:
+            data = run_pid_simulation(
+                initial_gap_mm=initial_gap,
+                max_steps=max_steps,
+                use_gui=False,
+                disturbance_force_std=disturbance_force_std,
+                use_feedforward=use_feedforward,
+                record_video=False,
+                record_telemetry=False,
+                gains=gains,
+                verbose=False,
+            )
+            n = len(data["time"])
+            data["terminated_early"] = n < max_steps - 10
+            cost_sum += _cost_from_data(data, TARGET_GAP_MM, penalty_early=penalty_early)
+            n_evals += 1
+    return cost_sum / n_evals
+
+
+def run_optimization(
+    n_trials: int = DEFAULT_N_TRIALS,
+    timeout: int = DEFAULT_TIMEOUT_S,
+    initial_gaps_mm: list = None,
+    max_steps: int = DEFAULT_MAX_STEPS,
+    use_feedforward: bool = True,
+    penalty_early: float = 500.0,
+    disturbance_force_std: float = 0.0,
+    n_objective_repeats: int = 1,
+    out_dir: str = None,
+    study_name: str = "pid_maglev",
+) -> optuna.Study:
+    """
+    Run Optuna study (TPE sampler) and save best params to JSON.
+
+    disturbance_force_std: passed to env so each step gets random force/torque noise (N).
+    n_objective_repeats: when > 1, each trial is evaluated this many times (different noise)
+        and the mean cost is returned, so Bayesian optimization sees a less noisy objective.
+
+    Returns:
+        study: Optuna Study (study.best_params, study.best_value).
+    """
+    if initial_gaps_mm is None:
+        initial_gaps_mm = DEFAULT_INITIAL_GAPS_MM
+    if out_dir is None:
+        out_dir = _SCRIPT_DIR
+
+    # Build feedforward LUT once so first trial doesn't do it inside run
+    build_feedforward_lut()
+
+    sampler = TPESampler(
+        n_startup_trials=min(20, n_trials // 4),
+        n_ei_candidates=24,
+        seed=42,
+    )
+    study = optuna.create_study(
+        direction="minimize",
+        study_name=study_name,
+        sampler=sampler,
+    )
+
+    study.optimize(
+        lambda t: objective(
+            t,
+            initial_gaps_mm=initial_gaps_mm,
+            max_steps=max_steps,
+            use_feedforward=use_feedforward,
+            penalty_early=penalty_early,
+            disturbance_force_std=disturbance_force_std,
+            n_objective_repeats=n_objective_repeats,
+        ),
+        n_trials=n_trials,
+        timeout=timeout,
+        show_progress_bar=True,
+    )
+
+    out_path = os.path.join(out_dir, "pid_best_params.json")
+    with open(out_path, "w") as f:
+        json.dump(study.best_params, f, indent=2)
+    print(f"Best cost: {study.best_value:.4f}")
+    print(f"Best params saved to {out_path}")
+    return study
+
+
+def run_staged_optimization(
+    stage_steps: list = None,
+    n_trials_per_stage: int = 50,
+    timeout_per_stage: int = None,
+    initial_gaps_mm: list = None,
+    use_feedforward: bool = True,
+    penalty_early: float = 500.0,
+    disturbance_force_std: float = 0.0,
+    n_objective_repeats: int = 1,
+    out_dir: str = None,
+) -> list:
+    """
+    Run optimization in stages with increasing max_steps, warm-starting each stage from the previous best.
+
+    disturbance_force_std: passed to env for stochastic force/torque noise (N).
+    n_objective_repeats: mean over this many evaluations per trial for a less noisy objective.
+
+    Example: stage_steps=[1500, 3000, 6000]
+    - Stage 1: optimize with 1500 steps (finds good gap/initial roll), save best.
+    - Stage 2: optimize with 3000 steps; first trial is the 1500-step best (evaluated at 3000 steps), then TPE suggests improvements.
+    - Stage 3: same with 6000 steps starting from stage 2's best.
+
+    This keeps good lift-off and gap control from the short-horizon run while refining for late-horizon roll stability.
+    """
+    if stage_steps is None:
+        stage_steps = [1500, 3000, 6000]
+    if initial_gaps_mm is None:
+        initial_gaps_mm = DEFAULT_INITIAL_GAPS_MM
+    if out_dir is None:
+        out_dir = _SCRIPT_DIR
+    if timeout_per_stage is None:
+        timeout_per_stage = DEFAULT_TIMEOUT_S
+
+    build_feedforward_lut()
+    best_params_path = os.path.join(out_dir, "pid_best_params.json")
+    studies = []
+
+    for stage_idx, max_steps in enumerate(stage_steps):
+        print(f"\n--- Stage {stage_idx + 1}/{len(stage_steps)}: max_steps={max_steps} ---")
+        study = optuna.create_study(
+            direction="minimize",
+            study_name=f"pid_maglev_stage_{max_steps}",
+            sampler=TPESampler(
+                n_startup_trials=min(20, n_trials_per_stage // 4),
+                n_ei_candidates=24,
+                seed=42 + stage_idx,
+            ),
+        )
+
+        # Warm start: enqueue previous stage's best so we refine from it (stage 0 has no previous)
+        if stage_idx > 0 and os.path.isfile(best_params_path):
+            with open(best_params_path) as f:
+                prev_best = json.load(f)
+            study.enqueue_trial(prev_best)
+            print(f"Enqueued previous best params (from stage {stage_steps[stage_idx - 1]} steps) as first trial.")
+
+        study.optimize(
+            lambda t: objective(
+                t,
+                initial_gaps_mm=initial_gaps_mm,
+                max_steps=max_steps,
+                use_feedforward=use_feedforward,
+                penalty_early=penalty_early,
+                disturbance_force_std=disturbance_force_std,
+                n_objective_repeats=n_objective_repeats,
+            ),
+            n_trials=n_trials_per_stage,
+            timeout=timeout_per_stage,
+            show_progress_bar=True,
+        )
+
+        with open(best_params_path, "w") as f:
+            json.dump(study.best_params, f, indent=2)
+        stage_path = os.path.join(out_dir, f"pid_best_params_{max_steps}.json")
+        with open(stage_path, "w") as f:
+            json.dump(study.best_params, f, indent=2)
+        print(f"Stage best cost: {study.best_value:.4f} -> saved to {best_params_path} and {stage_path}")
+        studies.append(study)
+
+    print(f"\nStaged optimization done. Final best params in {best_params_path}")
+    return studies
+
+
+if __name__ == "__main__":
+    import argparse
+    parser = argparse.ArgumentParser(description="Optuna PID tuning for maglev three-loop controller")
+    parser.add_argument("--n_trials", type=int, default=DEFAULT_N_TRIALS)
+    parser.add_argument("--timeout", type=int, default=DEFAULT_TIMEOUT_S)
+    parser.add_argument("--max_steps", type=int, default=DEFAULT_MAX_STEPS)
+    parser.add_argument("--gaps", type=float, nargs="+", default=DEFAULT_INITIAL_GAPS_MM)
+    parser.add_argument("--out_dir", type=str, default=_SCRIPT_DIR, help="Directory for pid_best_params.json (default: same as script)")
+    parser.add_argument("--no_feedforward", action="store_true")
+    parser.add_argument("--staged", action="store_true", help="Staged optimization: 1500 -> 3000 -> 6000 steps, each stage warm-starts from previous best")
+    parser.add_argument("--stage_steps", type=int, nargs="+", default=[1500, 3000, 6000], help="Steps per stage when using --staged (default: 1500 3000 6000)")
+    parser.add_argument("--n_trials_per_stage", type=int, default=DEFAULT_N_TRIALS, help="Trials per stage when using --staged")
+    parser.add_argument("--disturbance_force_std", type=float, default=0.0, help="Env disturbance force std (N); roll/pitch torque scale with this. Use >0 for noisy optimization.")
+    parser.add_argument("--n_objective_repeats", type=int, default=1, help="Evaluate each trial this many times and report mean cost (reduces noise for Bayesian optimization)")
+    args = parser.parse_args()
+
+    if args.staged:
+        run_staged_optimization(
+            stage_steps=args.stage_steps,
+            n_trials_per_stage=args.n_trials_per_stage,
+            timeout_per_stage=args.timeout,
+            initial_gaps_mm=args.gaps,
+            use_feedforward=not args.no_feedforward,
+            disturbance_force_std=args.disturbance_force_std,
+            n_objective_repeats=args.n_objective_repeats,
+            out_dir=args.out_dir,
+        )
+    else:
+        run_optimization(
+            n_trials=args.n_trials,
+            timeout=args.timeout,
+            initial_gaps_mm=args.gaps,
+            max_steps=args.max_steps,
+            use_feedforward=not args.no_feedforward,
+            disturbance_force_std=args.disturbance_force_std,
+            n_objective_repeats=args.n_objective_repeats,
+            out_dir=args.out_dir,
+        )

lev_sim/pid_best_params.json

@@ -0,0 +1,11 @@
+{
+  "height_kp": 100.962395560374814,
+  "height_ki": 0.0,
+  "height_kd": 8.1738092637166575,
+  "roll_kp": 0.600856607986966,
+  "roll_ki": 0.0,
+  "roll_kd": -0.1,
+  "pitch_kp": 50.0114708799258271,
+  "pitch_ki": 0,
+  "pitch_kd": 1.8990306608320433
+}

lev_sim/pid_best_params_1500.json

@@ -0,0 +1,11 @@
+{
+  "height_kp": 61.34660453658844,
+  "height_ki": 5.337339965349835,
+  "height_kd": 12.13071554123404,
+  "roll_kp": 5.838881924776812,
+  "roll_ki": 0.11040111644948386,
+  "roll_kd": 0.0401383180893775,
+  "pitch_kp": 0.10114651080341679,
+  "pitch_ki": 0.06948994902747452,
+  "pitch_kd": 0.16948986671689478
+}

lev_sim/pid_best_params_3000.json

@@ -0,0 +1,11 @@
+{
+  "height_kp": 6.79952358593656,
+  "height_ki": 2.7199339229214856,
+  "height_kd": 12.166576111298163,
+  "roll_kp": 1.7073146141313746,
+  "roll_ki": 0.026221209129363342,
+  "roll_kd": 0.018353603438859525,
+  "pitch_kp": 1.2333241666054757,
+  "pitch_ki": 0.03544610305322942,
+  "pitch_kd": 0.3711162924888727
+}

lev_sim/pid_best_params_6000.json

@@ -0,0 +1,11 @@
+{
+  "height_kp": 5.962395560374814,
+  "height_ki": 0.5381137743695537,
+  "height_kd": 1.1738092637166575,
+  "roll_kp": 0.48249575996443006,
+  "roll_ki": 0.016923890033141546,
+  "roll_kd": 0.013609429509460135,
+  "pitch_kp": 0.10114708799258271,
+  "pitch_ki": 1.906050976563914,
+  "pitch_kd": 1.8990306608320433
+}

lev_sim/pid_controller.py

@@ -0,0 +1,92 @@
+"""
+PID controller and default gains for the maglev three-loop (height, roll, pitch) control.
+Used by lev_PID.ipynb and optuna_pid_tune.py.
+"""
+
+import numpy as np
+
+
+class PIDController:
+    """Simple PID controller with anti-windup."""
+
+    def __init__(
+        self,
+        kp: float,
+        ki: float,
+        kd: float,
+        output_min: float = -1.0,
+        output_max: float = 1.0,
+        integral_limit: float = None,
+    ):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.output_min = output_min
+        self.output_max = output_max
+        self.integral_limit = (
+            integral_limit if integral_limit is not None else abs(output_max) * 2
+        )
+
+        self.integral = 0.0
+        self.prev_error = 0.0
+        self.first_update = True
+
+    def reset(self):
+        """Reset controller state."""
+        self.integral = 0.0
+        self.prev_error = 0.0
+        self.first_update = True
+
+    def update(self, error: float, dt: float) -> float:
+        """Compute PID output.
+
+        Args:
+            error: Current error (setpoint - measurement)
+            dt: Time step in seconds
+
+        Returns:
+            Control output (clamped to output limits)
+        """
+        p_term = self.kp * error
+
+        self.integral += error * dt
+        self.integral = np.clip(
+            self.integral, -self.integral_limit, self.integral_limit
+        )
+        i_term = self.ki * self.integral
+
+        if self.first_update:
+            d_term = 0.0
+            self.first_update = False
+        else:
+            d_term = self.kd * (error - self.prev_error) / dt
+
+        self.prev_error = error
+        output = p_term + i_term + d_term
+        return np.clip(output, self.output_min, self.output_max)
+
+
+# Default gains: height (main), roll, pitch.
+# Optimizer and notebook can override via gains dict passed to run_pid_simulation.
+DEFAULT_GAINS = {
+    "height_kp": 50.0,
+    "height_ki": 5.0,
+    "height_kd": 10.0,
+    "roll_kp": 2.0,
+    "roll_ki": 0.5,
+    "roll_kd": 0.5,
+    "pitch_kp": 2.0,
+    "pitch_ki": 0.5,
+    "pitch_kd": 0.5,
+}
+
+# Backward-compat names for notebook (optional)
+HEIGHT_KP = DEFAULT_GAINS["height_kp"]
+HEIGHT_KI = DEFAULT_GAINS["height_ki"]
+HEIGHT_KD = DEFAULT_GAINS["height_kd"]
+ROLL_KP = DEFAULT_GAINS["roll_kp"]
+ROLL_KI = DEFAULT_GAINS["roll_ki"]
+ROLL_KD = DEFAULT_GAINS["roll_kd"]
+PITCH_KP = DEFAULT_GAINS["pitch_kp"]
+PITCH_KI = DEFAULT_GAINS["pitch_ki"]
+PITCH_KD = DEFAULT_GAINS["pitch_kd"]

lev_sim/pid_simulation.py

@@ -0,0 +1,216 @@
+"""
+Feedforward + PID simulation runner for the maglev pod.
+Uses LevPodEnv, MaglevPredictor (feedforward), and PIDController with configurable gains.
+"""
+
+import numpy as np
+from lev_pod_env import LevPodEnv, TARGET_GAP
+from maglev_predictor import MaglevPredictor
+from pid_controller import PIDController, DEFAULT_GAINS
+
+# Feedforward LUT (built on first use)
+_FF_GAP_LUT = None
+_FF_PWM_LUT = None
+
+
+def build_feedforward_lut(
+    pod_mass: float = 9.4,
+    coil_r: float = 1.1,
+    v_supply: float = 12.0,
+    gap_min: float = 3.0,
+    gap_max: float = 35.0,
+    n_points: int = 500,
+):
+    """Build gap [mm] -> equilibrium PWM lookup table. Returns (gap_lut, pwm_lut) for np.interp."""
+    global _FF_GAP_LUT, _FF_PWM_LUT
+    target_per_yoke = pod_mass * 9.81 / 2.0
+    predictor = MaglevPredictor()
+
+    def _find_eq_current(gap_mm):
+        a, b = -10.2, 10.2
+        fa, _ = predictor.predict(a, a, 0.0, gap_mm)
+        for _ in range(80):
+            mid = (a + b) / 2.0
+            fm, _ = predictor.predict(mid, mid, 0.0, gap_mm)
+            if (fa > target_per_yoke) == (fm > target_per_yoke):
+                a, fa = mid, fm
+            else:
+                b = mid
+        return (a + b) / 2.0
+
+    _FF_GAP_LUT = np.linspace(gap_min, gap_max, n_points)
+    curr_lut = np.array([_find_eq_current(g) for g in _FF_GAP_LUT])
+    _FF_PWM_LUT = np.clip(curr_lut * coil_r / v_supply, -1.0, 1.0)
+    return _FF_GAP_LUT, _FF_PWM_LUT
+
+
+def feedforward_pwm(gap_mm: float) -> float:
+    """Interpolate equilibrium PWM for gap height [mm]. Builds LUT on first call."""
+    global _FF_GAP_LUT, _FF_PWM_LUT
+    if _FF_GAP_LUT is None:
+        build_feedforward_lut()
+    return float(np.interp(gap_mm, _FF_GAP_LUT, _FF_PWM_LUT))
+
+
+def run_pid_simulation(
+    initial_gap_mm: float = 14.0,
+    max_steps: int = 2000,
+    use_gui: bool = False,
+    disturbance_step: int = None,
+    disturbance_force: float = 0.0,
+    disturbance_force_std: float = 0.0,
+    use_feedforward: bool = True,
+    record_video: bool = False,
+    record_telemetry: bool = False,
+    record_dir: str = "recordings",
+    gains: dict = None,
+    verbose: bool = True,
+) -> dict:
+    """
+    Run one feedforward + PID simulation. Gains can be passed for tuning (e.g. Optuna).
+
+    Args:
+        initial_gap_mm: Starting gap height (mm).
+        max_steps: Max simulation steps.
+        use_gui: PyBullet GUI (avoid in notebooks).
+        disturbance_step: Step for impulse (None = none).
+        disturbance_force: Impulse force (N).
+        disturbance_force_std: Noise std (N).
+        use_feedforward: Use MaglevPredictor feedforward.
+        record_video: Save MP4.
+        record_telemetry: Save telemetry PNG.
+        record_dir: Output directory.
+        gains: Dict with keys height_kp, height_ki, height_kd, roll_kp, roll_ki, roll_kd,
+               pitch_kp, pitch_ki, pitch_kd. If None, uses DEFAULT_GAINS.
+        verbose: Print progress.
+
+    Returns:
+        data: dict of arrays (time, gap_avg, roll_deg, pitch_deg, currents, pwms, ...).
+    """
+    g = gains if gains is not None else DEFAULT_GAINS
+    env = LevPodEnv(
+        use_gui=use_gui,
+        initial_gap_mm=initial_gap_mm,
+        max_steps=max_steps,
+        disturbance_force_std=disturbance_force_std,
+        record_video=record_video,
+        record_telemetry=record_telemetry,
+        record_dir=record_dir,
+    )
+    dt = env.dt
+
+    height_pid = PIDController(
+        g["height_kp"], g["height_ki"], g["height_kd"],
+        output_min=-1.0, output_max=1.0,
+    )
+    roll_pid = PIDController(
+        g["roll_kp"], g["roll_ki"], g["roll_kd"],
+        output_min=-0.5, output_max=0.5,
+    )
+    pitch_pid = PIDController(
+        g["pitch_kp"], g["pitch_ki"], g["pitch_kd"],
+        output_min=-0.5, output_max=0.5,
+    )
+
+    data = {
+        "time": [], "gap_front": [], "gap_back": [], "gap_avg": [],
+        "roll_deg": [], "pitch_deg": [],
+        "current_FL": [], "current_FR": [], "current_BL": [], "current_BR": [],
+        "current_total": [], "pwm_FL": [], "pwm_FR": [], "pwm_BL": [], "pwm_BR": [],
+        "ff_pwm": [],
+    }
+
+    obs, _ = env.reset()
+    target_gap = TARGET_GAP
+    y_distance = 0.1016
+    x_distance = 0.2518
+
+    if verbose:
+        print(f"Starting simulation: initial_gap={initial_gap_mm}mm, target={target_gap*1000:.2f}mm")
+        if disturbance_step is not None:
+            print(f"  Impulse disturbance: {disturbance_force}N at step {disturbance_step}")
+        if disturbance_force_std > 0:
+            print(f"  Stochastic noise: std={disturbance_force_std}N")
+        print(f"  Feedforward: {'enabled' if use_feedforward else 'disabled'}")
+        if record_video or record_telemetry:
+            print(f"  Recording: video={record_video}, telemetry={record_telemetry} → {record_dir}/")
+
+    for step in range(max_steps):
+        t = step * dt
+        gaps_normalized = obs[:4]
+        gaps = gaps_normalized * env.gap_scale + TARGET_GAP
+        gap_front = gaps[2]
+        gap_back = gaps[3]
+        gap_left = gaps[1]
+        gap_right = gaps[0]
+        gap_avg = (gap_front + gap_back + gap_left + gap_right) / 4
+
+        roll_angle = np.arcsin(np.clip((gap_left - gap_right) / y_distance, -1, 1))
+        pitch_angle = np.arcsin(np.clip((gap_back - gap_front) / x_distance, -1, 1))
+
+        ff_base = feedforward_pwm(gap_avg * 1000) if use_feedforward else 0.0
+
+        height_error = target_gap - gap_avg
+        roll_error = -roll_angle
+        pitch_error = -pitch_angle
+
+        height_correction = height_pid.update(height_error, dt)
+        roll_correction = roll_pid.update(roll_error, dt)
+        pitch_correction = pitch_pid.update(pitch_error, dt)
+
+        pwm_FL = np.clip(ff_base + height_correction - roll_correction - pitch_correction, -1, 1)
+        pwm_FR = np.clip(ff_base + height_correction + roll_correction - pitch_correction, -1, 1)
+        pwm_BL = np.clip(ff_base + height_correction - roll_correction + pitch_correction, -1, 1)
+        pwm_BR = np.clip(ff_base + height_correction + roll_correction + pitch_correction, -1, 1)
+
+        action = np.array([pwm_FL, pwm_FR, pwm_BL, pwm_BR], dtype=np.float32)
+
+        if disturbance_step is not None and step == disturbance_step:
+            env.apply_impulse(disturbance_force)
+            # Coupled torque: random direction, magnitude = 0.5 * |impulse force| (N·m)
+            torque_mag = 0.5 * abs(disturbance_force)
+            direction = np.random.randn(3)
+            direction = direction / (np.linalg.norm(direction) + 1e-12)
+            torque_nm = (torque_mag * direction).tolist()
+            env.apply_torque_impulse(torque_nm)
+            if verbose:
+                print(f"  Applied {disturbance_force}N impulse and {torque_mag:.2f} N·m torque at step {step}")
+
+        obs, _, terminated, truncated, info = env.step(action)
+
+        data["time"].append(t)
+        data["gap_front"].append(info["gap_front_yoke"] * 1000)
+        data["gap_back"].append(info["gap_back_yoke"] * 1000)
+        data["gap_avg"].append(info["gap_avg"] * 1000)
+        data["roll_deg"].append(np.degrees(info["roll"]))
+        data["pitch_deg"].append(np.degrees(info["pitch"]))
+        data["current_FL"].append(info["curr_front_L"])
+        data["current_FR"].append(info["curr_front_R"])
+        data["current_BL"].append(info["curr_back_L"])
+        data["current_BR"].append(info["curr_back_R"])
+        data["current_total"].append(
+            abs(info["curr_front_L"]) + abs(info["curr_front_R"])
+            + abs(info["curr_back_L"]) + abs(info["curr_back_R"])
+        )
+        data["ff_pwm"].append(ff_base)
+        data["pwm_FL"].append(pwm_FL)
+        data["pwm_FR"].append(pwm_FR)
+        data["pwm_BL"].append(pwm_BL)
+        data["pwm_BR"].append(pwm_BR)
+
+        if terminated or truncated:
+            if verbose:
+                print(f"  Simulation ended at step {step} (terminated={terminated})")
+            break
+
+    env.close()
+
+    for key in data:
+        data[key] = np.array(data[key])
+
+    if verbose:
+        print(f"Simulation complete: {len(data['time'])} steps, {data['time'][-1]:.2f}s")
+        print(f"  Final gap: {data['gap_avg'][-1]:.2f}mm (target: {target_gap*1000:.2f}mm)")
+        print(f"  Final roll: {data['roll_deg'][-1]:.3f}°, pitch: {data['pitch_deg'][-1]:.3f}°")
+
+    return data

lev_sim/pod.xml

@@ -2,8 +2,8 @@
 <robot name="lev_pod">
   <link name="base_link">
     <inertial>
-      <mass value="5.8"/>
+      <mass value="9.4"/>
-      <inertia ixx="0.0192942414" ixy="0.0" ixz="0.0" iyy="0.130582305" iyz="0.0" izz="0.13760599326"/>
+      <inertia ixx="0.03162537654" ixy="0.0" ixz="0.0" iyy="0.21929017831" iyz="0.0" izz="0.21430205089"/>
     </inertial>
     
     <collision>
@@ -13,19 +13,19 @@
     
     Bolts
     <collision>
-      <origin rpy="0 0 0" xyz="0.285 0.03 0.09585"/>
+      <origin rpy="0 0 0" xyz="0.285 0.03 0.09085"/>
       <geometry><box size="0.01 0.01 0.01"/></geometry>
     </collision>
     <collision>
-      <origin rpy="0 0 0" xyz="0.285 -0.03 0.09585"/>
+      <origin rpy="0 0 0" xyz="0.285 -0.03 0.09085"/>
       <geometry><box size="0.01 0.01 0.01"/></geometry>
     </collision>
     <collision>
-      <origin rpy="0 0 0" xyz="-0.285 0.03 0.09585"/>
+      <origin rpy="0 0 0" xyz="-0.285 0.03 0.09085"/>
       <geometry><box size="0.01 0.01 0.01"/></geometry>
     </collision>
     <collision>
-      <origin rpy="0 0 0" xyz="-0.285 -0.03 0.09585"/>
+      <origin rpy="0 0 0" xyz="-0.285 -0.03 0.09085"/>
       <geometry><box size="0.01 0.01 0.01"/></geometry>
     </collision>
 

lev_sim/polynomial_equations.txt

@@ -0,0 +1,430 @@
+Magnetic Levitation Force & Torque — Explicit Polynomial Equations
+Polynomial Degree: 6
+Variables:
+  currL [A]      = Left coil current
+  currR [A]      = Right coil current
+  rollDeg [deg]  = Roll angle
+  invGap         = 1 / GapHeight [1/mm]
+
+================================================================================
+FORCE [N] = 
+    -1.3837763236e+01
+  + (+3.4900958018e-01) * currL [A]
+  + (+3.4444782070e-01) * currR [A]
+  + (+1.3948921341e-07) * rollDeg [deg]
+  + (+6.3404397040e+02) * invGap
+  + (+2.0808153074e-02) * currL [A]^2
+  + (-8.2940557511e-04) * currL [A] * currR [A]
+  + (+2.2629904125e-02) * currL [A] * rollDeg [deg]
+  + (-1.4941727622e+01) * currL [A] * invGap
+  + (+2.1137560003e-02) * currR [A]^2
+  + (-2.2629886279e-02) * currR [A] * rollDeg [deg]
+  + (-1.4834895226e+01) * currR [A] * invGap
+  + (-7.3648244412e-01) * rollDeg [deg]^2
+  + (-7.6740357314e-06) * rollDeg [deg] * invGap
+  + (+1.3767217831e+03) * invGap^2
+  + (-1.1531758728e-04) * currL [A]^3
+  + (+7.2971576377e-05) * currL [A]^2 * currR [A]
+  + (+5.4868393021e-06) * currL [A]^2 * rollDeg [deg]
+  + (-1.0521912466e+00) * currL [A]^2 * invGap
+  + (+5.2348894922e-05) * currL [A] * currR [A]^2
+  + (+2.1794350221e-09) * currL [A] * currR [A] * rollDeg [deg]
+  + (+3.2678067078e-02) * currL [A] * currR [A] * invGap
+  + (-6.3017520966e-03) * currL [A] * rollDeg [deg]^2
+  + (-2.6462505434e-01) * currL [A] * rollDeg [deg] * invGap
+  + (-2.9510804409e+01) * currL [A] * invGap^2
+  + (-7.2152022082e-05) * currR [A]^3
+  + (-5.4823932308e-06) * currR [A]^2 * rollDeg [deg]
+  + (-1.0708822638e+00) * currR [A]^2 * invGap
+  + (-6.3914882711e-03) * currR [A] * rollDeg [deg]^2
+  + (+2.6462554272e-01) * currR [A] * rollDeg [deg] * invGap
+  + (-3.0234998506e+01) * currR [A] * invGap^2
+  + (+4.5881294085e-09) * rollDeg [deg]^3
+  + (+2.5797308313e+01) * rollDeg [deg]^2 * invGap
+  + (+4.6682794236e-05) * rollDeg [deg] * invGap^2
+  + (-5.3401486713e+03) * invGap^3
+  + (+1.3320765542e-06) * currL [A]^4
+  + (+3.2152411427e-06) * currL [A]^3 * currR [A]
+  + (+1.2629467079e-05) * currL [A]^3 * rollDeg [deg]
+  + (+1.7892402808e-03) * currL [A]^3 * invGap
+  + (+3.0619624267e-06) * currL [A]^2 * currR [A]^2
+  + (-1.5568499556e-07) * currL [A]^2 * currR [A] * rollDeg [deg]
+  + (-1.0714912904e-03) * currL [A]^2 * currR [A] * invGap
+  + (+8.7713928261e-05) * currL [A]^2 * rollDeg [deg]^2
+  + (+4.5854056157e-03) * currL [A]^2 * rollDeg [deg] * invGap
+  + (+2.0746027602e+01) * currL [A]^2 * invGap^2
+  + (+3.8655549044e-06) * currL [A] * currR [A]^3
+  + (+1.5545726439e-07) * currL [A] * currR [A]^2 * rollDeg [deg]
+  + (-7.0411774478e-04) * currL [A] * currR [A]^2 * invGap
+  + (-3.8261231547e-05) * currL [A] * currR [A] * rollDeg [deg]^2
+  + (-9.5017239129e-09) * currL [A] * currR [A] * rollDeg [deg] * invGap
+  + (+7.0646804762e-01) * currL [A] * currR [A] * invGap^2
+  + (-2.5574383917e-03) * currL [A] * rollDeg [deg]^3
+  + (-3.3355499360e-02) * currL [A] * rollDeg [deg]^2 * invGap
+  + (-2.1337761184e+01) * currL [A] * rollDeg [deg] * invGap^2
+  + (+1.2343471808e+02) * currL [A] * invGap^3
+  + (+7.7592074199e-07) * currR [A]^4
+  + (-1.2629426979e-05) * currR [A]^3 * rollDeg [deg]
+  + (+8.6147973530e-04) * currR [A]^3 * invGap
+  + (+8.0130692138e-05) * currR [A]^2 * rollDeg [deg]^2
+  + (-4.5854241649e-03) * currR [A]^2 * rollDeg [deg] * invGap
+  + (+2.1139446060e+01) * currR [A]^2 * invGap^2
+  + (+2.5574380197e-03) * currR [A] * rollDeg [deg]^3
+  + (-2.9857453053e-02) * currR [A] * rollDeg [deg]^2 * invGap
+  + (+2.1337762097e+01) * currR [A] * rollDeg [deg] * invGap^2
+  + (+1.2514911864e+02) * currR [A] * invGap^3
+  + (+2.9443521553e-02) * rollDeg [deg]^4
+  + (-7.9265644452e-09) * rollDeg [deg]^3 * invGap
+  + (-2.3563990269e+02) * rollDeg [deg]^2 * invGap^2
+  + (+1.6353835994e-04) * rollDeg [deg] * invGap^3
+  + (-2.2354105572e+03) * invGap^4
+  + (+3.1117360777e-07) * currL [A]^5
+  + (-1.8750259301e-07) * currL [A]^4 * currR [A]
+  + (-8.5198564648e-08) * currL [A]^4 * rollDeg [deg]
+  + (-6.3869937977e-05) * currL [A]^4 * invGap
+  + (-1.4911217505e-07) * currL [A]^3 * currR [A]^2
+  + (+1.7061909929e-07) * currL [A]^3 * currR [A] * rollDeg [deg]
+  + (-7.4262537249e-06) * currL [A]^3 * currR [A] * invGap
+  + (+7.2402232831e-07) * currL [A]^3 * rollDeg [deg]^2
+  + (+2.1841125430e-05) * currL [A]^3 * rollDeg [deg] * invGap
+  + (-3.4357925518e-03) * currL [A]^3 * invGap^2
+  + (-1.6416880300e-07) * currL [A]^2 * currR [A]^3
+  + (+3.6385798723e-05) * currL [A]^2 * currR [A]^2 * invGap
+  + (+5.2924336558e-07) * currL [A]^2 * currR [A] * rollDeg [deg]^2
+  + (+8.5467922645e-05) * currL [A]^2 * currR [A] * rollDeg [deg] * invGap
+  + (+4.5573594510e-03) * currL [A]^2 * currR [A] * invGap^2
+  + (-2.9680843072e-05) * currL [A]^2 * rollDeg [deg]^3
+  + (-3.7951808035e-03) * currL [A]^2 * rollDeg [deg]^2 * invGap
+  + (+4.7717250306e-02) * currL [A]^2 * rollDeg [deg] * invGap^2
+  + (-1.4608679078e+02) * currL [A]^2 * invGap^3
+  + (-1.2370955460e-07) * currL [A] * currR [A]^4
+  + (-1.7060417434e-07) * currL [A] * currR [A]^3 * rollDeg [deg]
+  + (-1.3936700967e-05) * currL [A] * currR [A]^3 * invGap
+  + (+4.5908154078e-07) * currL [A] * currR [A]^2 * rollDeg [deg]^2
+  + (-8.5469212391e-05) * currL [A] * currR [A]^2 * rollDeg [deg] * invGap
+  + (+1.6991917750e-03) * currL [A] * currR [A]^2 * invGap^2
+  + (-3.7373312737e-04) * currL [A] * currR [A] * rollDeg [deg]^2 * invGap
+  + (+1.5086926031e-07) * currL [A] * currR [A] * rollDeg [deg] * invGap^2
+  + (-7.5987537316e+00) * currL [A] * currR [A] * invGap^3
+  + (+6.6508849418e-04) * currL [A] * rollDeg [deg]^4
+  + (+9.6378055653e-02) * currL [A] * rollDeg [deg]^3 * invGap
+  + (+2.4435308561e+00) * currL [A] * rollDeg [deg]^2 * invGap^2
+  + (+1.2664933542e+02) * currL [A] * rollDeg [deg] * invGap^3
+  + (+5.1887450366e+01) * currL [A] * invGap^4
+  + (+2.3180087538e-07) * currR [A]^5
+  + (+8.5217195078e-08) * currR [A]^4 * rollDeg [deg]
+  + (-6.9233585420e-05) * currR [A]^4 * invGap
+  + (+7.5608402383e-07) * currR [A]^3 * rollDeg [deg]^2
+  + (-2.1841873013e-05) * currR [A]^3 * rollDeg [deg] * invGap
+  + (+3.1131797033e-03) * currR [A]^3 * invGap^2
+  + (+2.9680840171e-05) * currR [A]^2 * rollDeg [deg]^3
+  + (-3.7292231655e-03) * currR [A]^2 * rollDeg [deg]^2 * invGap
+  + (-4.7717023731e-02) * currR [A]^2 * rollDeg [deg] * invGap^2
+  + (-1.4923564858e+02) * currR [A]^2 * invGap^3
+  + (+6.6540367362e-04) * currR [A] * rollDeg [deg]^4
+  + (-9.6378069105e-02) * currR [A] * rollDeg [deg]^3 * invGap
+  + (+2.3836959223e+00) * currR [A] * rollDeg [deg]^2 * invGap^2
+  + (-1.2664937015e+02) * currR [A] * rollDeg [deg] * invGap^3
+  + (+5.2627468646e+01) * currR [A] * invGap^4
+  + (+1.4646059365e-10) * rollDeg [deg]^5
+  + (-1.0417153181e+00) * rollDeg [deg]^4 * invGap
+  + (+1.1431957446e-06) * rollDeg [deg]^3 * invGap^2
+  + (+9.2056028147e+02) * rollDeg [deg]^2 * invGap^3
+  + (-3.1228040188e-04) * rollDeg [deg] * invGap^4
+  + (-5.9623186232e+02) * invGap^5
+  + (+2.9763214116e-09) * currL [A]^6
+  + (-6.2261733547e-09) * currL [A]^5 * currR [A]
+  + (-3.7175269085e-08) * currL [A]^5 * rollDeg [deg]
+  + (-3.1125615720e-06) * currL [A]^5 * invGap
+  + (-9.9876729109e-09) * currL [A]^4 * currR [A]^2
+  + (-1.3294041423e-08) * currL [A]^4 * currR [A] * rollDeg [deg]
+  + (+2.2610334440e-06) * currL [A]^4 * currR [A] * invGap
+  + (+2.4101829865e-08) * currL [A]^4 * rollDeg [deg]^2
+  + (+2.5320289563e-06) * currL [A]^4 * rollDeg [deg] * invGap
+  + (+2.0532678993e-04) * currL [A]^4 * invGap^2
+  + (-7.0411871889e-09) * currL [A]^3 * currR [A]^3
+  + (+1.1956544199e-08) * currL [A]^3 * currR [A]^2 * rollDeg [deg]
+  + (+1.9188820701e-06) * currL [A]^3 * currR [A]^2 * invGap
+  + (+6.3643952330e-08) * currL [A]^3 * currR [A] * rollDeg [deg]^2
+  + (-3.3660324359e-06) * currL [A]^3 * currR [A] * rollDeg [deg] * invGap
+  + (+1.0594658745e-05) * currL [A]^3 * currR [A] * invGap^2
+  + (-2.5937761805e-07) * currL [A]^3 * rollDeg [deg]^3
+  + (-7.2227672945e-06) * currL [A]^3 * rollDeg [deg]^2 * invGap
+  + (+9.6876113423e-05) * currL [A]^3 * rollDeg [deg] * invGap^2
+  + (+8.5422144858e-03) * currL [A]^3 * invGap^3
+  + (-1.0519386251e-08) * currL [A]^2 * currR [A]^4
+  + (-1.1957411061e-08) * currL [A]^2 * currR [A]^3 * rollDeg [deg]
+  + (+2.1222821118e-06) * currL [A]^2 * currR [A]^3 * invGap
+  + (+4.4935102750e-08) * currL [A]^2 * currR [A]^2 * rollDeg [deg]^2
+  + (-1.2807847845e-04) * currL [A]^2 * currR [A]^2 * invGap^2
+  + (+5.7679319987e-08) * currL [A]^2 * currR [A] * rollDeg [deg]^3
+  + (-1.4777475671e-05) * currL [A]^2 * currR [A] * rollDeg [deg]^2 * invGap
+  + (-2.5449360510e-04) * currL [A]^2 * currR [A] * rollDeg [deg] * invGap^2
+  + (-1.2272562713e-02) * currL [A]^2 * currR [A] * invGap^3
+  + (+8.8287809685e-07) * currL [A]^2 * rollDeg [deg]^4
+  + (+4.4586018461e-04) * currL [A]^2 * rollDeg [deg]^3 * invGap
+  + (+3.8576999819e-02) * currL [A]^2 * rollDeg [deg]^2 * invGap^2
+  + (+3.4044411238e-01) * currL [A]^2 * rollDeg [deg] * invGap^3
+  + (+3.5950095140e+02) * currL [A]^2 * invGap^4
+  + (-8.4967268776e-09) * currL [A] * currR [A]^5
+  + (+1.3295050394e-08) * currL [A] * currR [A]^4 * rollDeg [deg]
+  + (+1.7032328259e-06) * currL [A] * currR [A]^4 * invGap
+  + (+7.0043846279e-08) * currL [A] * currR [A]^3 * rollDeg [deg]^2
+  + (+3.3657944138e-06) * currL [A] * currR [A]^3 * rollDeg [deg] * invGap
+  + (+6.8900834879e-05) * currL [A] * currR [A]^3 * invGap^2
+  + (-5.7674824028e-08) * currL [A] * currR [A]^2 * rollDeg [deg]^3
+  + (-1.4623729019e-05) * currL [A] * currR [A]^2 * rollDeg [deg]^2 * invGap
+  + (+2.5450219980e-04) * currL [A] * currR [A]^2 * rollDeg [deg] * invGap^2
+  + (-1.0593500843e-03) * currL [A] * currR [A]^2 * invGap^3
+  + (+1.0484950490e-06) * currL [A] * currR [A] * rollDeg [deg]^4
+  + (-3.0932389983e-10) * currL [A] * currR [A] * rollDeg [deg]^3 * invGap
+  + (+5.6370243409e-03) * currL [A] * currR [A] * rollDeg [deg]^2 * invGap^2
+  + (-6.1682354633e-07) * currL [A] * currR [A] * rollDeg [deg] * invGap^3
+  + (+2.2923219836e+01) * currL [A] * currR [A] * invGap^4
+  + (-2.3420164800e-05) * currL [A] * rollDeg [deg]^5
+  + (-9.2739975478e-03) * currL [A] * rollDeg [deg]^4 * invGap
+  + (-7.4080422659e-01) * currL [A] * rollDeg [deg]^3 * invGap^2
+  + (-2.1756879968e+01) * currL [A] * rollDeg [deg]^2 * invGap^3
+  + (-3.5929041347e+02) * currL [A] * rollDeg [deg] * invGap^4
+  + (+1.3909390562e+01) * currL [A] * invGap^5
+  + (+6.1617129177e-09) * currR [A]^6
+  + (+3.7175730938e-08) * currR [A]^5 * rollDeg [deg]
+  + (-2.1826492898e-06) * currR [A]^5 * invGap
+  + (+3.0887363778e-08) * currR [A]^4 * rollDeg [deg]^2
+  + (-2.5321109476e-06) * currR [A]^4 * rollDeg [deg] * invGap
+  + (+2.0057675659e-04) * currR [A]^4 * invGap^2
+  + (+2.5936417813e-07) * currR [A]^3 * rollDeg [deg]^3
+  + (-7.9156716870e-06) * currR [A]^3 * rollDeg [deg]^2 * invGap
+  + (-9.6870862064e-05) * currR [A]^3 * rollDeg [deg] * invGap^2
+  + (-1.1576514633e-02) * currR [A]^3 * invGap^3
+  + (+1.0210733379e-06) * currR [A]^2 * rollDeg [deg]^4
+  + (-4.4586066717e-04) * currR [A]^2 * rollDeg [deg]^3 * invGap
+  + (+3.8287512141e-02) * currR [A]^2 * rollDeg [deg]^2 * invGap^2
+  + (-3.4044501906e-01) * currR [A]^2 * rollDeg [deg] * invGap^3
+  + (+3.6814992351e+02) * currR [A]^2 * invGap^4
+  + (+2.3420159192e-05) * currR [A] * rollDeg [deg]^5
+  + (-9.2337309491e-03) * currR [A] * rollDeg [deg]^4 * invGap
+  + (+7.4080431520e-01) * currR [A] * rollDeg [deg]^3 * invGap^2
+  + (-2.1520950367e+01) * currR [A] * rollDeg [deg]^2 * invGap^3
+  + (+3.5929051935e+02) * currR [A] * rollDeg [deg] * invGap^4
+  + (+1.4109361400e+01) * currR [A] * invGap^5
+  + (+1.7232857807e-04) * rollDeg [deg]^6
+  + (-1.5228368774e-09) * rollDeg [deg]^5 * invGap
+  + (+7.4229949571e+00) * rollDeg [deg]^4 * invGap^2
+  + (-4.7116044996e-06) * rollDeg [deg]^3 * invGap^3
+  + (+4.0058823232e+02) * rollDeg [deg]^2 * invGap^4
+  + (+7.4032294285e-04) * rollDeg [deg] * invGap^5
+  + (-1.3151563792e+02) * invGap^6
+
+================================================================================
+TORQUE [mN*m] = 
+    -2.5609643910e+00
+  + (+1.2736755974e+01) * currL [A]
+  + (-1.3082527996e+01) * currR [A]
+  + (-1.3305641735e+01) * rollDeg [deg]
+  + (+4.4151637366e+01) * invGap
+  + (-6.3923649292e-02) * currL [A]^2
+  + (-1.1560951475e-02) * currL [A] * currR [A]
+  + (+1.8386370689e+00) * currL [A] * rollDeg [deg]
+  + (-4.5922645259e+02) * currL [A] * invGap
+  + (+9.8233200925e-02) * currR [A]^2
+  + (+1.8386371393e+00) * currR [A] * rollDeg [deg]
+  + (+4.7236896866e+02) * currR [A] * invGap
+  + (+9.1214755261e-01) * rollDeg [deg]^2
+  + (-1.0749540976e+03) * rollDeg [deg] * invGap
+  + (-1.7435331556e+02) * invGap^2
+  + (+1.3682740756e-03) * currL [A]^3
+  + (+1.9533070101e-03) * currL [A]^2 * currR [A]
+  + (+1.9657240805e-02) * currL [A]^2 * rollDeg [deg]
+  + (+2.0525258396e+00) * currL [A]^2 * invGap
+  + (-2.0087168696e-03) * currL [A] * currR [A]^2
+  + (-7.7704180912e-03) * currL [A] * currR [A] * rollDeg [deg]
+  + (+4.6783332660e-01) * currL [A] * currR [A] * invGap
+  + (-1.5697787523e-01) * currL [A] * rollDeg [deg]^2
+  + (-2.5279026405e+01) * currL [A] * rollDeg [deg] * invGap
+  + (-2.6796288784e+03) * currL [A] * invGap^2
+  + (-6.9397567186e-04) * currR [A]^3
+  + (+1.9657238531e-02) * currR [A]^2 * rollDeg [deg]
+  + (-3.1136966908e+00) * currR [A]^2 * invGap
+  + (+1.8298169417e-01) * currR [A] * rollDeg [deg]^2
+  + (-2.5279027176e+01) * currR [A] * rollDeg [deg] * invGap
+  + (+2.5386184669e+03) * currR [A] * invGap^2
+  + (+6.2974733858e-01) * rollDeg [deg]^3
+  + (-1.3210284466e+01) * rollDeg [deg]^2 * invGap
+  + (+4.7007171607e+04) * rollDeg [deg] * invGap^2
+  + (-3.3555302551e+02) * invGap^3
+  + (-1.4305035722e-04) * currL [A]^4
+  + (-1.6602474226e-05) * currL [A]^3 * currR [A]
+  + (-1.2886823540e-04) * currL [A]^3 * rollDeg [deg]
+  + (+6.0143039665e-03) * currL [A]^3 * invGap
+  + (+1.2951511025e-05) * currL [A]^2 * currR [A]^2
+  + (-5.1776437670e-04) * currL [A]^2 * currR [A] * rollDeg [deg]
+  + (-3.8569371878e-02) * currL [A]^2 * currR [A] * invGap
+  + (+5.2166822420e-03) * currL [A]^2 * rollDeg [deg]^2
+  + (-6.2117182881e-01) * currL [A]^2 * rollDeg [deg] * invGap
+  + (+9.0620919904e+01) * currL [A]^2 * invGap^2
+  + (+1.3270592660e-06) * currL [A] * currR [A]^3
+  + (-5.1776417343e-04) * currL [A] * currR [A]^2 * rollDeg [deg]
+  + (+4.2265553856e-02) * currL [A] * currR [A]^2 * invGap
+  + (-2.2041148798e-04) * currL [A] * currR [A] * rollDeg [deg]^2
+  + (+2.4509139646e-01) * currL [A] * currR [A] * rollDeg [deg] * invGap
+  + (-4.9160159525e+00) * currL [A] * currR [A] * invGap^2
+  + (-1.7295894521e-01) * currL [A] * rollDeg [deg]^3
+  + (-6.7494817486e+00) * currL [A] * rollDeg [deg]^2 * invGap
+  + (-8.6427297569e+02) * currL [A] * rollDeg [deg] * invGap^2
+  + (+8.5965909925e+03) * currL [A] * invGap^3
+  + (-1.6550913550e-05) * currR [A]^4
+  + (-1.2886821877e-04) * currR [A]^3 * rollDeg [deg]
+  + (-3.0326211668e-02) * currR [A]^3 * invGap
+  + (-5.1175783571e-03) * currR [A]^2 * rollDeg [deg]^2
+  + (-6.2117181767e-01) * currR [A]^2 * rollDeg [deg] * invGap
+  + (-7.5004948389e+01) * currR [A]^2 * invGap^2
+  + (-1.7295894543e-01) * currR [A] * rollDeg [deg]^3
+  + (+5.7289182157e+00) * currR [A] * rollDeg [deg]^2 * invGap
+  + (-8.6427297453e+02) * currR [A] * rollDeg [deg] * invGap^2
+  + (-8.1554262787e+03) * currR [A] * invGap^3
+  + (-8.0890042154e-02) * rollDeg [deg]^4
+  + (+1.7462417614e+02) * rollDeg [deg]^3 * invGap
+  + (+5.9548102453e+01) * rollDeg [deg]^2 * invGap^2
+  + (-9.1016550502e+04) * rollDeg [deg] * invGap^3
+  + (-1.2462037728e+02) * invGap^4
+  + (-1.2219304153e-05) * currL [A]^5
+  + (-6.3133366268e-06) * currL [A]^4 * currR [A]
+  + (+5.1514166444e-07) * currL [A]^4 * rollDeg [deg]
+  + (+1.1879557083e-03) * currL [A]^4 * invGap
+  + (-1.0954778098e-06) * currL [A]^3 * currR [A]^2
+  + (-5.8420851161e-06) * currL [A]^3 * currR [A] * rollDeg [deg]
+  + (+2.5831185234e-03) * currL [A]^3 * currR [A] * invGap
+  + (+7.9254089698e-05) * currL [A]^3 * rollDeg [deg]^2
+  + (-1.7220995748e-03) * currL [A]^3 * rollDeg [deg] * invGap
+  + (-4.0028155701e-01) * currL [A]^3 * invGap^2
+  + (+1.7079834365e-06) * currL [A]^2 * currR [A]^3
+  + (-7.8935650691e-06) * currL [A]^2 * currR [A]^2 * rollDeg [deg]
+  + (-3.2330488590e-04) * currL [A]^2 * currR [A]^2 * invGap
+  + (+9.9753661971e-05) * currL [A]^2 * currR [A] * rollDeg [deg]^2
+  + (+7.2169993632e-03) * currL [A]^2 * currR [A] * rollDeg [deg] * invGap
+  + (+5.4494940800e-01) * currL [A]^2 * currR [A] * invGap^2
+  + (-1.5381149512e-03) * currL [A]^2 * rollDeg [deg]^3
+  + (-1.9907607357e-01) * currL [A]^2 * rollDeg [deg]^2 * invGap
+  + (+1.0517964842e+01) * currL [A]^2 * rollDeg [deg] * invGap^2
+  + (-5.9716223372e+02) * currL [A]^2 * invGap^3
+  + (+5.7682329953e-06) * currL [A] * currR [A]^4
+  + (-5.8420830129e-06) * currL [A] * currR [A]^3 * rollDeg [deg]
+  + (-3.1387669824e-03) * currL [A] * currR [A]^3 * invGap
+  + (-9.2627696858e-05) * currL [A] * currR [A]^2 * rollDeg [deg]^2
+  + (+7.2169983158e-03) * currL [A] * currR [A]^2 * rollDeg [deg] * invGap
+  + (-5.4112374537e-01) * currL [A] * currR [A]^2 * invGap^2
+  + (-3.1504005406e-05) * currL [A] * currR [A] * rollDeg [deg]^3
+  + (-1.7005015703e-04) * currL [A] * currR [A] * rollDeg [deg]^2 * invGap
+  + (-2.3976914010e+00) * currL [A] * currR [A] * rollDeg [deg] * invGap^2
+  + (+2.0162876449e+01) * currL [A] * currR [A] * invGap^3
+  + (+3.0541236561e-02) * currL [A] * rollDeg [deg]^4
+  + (+6.0909890705e+00) * currL [A] * rollDeg [deg]^3 * invGap
+  + (+1.8686476537e+02) * currL [A] * rollDeg [deg]^2 * invGap^2
+  + (+5.4569961037e+03) * currL [A] * rollDeg [deg] * invGap^3
+  + (+3.6521334233e+03) * currL [A] * invGap^4
+  + (+1.2356741024e-05) * currR [A]^5
+  + (+5.1514541610e-07) * currR [A]^4 * rollDeg [deg]
+  + (-3.2093123475e-04) * currR [A]^4 * invGap
+  + (-9.5040930219e-05) * currR [A]^3 * rollDeg [deg]^2
+  + (-1.7221002130e-03) * currR [A]^3 * rollDeg [deg] * invGap
+  + (+6.5615872016e-01) * currR [A]^3 * invGap^2
+  + (-1.5381149487e-03) * currR [A]^2 * rollDeg [deg]^3
+  + (+2.0395948760e-01) * currR [A]^2 * rollDeg [deg]^2 * invGap
+  + (+1.0517964822e+01) * currR [A]^2 * rollDeg [deg] * invGap^2
+  + (+4.9837046412e+02) * currR [A]^2 * invGap^3
+  + (-3.0694304201e-02) * currR [A] * rollDeg [deg]^4
+  + (+6.0909890702e+00) * currR [A] * rollDeg [deg]^3 * invGap
+  + (-1.7712281328e+02) * currR [A] * rollDeg [deg]^2 * invGap^2
+  + (+5.4569961045e+03) * currR [A] * rollDeg [deg] * invGap^3
+  + (-3.4643372364e+03) * currR [A] * invGap^4
+  + (-3.1967283950e-01) * rollDeg [deg]^5
+  + (+7.2465373581e-01) * rollDeg [deg]^4 * invGap
+  + (-3.7694440105e+03) * rollDeg [deg]^3 * invGap^2
+  + (+4.4429019352e+01) * rollDeg [deg]^2 * invGap^3
+  + (-3.9730669147e+04) * rollDeg [deg] * invGap^4
+  + (-3.1813627757e+01) * invGap^5
+  + (+2.0551811986e-07) * currL [A]^6
+  + (-2.2810121436e-07) * currL [A]^5 * currR [A]
+  + (-1.3383146324e-07) * currL [A]^5 * rollDeg [deg]
+  + (+1.6049756900e-04) * currL [A]^5 * invGap
+  + (-5.6453359321e-07) * currL [A]^4 * currR [A]^2
+  + (+2.6031671041e-07) * currL [A]^4 * currR [A] * rollDeg [deg]
+  + (+1.2744387021e-05) * currL [A]^4 * currR [A] * invGap
+  + (+2.8978174669e-06) * currL [A]^4 * rollDeg [deg]^2
+  + (-9.3438872511e-05) * currL [A]^4 * rollDeg [deg] * invGap
+  + (-3.2491823205e-03) * currL [A]^4 * invGap^2
+  + (+3.7946676912e-09) * currL [A]^3 * currR [A]^3
+  + (+1.7599444391e-07) * currL [A]^3 * currR [A]^2 * rollDeg [deg]
+  + (+2.4965108992e-05) * currL [A]^3 * currR [A]^2 * invGap
+  + (+1.1286476678e-06) * currL [A]^3 * currR [A] * rollDeg [deg]^2
+  + (+4.5300600434e-05) * currL [A]^3 * currR [A] * rollDeg [deg] * invGap
+  + (-1.5200582693e-02) * currL [A]^3 * currR [A] * invGap^2
+  + (+2.7490628440e-06) * currL [A]^3 * rollDeg [deg]^3
+  + (-8.2493098857e-04) * currL [A]^3 * rollDeg [deg]^2 * invGap
+  + (+2.3944496913e-02) * currL [A]^3 * rollDeg [deg] * invGap^2
+  + (+1.2946953968e+00) * currL [A]^3 * invGap^3
+  + (+5.9211265580e-07) * currL [A]^2 * currR [A]^4
+  + (+1.7599464286e-07) * currL [A]^2 * currR [A]^3 * rollDeg [deg]
+  + (-3.7773169680e-05) * currL [A]^2 * currR [A]^3 * invGap
+  + (-7.7324017411e-08) * currL [A]^2 * currR [A]^2 * rollDeg [deg]^2
+  + (+1.6224376321e-04) * currL [A]^2 * currR [A]^2 * rollDeg [deg] * invGap
+  + (+1.1125393841e-03) * currL [A]^2 * currR [A]^2 * invGap^2
+  + (-1.8213825115e-08) * currL [A]^2 * currR [A] * rollDeg [deg]^3
+  + (-1.1582951117e-03) * currL [A]^2 * currR [A] * rollDeg [deg]^2 * invGap
+  + (-2.7423615740e-02) * currL [A]^2 * currR [A] * rollDeg [deg] * invGap^2
+  + (-1.8375761343e+00) * currL [A]^2 * currR [A] * invGap^3
+  + (+1.8148082155e-05) * currL [A]^2 * rollDeg [deg]^4
+  + (+2.5582361106e-02) * currL [A]^2 * rollDeg [deg]^3 * invGap
+  + (+1.8719567169e+00) * currL [A]^2 * rollDeg [deg]^2 * invGap^2
+  + (-7.9375540256e+00) * currL [A]^2 * rollDeg [deg] * invGap^3
+  + (+1.3851610060e+03) * currL [A]^2 * invGap^4
+  + (+3.6572413364e-07) * currL [A] * currR [A]^5
+  + (+2.6031864309e-07) * currL [A] * currR [A]^4 * rollDeg [deg]
+  + (-1.3740560576e-05) * currL [A] * currR [A]^4 * invGap
+  + (-1.1634704791e-06) * currL [A] * currR [A]^3 * rollDeg [deg]^2
+  + (+4.5300591111e-05) * currL [A] * currR [A]^3 * rollDeg [deg] * invGap
+  + (+1.7827349035e-02) * currL [A] * currR [A]^3 * invGap^2
+  + (-1.8220930542e-08) * currL [A] * currR [A]^2 * rollDeg [deg]^3
+  + (+1.0311063391e-03) * currL [A] * currR [A]^2 * rollDeg [deg]^2 * invGap
+  + (-2.7423615867e-02) * currL [A] * currR [A]^2 * rollDeg [deg] * invGap^2
+  + (+1.8202040056e+00) * currL [A] * currR [A]^2 * invGap^3
+  + (+1.4693588980e-05) * currL [A] * currR [A] * rollDeg [deg]^4
+  + (+2.0391970869e-03) * currL [A] * currR [A] * rollDeg [deg]^3 * invGap
+  + (-5.1289752136e-03) * currL [A] * currR [A] * rollDeg [deg]^2 * invGap^2
+  + (+1.3585046026e+01) * currL [A] * currR [A] * rollDeg [deg] * invGap^3
+  + (-2.5312909994e+01) * currL [A] * currR [A] * invGap^4
+  + (-1.2892918352e-03) * currL [A] * rollDeg [deg]^5
+  + (-4.2744856185e-01) * currL [A] * rollDeg [deg]^4 * invGap
+  + (-4.5230140146e+01) * currL [A] * rollDeg [deg]^3 * invGap^2
+  + (-1.2691925261e+03) * currL [A] * rollDeg [deg]^2 * invGap^3
+  + (-1.7946912044e+04) * currL [A] * rollDeg [deg] * invGap^4
+  + (+9.8200368161e+02) * currL [A] * invGap^5
+  + (+1.4402604620e-07) * currR [A]^6
+  + (-1.3383078112e-07) * currR [A]^5 * rollDeg [deg]
+  + (-1.5877721700e-04) * currR [A]^5 * invGap
+  + (-1.8463799734e-06) * currR [A]^4 * rollDeg [deg]^2
+  + (-9.3438911534e-05) * currR [A]^4 * rollDeg [deg] * invGap
+  + (-2.0062450751e-03) * currR [A]^4 * invGap^2
+  + (+2.7490636967e-06) * currR [A]^3 * rollDeg [deg]^3
+  + (+1.0293575893e-03) * currR [A]^3 * rollDeg [deg]^2 * invGap
+  + (+2.3944496992e-02) * currR [A]^3 * rollDeg [deg] * invGap^2
+  + (-2.1505766179e+00) * currR [A]^3 * invGap^3
+  + (-4.8050087258e-05) * currR [A]^2 * rollDeg [deg]^4
+  + (+2.5582361094e-02) * currR [A]^2 * rollDeg [deg]^3 * invGap
+  + (-1.8925775930e+00) * currR [A]^2 * rollDeg [deg]^2 * invGap^2
+  + (-7.9375540232e+00) * currR [A]^2 * rollDeg [deg] * invGap^3
+  + (-1.1457282555e+03) * currR [A]^2 * invGap^4
+  + (-1.2892918417e-03) * currR [A] * rollDeg [deg]^5
+  + (+4.3570132351e-01) * currR [A] * rollDeg [deg]^4 * invGap
+  + (-4.5230140146e+01) * currR [A] * rollDeg [deg]^3 * invGap^2
+  + (+1.2356182202e+03) * currR [A] * rollDeg [deg]^2 * invGap^3
+  + (-1.7946912045e+04) * currR [A] * rollDeg [deg] * invGap^4
+  + (-9.3157391357e+02) * currR [A] * invGap^5
+  + (+1.9887971069e-03) * rollDeg [deg]^6
+  + (+4.1532416818e+00) * rollDeg [deg]^5 * invGap
+  + (-3.7128109697e+00) * rollDeg [deg]^4 * invGap^2
+  + (+1.9150570779e+04) * rollDeg [deg]^3 * invGap^3
+  + (+1.9562107621e+01) * rollDeg [deg]^2 * invGap^4
+  + (-1.0765823661e+04) * rollDeg [deg] * invGap^5
+  + (-6.8540115783e+00) * invGap^6

lev_sim/temp.json

@@ -0,0 +1,11 @@
+{
+  "height_kp": 80.05607483893696,
+  "height_ki": 0,
+  "height_kd": 7.09266287860531,
+  "roll_kp": -0.600856607986966,
+  "roll_ki": 0,
+  "roll_kd": -0.1,
+  "pitch_kp": 50.3415835489009009,
+  "pitch_ki": 0.02319184022898008,
+  "pitch_kd": 0.017632648760979346
+}

lib/Controller.cpp

@@ -1,164 +0,0 @@
-#include "Controller.hpp"
-#include <Arduino.h>
-
-// CONTROLLER CONSTANTS
-float MAX_INTEGRAL_TERM = 1e4;
-
-void FullController::update() {
-  Left.readMM();
-  Right.readMM();
-  Front.readMM();
-  Back.readMM(); // read and update dists/oor for all sensors.
-
-  oor = Left.oor || Right.oor || Front.oor || Back.oor;
-
-  avgControl();
-  LRControl(); // run pwm functions.
-  FBControl();
-
-  FLPWM = constrain(avgPWM + LDiffPWM + FDiffPWM, -CAP, CAP);
-  BLPWM = constrain(avgPWM + LDiffPWM + BDiffPWM, -CAP, CAP);
-  FRPWM = constrain(avgPWM + RDiffPWM + FDiffPWM, -CAP, CAP);
-  BRPWM = constrain(avgPWM + RDiffPWM + BDiffPWM, -CAP, CAP);
-
-  // FLPWM = avgPWM;
-  // BLPWM = avgPWM;
-  // FRPWM = avgPWM;
-  // BRPWM = avgPWM;
-}
-
-void FullController::zeroPWMs() {
-  FLPWM = 0;
-  BLPWM = 0;
-  FRPWM = 0;
-  BRPWM = 0;
-}
-
-void FullController::sendOutputs() {
-  if (!outputOn) {
-    zeroPWMs();
-  }
-
-  // The following assumes 0 direction drives repulsion and 1 direction drives
-  // attraction. Using direct register writes to maintain fast PWM mode set by
-  // setupFastPWM()
-  digitalWrite(dirFL, FLPWM < 0);
-  OCR2A = abs(FLPWM); // Pin 11 -> Timer 2A
-  digitalWrite(dirBL, BLPWM < 0);
-  OCR1A = abs(BLPWM); // Pin 9 -> Timer 1A
-  digitalWrite(dirFR, FRPWM < 0);
-  OCR2B = abs(FRPWM); // Pin 3 -> Timer 2B
-  digitalWrite(dirBR, BRPWM < 0);
-  OCR1B = abs(BRPWM); // Pin 10 -> Timer 1B
-}
-
-void FullController::avgControl() {
-  avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f;
-  float eCurr = AvgRef - avg;
-
-  avgError.eDiff = eCurr - avgError.e;
-  if (!oor) {
-    avgError.eInt += eCurr;
-    avgError.eInt =
-        constrain(avgError.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
-  }
-  avgError.e = eCurr;
-
-  avgPWM = pwmFunc(avgConsts, avgError);
-}
-
-void FullController::LRControl() {
-  float diff = Right.mmVal - Left.mmVal; // how far above the right is the left?
-  float eCurr = diff - LRDiffRef; // how different is that from the reference?
-                                  // positive -> Left repels, Right attracts.
-  K_MAP rConsts = {
-      LConsts.attracting,
-      LConsts.repelling}; // apply attracting to repelling and vice versa.
-
-  LRDiffErr.eDiff = eCurr - LRDiffErr.e;
-
-  if (!oor) {
-    LRDiffErr.eInt += eCurr;
-    LRDiffErr.eInt =
-        constrain(LRDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
-  }
-
-  LRDiffErr.e = eCurr;
-
-  LDiffPWM = pwmFunc(LConsts, LRDiffErr);
-  RDiffPWM = -pwmFunc(rConsts, LRDiffErr);
-}
-
-void FullController::FBControl() {
-  float diff = Back.mmVal - Front.mmVal; // how far above the back is the front?
-  float eCurr = diff - FBDiffRef; // how different is that from ref? pos.->Front
-                                  // must repel, Back must attract
-  K_MAP bConsts = {FConsts.attracting, FConsts.repelling};
-
-  FBDiffErr.eDiff = eCurr - FBDiffErr.e;
-
-  if (!oor) {
-    FBDiffErr.eInt += eCurr;
-    FBDiffErr.eInt =
-        constrain(FBDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM);
-  }
-
-  FBDiffErr.e = eCurr;
-
-  FDiffPWM = pwmFunc(FConsts, FBDiffErr);
-  BDiffPWM = -pwmFunc(bConsts, FBDiffErr);
-}
-
-int16_t FullController::pwmFunc(K_MAP consts, Errors errs) {
-  if (oor)
-    return 0;
-  Constants constants = (errs.e < 0) ? consts.attracting : consts.repelling;
-  return (int)(constants.kp * errs.e + constants.ki * errs.eInt +
-                            constants.kd * errs.eDiff);
-}
-
-void FullController::report() {
-  // CSV Format: Left,Right,Front,Back,Avg,FLPWM,BLPWM,FRPWM,BRPWM,ControlOn
-  Serial.print(Left.mmVal);
-  Serial.print(",");
-  Serial.print(Right.mmVal);
-  Serial.print(",");
-  Serial.print(Front.mmVal);
-  Serial.print(",");
-  Serial.print(Back.mmVal);
-  Serial.print(",");
-  Serial.print(avg);
-  Serial.print(",");
-
-  Serial.print(FLPWM);
-  Serial.print(",");
-  Serial.print(BLPWM);
-  Serial.print(",");
-  Serial.print(FRPWM);
-  Serial.print(",");
-  Serial.print(BRPWM);
-  Serial.print(",");
-
-  Serial.println(outputOn);
-}
-
-void FullController::updateAvgPID(Constants repel, Constants attract) {
-  avgConsts.repelling = repel;
-  avgConsts.attracting = attract;
-}
-
-void FullController::updateLRPID(Constants down, Constants up) {
-  LConsts.repelling = down;
-  LConsts.attracting = up;
-}
-
-void FullController::updateFBPID(Constants down, Constants up) {
-  FConsts.repelling = down;
-  FConsts.attracting = up;
-}
-
-void FullController::updateReferences(float avgReference, float lrDiffReference, float fbDiffReference) {
-  AvgRef = avgReference;
-  LRDiffRef = lrDiffReference;
-  FBDiffRef = fbDiffReference;
-}

lib/Controller.hpp

@@ -1,104 +0,0 @@
-#ifndef CONTROLLER_HPP
-#define CONTROLLER_HPP
-
-#include <stdint.h>
-#include "IndSensorMap.hpp"
-
-// PIN MAPPING
-#define dirFR 2
-#define pwmFR 3
-#define dirBR 4
-#define pwmBR 10
-#define pwmFL 11
-#define dirFL 7
-#define dirBL 8
-#define pwmBL 9
-
-#define CAP 250
-
-typedef struct Constants {
-  float kp;
-  float ki;
-  float kd;
-} Constants;
-
-typedef struct K_MAP {
-  Constants repelling;
-  Constants attracting;
-} K_MAP;
-
-typedef struct FullConsts {
-  K_MAP avg;
-  K_MAP lColl; // repelling is applied to attracting and vice versa for the Right and Back collectives.
-  K_MAP fColl;
-} FullConsts;
-
-typedef struct Errors {
-  float e;
-  float eDiff;
-  float eInt;
-} Errors;
-
-class FullController {
-  public:
-    bool oor;
-    bool outputOn;
-
-    FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
-      FullConsts fullConsts, float avgRef, float lrDiffRef, float fbDiffRef)
-      : Left(l), Right(r), Front(f), Back(b), AvgRef(avgRef), LRDiffRef(lrDiffRef),
-      FBDiffRef(fbDiffRef), avgConsts(fullConsts.avg), LConsts(fullConsts.lColl),
-      FConsts(fullConsts.fColl), avgError({0,0,0}), LRDiffErr({0,0,0}), 
-      FBDiffErr({0,0,0}), oor(false), outputOn(false) {}
-
-    void update();
-    void zeroPWMs();
-    void sendOutputs();
-    void report();
-    
-    // PID tuning methods
-    void updateAvgPID(Constants repel, Constants attract);
-    void updateLRPID(Constants down, Constants up);
-    void updateFBPID(Constants down, Constants up);
-    
-    // Reference update methods
-    void updateReferences(float avgReference, float lrDiffReference, float fbDiffReference);
-
-  private:
-    void avgControl();
-    void LRControl();
-    void FBControl();
-    int16_t pwmFunc(K_MAP consts, Errors errs);
-
-    IndSensor& Front;
-    IndSensor& Back;
-    IndSensor& Right;
-    IndSensor& Left;
-
-    K_MAP avgConsts;
-    K_MAP LConsts;
-    K_MAP FConsts;
-
-    Errors avgError;
-    Errors LRDiffErr;
-    Errors FBDiffErr;
-
-    float AvgRef;
-    float LRDiffRef;
-    float FBDiffRef;
-    float avg;
-
-    int16_t avgPWM;
-    int16_t LDiffPWM;
-    int16_t RDiffPWM;
-    int16_t FDiffPWM;
-    int16_t BDiffPWM;
-    // Initially, I was going to make the Right and Back just the negatives,
-    // but having separate control functions running for each of these outputs might prove useful.
-
-    int16_t FLPWM;
-    int16_t BLPWM;
-    int16_t FRPWM;
-    int16_t BRPWM;
-};
-#endif // CONTROLLER_HPP