/* * omnipen.ino — OmniPen Letter Recognition Firmware * =================================================== * Hardware: Seeed XIAO nRF52840 Sense + Adafruit LSM6DSOXTR (external IMU) * * What this sketch does * ───────────────────── * 1. At startup: calibrate gravity by averaging 50 still-pen readings. * 2. Wait for the pen to start moving (acceleration > START_THRESHOLD). * 3. Record up to 2 seconds of 6-axis IMU data at 50 Hz. * 4. Stop recording when the pen is still again. * 5. Reconstruct a 2-D pen-tip trajectory by double-integrating the * gravity-removed acceleration signal, then correct for drift.......................................................................................................................................................................................................................................................................................................................................................................................................................................................................................................... * 6. Render the trajectory as a 28×28 greyscale "ink" image. * 7. Feed that image into the TFLite CNN and print the top-3 letter * guesses with confidence percentages over Serial (115200 baud). * * Bridging IMU data → EMNIST images * ────────────────────────────────── * The CNN was trained on EMNIST — 28×28 pixel images of letters. The pen * produces IMU time-series data. To use the same CNN we reconstruct the * path the pen tip traced (by integrating acceleration), draw it on a * 28×28 canvas, and classify that canvas image. * * Required libraries (Sketch → Include Library → Manage Libraries) * ────────────────────────────────────────────────────────────────── * • Adafruit LSM6DS (includes LSM6DSOX support) * • Adafruit BusIO (dependency of LSM6DS) * • Adafruit Unified Sensor (dependency of LSM6DS) * (no ML library needed — inference is implemented in inference.h) * * Wiring (XIAO → LSM6DSOXTR breakout) * ───────────────────────────────────── * XIAO 3V3 → VIN * XIAO GND → GND * XIAO SDA → SDA (D4 on XIAO) * XIAO SCL → SCL (D5 on XIAO) * * Files in this folder * ───────────────────── * omnipen.ino ← this file * model_data.h ← generated by training/train_model.py — copy it here */ // ───────────────────────────────────────────────────────────────────────────── // Includes // ───────────────────────────────────────────────────────────────────────────── #include #include // inference.h implements the CNN forward pass in plain C++ (no ML library). // weights.h contains the float weights exported by train_model.py. // Both files must be in the same folder as this sketch. #include "inference.h" #include "model_data.h" // The trained model embedded as a byte array // ───────────────────────────────────────────────────────────────────────────── // Configuration // Adjust these values if recognition is unreliable on your hardware/style. // ───────────────────────────────────────────────────────────────────────────── // Sampling static constexpr int SAMPLE_RATE_HZ = 50; // IMU readings per second static constexpr int MAX_SAMPLES = 100; // 100 ÷ 50 Hz = 2-second window static constexpr float DT = 1.0f / SAMPLE_RATE_HZ; // seconds per sample // Motion thresholds (units: m/s² — gravity is ~9.8 m/s²) // If recording triggers accidentally from noise: raise START_THRESHOLD. // If recording cuts off before you finish a letter: lower STOP_THRESHOLD or // increase STILL_COUNT_NEEDED. static constexpr float START_THRESHOLD = 0.8f; // net accel to begin recording static constexpr float STOP_THRESHOLD = 0.4f; // net accel below this = "still" static constexpr int STILL_COUNT_NEEDED = 25; // 25 samples × 20 ms = 0.5 s still // Canvas static constexpr int IMG_SIZE = 28; // Must match the trained model input static constexpr int BRUSH_RADIUS = 1; // Pixel half-width of the "ink" brush // Set to 1 to print the 28×28 trajectory image as ASCII art after each // recognition. Useful for diagnosing trajectory-reconstruction issues. #define DEBUG_PRINT_IMAGE 0 // ───────────────────────────────────────────────────────────────────────────── // Letter labels // ───────────────────────────────────────────────────────────────────────────── static const char LETTERS[26] = { 'A','B','C','D','E','F','G','H','I','J','K','L','M', 'N','O','P','Q','R','S','T','U','V','W','X','Y','Z' }; // ───────────────────────────────────────────────────────────────────────────── // Global objects // ───────────────────────────────────────────────────────────────────────────── Adafruit_LSM6DSOX imu; // (No ML engine object needed — run_inference() in inference.h manages // its own static activation buffers internally.) // IMU sample ring — holds one writing gesture worth of raw readings. // 'struct' groups the six sensor axes into one tidy unit per timestamp. struct ImuSample { float ax, ay, az; // linear acceleration after gravity removal [m/s²] float gx, gy, gz; // angular velocity (not used in basic mode) [rad/s] }; static ImuSample samples[MAX_SAMPLES]; static int sample_count = 0; // Gravity vector measured during the still-calibration phase. // Made global so buildImage() can use it when projecting onto the writing plane. static float grav_x = 0.0f, grav_y = 0.0f, grav_z = 0.0f; // Trajectory image — 0.0 = background (white), 1.0 = ink (black) static float image[IMG_SIZE][IMG_SIZE]; // ───────────────────────────────────────────────────────────────────────────── // drawLine() // ───────────────────────────────────────────────────────────────────────────── /* * Draw a thick line from pixel (x0,y0) to (x1,y1) using Bresenham's * line algorithm with a square brush of BRUSH_RADIUS half-size. * * Why Bresenham? * ────────────── * Between two consecutive 50 Hz IMU samples the pen can move several pixels. * If we just paint single points, the trajectory looks like a dotted line * with gaps. Bresenham fills in all the pixels along the straight path * between consecutive points, giving a smooth, connected stroke — much closer * to what an EMNIST training image looks like. */ static void drawLine(float fx0, float fy0, float fx1, float fy1) { int x0 = (int)roundf(fx0), y0 = (int)roundf(fy0); int x1 = (int)roundf(fx1), y1 = (int)roundf(fy1); int dx = abs(x1 - x0), sx = (x0 < x1) ? 1 : -1; int dy = abs(y1 - y0), sy = (y0 < y1) ? 1 : -1; int err = dx - dy; while (true) { // Paint a (2*BRUSH_RADIUS+1) × (2*BRUSH_RADIUS+1) square at (x0,y0) for (int dr = -BRUSH_RADIUS; dr <= BRUSH_RADIUS; dr++) { for (int dc = -BRUSH_RADIUS; dc <= BRUSH_RADIUS; dc++) { int r = y0 + dr, c = x0 + dc; if (r >= 0 && r < IMG_SIZE && c >= 0 && c < IMG_SIZE) image[r][c] = 1.0f; } } if (x0 == x1 && y0 == y1) break; int e2 = 2 * err; if (e2 > -dy) { err -= dy; x0 += sx; } if (e2 < dx) { err += dx; y0 += sy; } } } // ───────────────────────────────────────────────────────────────────────────── // buildImage() // ───────────────────────────────────────────────────────────────────────────── /* * Convert the gravity-corrected IMU samples in `samples[]` into the 28×28 * float image in `image[][]`. * * The algorithm in plain English: * ───────────────────────────────── * Acceleration is the rate of change of velocity. * Velocity is the rate of change of position. * * So: position = ∫∫ acceleration dt dt (double integral) * * We compute this numerically with simple Euler integration. * The complication is sensor bias: even a tiny constant offset in the * accelerometer (say 0.01 m/s²) integrates into an ever-growing velocity * drift (~0.01 m/s after 1 s) and then position drift. Over a 2-second * letter stroke this can be several centimetres — bigger than the letter. * * Fix: we know the pen starts and ends at rest (zero velocity), so any * non-zero velocity at the end of the gesture is pure drift. We subtract * a linearly growing correction that brings the final velocity back to zero. * This "linear detrend" is simple, fast, and works well for gestures where * the pen returns near its starting position. * * Returns false if motion was too small to be a letter. */ static bool buildImage() { // Temporary arrays — declared static to avoid placing them on the stack. // (Stack size on nRF52840 is limited; these arrays are 100 * 4 = 400 bytes each.) static float vx[MAX_SAMPLES], vy[MAX_SAMPLES]; static float px[MAX_SAMPLES], py[MAX_SAMPLES]; // ── 0. Project 3D acceleration onto the writing plane ──────────────────── // Problem: the writing surface can be at any angle — a table, a whiteboard, // a tilted notebook. We must not assume that ax and ay are the writing axes. // // Solution: the gravity vector (measured during still calibration) always // points perpendicular to a horizontal surface — but more generally, it // points perpendicular to the plane that gravity is perpendicular to, which // is exactly the plane the pen tip moves in when writing on ANY flat surface. // (On a tilted surface the pen's in-plane motion is still perpendicular to // the gravity component along the surface normal, and projecting out the // full gravity direction gives us what we need.) // // We build two orthogonal basis vectors that together span the // gravity-perpendicular plane, then project each sample's 3-axis linear // acceleration onto those two axes to get the 2D (x, y) writing coordinates. // // The resulting x/y axes are consistent within a session but have an // arbitrary azimuth (compass heading), so a letter might be rendered rotated // relative to the canonical EMNIST orientation. The random-rotation // augmentation added to the training script makes the model tolerant of this. // Normalise the gravity vector to a unit vector ĝ = "direction of down" float g_len = sqrtf(grav_x*grav_x + grav_y*grav_y + grav_z*grav_z); if (g_len < 0.1f) g_len = 9.81f; // guard against a bad (near-zero) calibration float g0 = grav_x/g_len, g1 = grav_y/g_len, g2 = grav_z/g_len; // Pick a reference vector = whichever sensor axis is LEAST aligned with ĝ. // This avoids near-zero cross-products that cause numerical blow-up. float ref0, ref1, ref2; float ag0 = fabsf(g0), ag1 = fabsf(g1), ag2 = fabsf(g2); if (ag0 <= ag1 && ag0 <= ag2) { ref0=1; ref1=0; ref2=0; } else if (ag1 <= ag2) { ref0=0; ref1=1; ref2=0; } else { ref0=0; ref1=0; ref2=1; } // Gram-Schmidt: basis1 = normalise(ref − (ref·ĝ) ĝ) float dot_rg = ref0*g0 + ref1*g1 + ref2*g2; float b1_0 = ref0 - dot_rg*g0, b1_1 = ref1 - dot_rg*g1, b1_2 = ref2 - dot_rg*g2; float b1_len = sqrtf(b1_0*b1_0 + b1_1*b1_1 + b1_2*b1_2); b1_0 /= b1_len; b1_1 /= b1_len; b1_2 /= b1_len; // basis2 = ĝ × basis1 (cross product gives the third orthogonal axis) float b2_0 = g1*b1_2 - g2*b1_1; float b2_1 = g2*b1_0 - g0*b1_2; float b2_2 = g0*b1_1 - g1*b1_0; // Project every sample's 3-axis acceleration onto (basis1, basis2). // We overwrite ax/ay with the resulting 2D coordinates so the rest of // buildImage() can proceed without knowing anything about 3D geometry. for (int i = 0; i < sample_count; i++) { float a0 = samples[i].ax, a1 = samples[i].ay, a2 = samples[i].az; samples[i].ax = a0*b1_0 + a1*b1_1 + a2*b1_2; // x in writing plane samples[i].ay = a0*b2_0 + a1*b2_1 + a2*b2_2; // y in writing plane } // ── 1. Integrate acceleration → velocity ───────────────────────────────── // v[0] = 0 (pen was at rest at the start of recording) // v[i] = v[i-1] + a[i] * dt vx[0] = vy[0] = 0.0f; for (int i = 1; i < sample_count; i++) { vx[i] = vx[i - 1] + samples[i].ax * DT; vy[i] = vy[i - 1] + samples[i].ay * DT; } // ── 2. Linear drift correction ──────────────────────────────────────────── // At the end of the gesture the pen is still → true final velocity = 0. // vx[sample_count-1] is the accumulated drift; remove it linearly. // // Imagine the drift as a ramp that rises from 0 at the start to // vx_drift at the end. Subtracting that ramp straightens the velocity // trajectory without distorting the middle portion. float vx_drift = vx[sample_count - 1]; float vy_drift = vy[sample_count - 1]; for (int i = 0; i < sample_count; i++) { float t = (float)i / (float)(sample_count - 1); // 0.0 → 1.0 vx[i] -= vx_drift * t; vy[i] -= vy_drift * t; } // ── 3. Integrate velocity → position ───────────────────────────────────── px[0] = py[0] = 0.0f; for (int i = 1; i < sample_count; i++) { px[i] = px[i - 1] + vx[i] * DT; py[i] = py[i - 1] + vy[i] * DT; } // ── 4. Guard: reject gestures too small to be a letter ─────────────────── float min_x = px[0], max_x = px[0]; float min_y = py[0], max_y = py[0]; for (int i = 1; i < sample_count; i++) { if (px[i] < min_x) min_x = px[i]; if (px[i] > max_x) max_x = px[i]; if (py[i] < min_y) min_y = py[i]; if (py[i] > max_y) max_y = py[i]; } float range_x = max_x - min_x; float range_y = max_y - min_y; // Use the larger axis to preserve aspect ratio in the normalisation below. float range = (range_x > range_y) ? range_x : range_y; if (range < 1e-4f) { Serial.println("[WARN] Motion too small — write a bigger letter."); return false; } // ── 5. Map trajectory to [0, IMG_SIZE-1] canvas ────────────────────────── // Centre the trajectory on the canvas and scale it so the widest axis // fills the canvas width minus one brush-width border on each side. float cx = (min_x + max_x) * 0.5f; float cy = (min_y + max_y) * 0.5f; float margin = (float)(BRUSH_RADIUS + 1); float scale = (float)(IMG_SIZE - 1 - 2.0f * margin) / range; float origin = (float)(IMG_SIZE - 1) * 0.5f; // ── 6. Clear the canvas and paint the trajectory ────────────────────────── memset(image, 0, sizeof(image)); for (int i = 1; i < sample_count; i++) { float x0 = (px[i - 1] - cx) * scale + origin; float y0 = (py[i - 1] - cy) * scale + origin; float x1 = (px[i] - cx) * scale + origin; float y1 = (py[i] - cy) * scale + origin; drawLine(x0, y0, x1, y1); } return true; } // ───────────────────────────────────────────────────────────────────────────── // debugPrintImage() // ───────────────────────────────────────────────────────────────────────────── /* * Print the 28×28 canvas as ASCII art to the Serial Monitor. * '#' = ink pixel, '.' = background pixel. * * Enable by setting #define DEBUG_PRINT_IMAGE 1 near the top of this file. * This is very helpful when first setting up: if the ASCII shape looks like * the letter you wrote, the trajectory reconstruction is working correctly. * If it looks like noise or a scribble, check your IMU axis orientation and * gravity-subtraction step. */ static void debugPrintImage() { #if DEBUG_PRINT_IMAGE Serial.println("Trajectory (28x28):"); for (int row = 0; row < IMG_SIZE; row++) { for (int col = 0; col < IMG_SIZE; col++) { Serial.print(image[row][col] > 0.5f ? '#' : '.'); } Serial.println(); } #endif } // ───────────────────────────────────────────────────────────────────────────── // collectData() // ───────────────────────────────────────────────────────────────────────────── /* * Calibrate gravity, wait for motion, record IMU samples until the pen * is still again, and fill the global `samples[]` buffer. * * Returns true if enough samples were collected, false otherwise. * * Gravity calibration * ──────────────────── * The accelerometer always measures the sum of gravity + linear motion. * To get just the motion, we need to subtract gravity. * * While the pen is still, acceleration = gravity only. We collect 50 * samples and average them to get a stable gravity estimate. * Each time the sketch runs this calibration, so if you change the pen's * resting orientation between letters that is handled automatically. */ static bool collectData() { sensors_event_t acc_evt, gyro_evt, temp_evt; // ── Calibration: measure gravity ───────────────────────────────────────── Serial.println("Hold the pen still … (calibrating gravity)"); grav_x = 0; grav_y = 0; grav_z = 0; // reset globals before accumulating const int CALIB_N = 50; for (int i = 0; i < CALIB_N; i++) { imu.getEvent(&acc_evt, &gyro_evt, &temp_evt); grav_x += acc_evt.acceleration.x; grav_y += acc_evt.acceleration.y; grav_z += acc_evt.acceleration.z; delay(1000 / SAMPLE_RATE_HZ); // 20 ms at 50 Hz } grav_x /= CALIB_N; grav_y /= CALIB_N; grav_z /= CALIB_N; Serial.print("Gravity vector (x,y,z) m/s²: "); Serial.print(grav_x, 2); Serial.print(", "); Serial.print(grav_y, 2); Serial.print(", "); Serial.println(grav_z, 2); // ── Wait for motion to start ────────────────────────────────────────────── Serial.println("Ready — write a letter now."); while (true) { imu.getEvent(&acc_evt, &gyro_evt, &temp_evt); float lx = acc_evt.acceleration.x - grav_x; float ly = acc_evt.acceleration.y - grav_y; float mag = sqrtf(lx * lx + ly * ly); if (mag > START_THRESHOLD) { Serial.println("Recording …"); break; } delay(1000 / SAMPLE_RATE_HZ); } // ── Record samples until pen is still again ─────────────────────────────── sample_count = 0; int still_count = 0; while (sample_count < MAX_SAMPLES) { imu.getEvent(&acc_evt, &gyro_evt, &temp_evt); // Subtract the gravity estimate from the accelerometer reading. // What remains is purely the pen's linear acceleration. ImuSample s; s.ax = acc_evt.acceleration.x - grav_x; s.ay = acc_evt.acceleration.y - grav_y; s.az = acc_evt.acceleration.z - grav_z; s.gx = gyro_evt.gyro.x; s.gy = gyro_evt.gyro.y; s.gz = gyro_evt.gyro.z; samples[sample_count++] = s; // Check XY motion magnitude (the writing plane) float mag = sqrtf(s.ax * s.ax + s.ay * s.ay); if (mag < STOP_THRESHOLD) { still_count++; if (still_count >= STILL_COUNT_NEEDED) { Serial.println("Motion ended — processing …"); break; } } else { still_count = 0; // Reset counter whenever motion resumes } delay(1000 / SAMPLE_RATE_HZ); // Maintain ~50 Hz sample rate } if (sample_count < 10) { Serial.println("[WARN] Too few samples — try again."); return false; } Serial.print("Recorded "); Serial.print(sample_count); Serial.println(" samples."); return true; } // ───────────────────────────────────────────────────────────────────────────── // runInference() // ───────────────────────────────────────────────────────────────────────────── /* * Run the CNN forward pass on the current `image` canvas and print the * top-3 letter predictions with confidence percentages. */ static void runInference() { float probs[26]; run_inference(image, probs); // defined in inference.h // ── Find top-3 predictions ──────────────────────────────────────────────── int top_idx[3] = {0, 0, 0}; float top_prob[3] = {-1.0f, -1.0f, -1.0f}; for (int i = 0; i < 26; i++) { if (probs[i] > top_prob[0]) { top_prob[2] = top_prob[1]; top_idx[2] = top_idx[1]; top_prob[1] = top_prob[0]; top_idx[1] = top_idx[0]; top_prob[0] = probs[i]; top_idx[0] = i; } else if (probs[i] > top_prob[1]) { top_prob[2] = top_prob[1]; top_idx[2] = top_idx[1]; top_prob[1] = probs[i]; top_idx[1] = i; } else if (probs[i] > top_prob[2]) { top_prob[2] = probs[i]; top_idx[2] = i; } } // ── Print result ────────────────────────────────────────────────────────── Serial.println("──────────────────────────"); Serial.print("Recognised: "); Serial.print(LETTERS[top_idx[0]]); Serial.print(" ("); Serial.print(top_prob[0] * 100.0f, 1); Serial.println("%)"); Serial.println("Top 3:"); for (int k = 0; k < 3; k++) { Serial.print(" "); Serial.print(LETTERS[top_idx[k]]); Serial.print(" : "); Serial.print(top_prob[k] * 100.0f, 1); Serial.println("%"); } Serial.println("──────────────────────────\n"); } // ───────────────────────────────────────────────────────────────────────────── // setup() // ───────────────────────────────────────────────────────────────────────────── void setup() { Serial.begin(115200); // Wait up to 3 seconds for the Serial Monitor to connect (USB CDC). // Remove this if you don't want the delay after reset. while (!Serial && millis() < 3000); Serial.println("\n=== OmniPen Letter Recognition ===\n"); // ── Initialise IMU ──────────────────────────────────────────────────────── // begin_I2C() scans the default address (0x6A). // If your breakout has the SA0 pin tied HIGH, change to: // imu.begin_I2C(0x6B) if (!imu.begin_I2C()) { Serial.println("[FATAL] LSM6DSOX not found."); Serial.println(" Check wiring: SDA→D4, SCL→D5, VIN→3V3, GND→GND"); while (true); // Halt — nothing else can work without the IMU } // Set output data rate to 104 Hz. // The firmware samples at 50 Hz; setting 104 Hz ensures fresh data is // always available when we call getEvent(). imu.setAccelDataRate(LSM6DS_RATE_104_HZ); imu.setGyroDataRate(LSM6DS_RATE_104_HZ); // Optional: set accelerometer range (±4g is a good balance for writing) imu.setAccelRange(LSM6DS_ACCEL_RANGE_4_G); Serial.println("IMU: OK"); // Weights are compiled in via weights.h — nothing to load at runtime. Serial.println("Model: OK (weights compiled in)"); Serial.println("\nReady! Write a letter in the air above a flat surface."); Serial.println("The pen's X/Y plane should be parallel to the writing surface."); Serial.println("────────────────────────────────────────────────────────────\n"); } // ───────────────────────────────────────────────────────────────────────────── // loop() // ───────────────────────────────────────────────────────────────────────────── void loop() { // 1. Calibrate, wait for motion, record a writing gesture. if (!collectData()) return; // 2. Convert the IMU samples to a 28×28 trajectory image. if (!buildImage()) return; // 3. (Optional) print ASCII preview of the image for debugging. debugPrintImage(); // 4. Run the CNN and print the top-3 letter predictions. runInference(); // 5. Short pause so the Serial output is readable before the next prompt. delay(1000); } // ───────────────────────────────────────────────────────────────────────────── // Troubleshooting guide // ───────────────────────────────────────────────────────────────────────────── // // "IMU not found" // Verify wiring — SDA/SCL may be swapped, or VIN should be 3.3 V not 5 V. // Check the I2C address: run an I2C scanner sketch and see what address // the breakout reports. Change imu.begin_I2C() accordingly. // // "AllocateTensors() failed" // (no longer applicable — inference.h uses fixed static buffers) // // "Recording triggers from noise" // Increase START_THRESHOLD (e.g. 1.2 or 1.5). // // "Recording stops mid-letter" // Decrease STOP_THRESHOLD (e.g. 0.25) or increase STILL_COUNT_NEEDED. // // "Letters are misrecognised consistently" // Enable DEBUG_PRINT_IMAGE (set to 1) and look at the ASCII art. // If the shapes look roughly like the intended letters, the issue is // a mismatch between your writing style and EMNIST — fine-tune the // model with real pen data (see train_model.py "Going further" section). // If the shapes look like noise or scribbles, the trajectory reconstruction // needs tuning — try writing slower, or check gravity calibration. // // "The ASCII art letter is upside-down or mirrored" // Your IMU orientation differs from the assumed mounting. // In buildImage(), after the normalisation step, try flipping: // x → IMG_SIZE-1-x for a left-right mirror // y → IMG_SIZE-1-y for an upside-down flip // Swap x and y if the letter is rotated 90°.