ESP/README.md

# ESP32 IMU Bridge

This project bridges IMU data through an ESP32 to a PC, with optional gripper/relay control, real-time serial monitoring, CSV export, and signal-quality visualization.

## Features

- Reads WIT-format IMU UART frames on the ESP32: acceleration, gyro, angle, and temperature.
- Streams custom binary packets from the ESP32 to the PC over USB serial at 50 Hz.
- Allows the PC to control the gripper with simple keyboard commands:
  - `o`: OPEN
  - `c`: CLOSE
  - `s`: STOP
- Generates statistics, CSV files, and plots from either live serial data or saved text logs.
- Estimates world-frame linear acceleration and velocity from IMU acceleration and Euler angles.
- Uses a physical switch on GPIO27 to trigger one-shot gripper open/close pulses.

## Files

| File | Description |
| --- | --- |
| `main.py` | Main MicroPython program for the ESP32. It reads the IMU, controls DIO, and streams USB serial packets. |
| `esp_bridge.py` | PC-side Python wrapper class for integrating the ESP32 bridge into other programs. |
| `pc_reader.py` | PC-side real-time serial reader with keyboard gripper commands. |
| `visualise.py` | Sampling, statistics, CSV export, plotting, linear-acceleration estimation, and velocity integration tool. |
| `test.py` | ESP32 UART pin scan/debug helper. |
| `imu_data.csv` | Example or exported IMU sample data. |
| `rcd.txt` | Example text log from `pc_reader.py`. |
| `imu_quality*.png` | Visualization output images. |

## Hardware Wiring

### IMU UART

| IMU | ESP32 |
| --- | --- |
| TX yellow wire | GPIO16 / RX2 |
| RX green wire | GPIO17 / TX2 |
| GND | GND |

The default IMU baud rate is `921600`.

### Digital Inputs

Default digital input pins in `main.py`:

```python
DIN_PINS = (25, 26, 27, 32)
```

These inputs use `Pin.PULL_UP`, so normally:

- floating/open reads as `1`
- connected to ground/closed reads as `0`

GPIO27 is used as the physical toggle switch:

- `OFF -> ON`: triggers a CLOSE pulse
- `ON -> OFF`: triggers an OPEN pulse

### Digital Outputs / Relay

Default output pins in `main.py`:

```python
DOUT_PINS = (18, 19)
```

The relay is low-level triggered:

- GPIO HIGH: relay off
- GPIO LOW: relay on

Current output logic:

- GPIO18 / IN1 LOW: CLOSE
- GPIO19 / IN2 LOW: OPEN
- both GPIOs HIGH: STOP

## ESP32 Setup

1. Flash MicroPython firmware to the ESP32.
2. Upload `main.py` to the ESP32 root filesystem.
3. Reset the ESP32.

After reset, the program stays quiet for about 3 seconds, then starts streaming binary packets over USB serial.

Example upload commands:

```bash
mpremote connect /dev/ttyUSB0 fs cp main.py :main.py
mpremote connect /dev/ttyUSB0 reset
```

Replace `/dev/ttyUSB0` with your actual ESP32 serial port if needed.

## PC Environment

Python 3 is recommended.

Install dependencies:

```bash
python3 -m pip install pyserial matplotlib numpy
```

If you only use `pc_reader.py` or `esp_bridge.py`, `pyserial` is enough.

## Real-Time Reading And Control

Run:

```bash
python3 pc_reader.py /dev/ttyUSB0 --baud 115200
```

The output should look similar to:

```text
#     793   50.0Hz DIN=[0, 1, 0, 0] DOUT=[1, 0] ACC=[...] GYRO=[...] ANGLE=[...]
```

Type a command in the terminal and press Enter:

```text
o
c
s
```

These commands mean OPEN, CLOSE, and STOP.

## Python API Usage

`esp_bridge.py` provides an `ESP32Bridge` class for use in other PC-side Python programs:

```python
from esp_bridge import ESP32Bridge

with ESP32Bridge("/dev/ttyUSB0") as bridge:
    bridge.open_gripper()
    data = bridge.read_packet_with_frequency()
    print(data)
    bridge.stop_gripper()
```

Continuous reading example:

```python
from esp_bridge import ESP32Bridge

with ESP32Bridge("/dev/ttyUSB0") as bridge:
    for packet in bridge.iter_packets(seconds=10):
        print(packet["freq_hz"], packet["acc_g"], packet["angle_deg"])
```

## Sampling, Export, And Visualization

Sample from serial for 10 seconds, save a CSV file, and generate a plot:

```bash
python3 visualise.py --port /dev/ttyUSB0 --baud 115200 --seconds 10 --csv imu_data.csv --output imu_quality.png
```

Save files only without opening a plot window:

```bash
python3 visualise.py --port /dev/ttyUSB0 --baud 115200 --seconds 10 --csv imu_data.csv --output imu_quality.png --no-show
```

Read from a `pc_reader.py` text log and plot it:

```bash
python3 visualise.py --file rcd.txt --output imu_quality.png
```

`visualise.py` now adds derived motion columns before saving or plotting:

| Column | Description |
| --- | --- |
| `lin_acc_x_ms2`, `lin_acc_y_ms2`, `lin_acc_z_ms2` | Estimated world-frame linear acceleration in `m/s^2` after gravity and bias compensation. |
| `vel_x_ms`, `vel_y_ms`, `vel_z_ms` | Estimated world-frame velocity in `m/s` from integrating linear acceleration. |

The generated plot contains six stacked views:

- raw acceleration
- linear acceleration
- velocity
- gyro
- angle
- packet frequency

CSV files are saved under `csv/`, PNG files are saved under `png/`, and other output extensions are saved under `output/`. If the target file already exists, `visualise.py` appends a timestamp to avoid overwriting it, for example:

```text
png/imu_quality_20260701_131902.png
```

## Linear Acceleration And Velocity

The raw accelerometer output includes both motion acceleration and the support-force/gravity-related acceleration measured by the IMU. When the sensor is placed horizontally and kept still, it will still measure about `1g` on the vertical axis. This is expected: the table support force prevents free fall, and the accelerometer senses that proper acceleration.

Because of this, `visualise.py` removes the static `1g` component before integrating velocity. The current calculation is:

1. Read raw acceleration in the IMU/body frame: `acc_x_g`, `acc_y_g`, `acc_z_g`.
2. Convert Euler angles to a body-to-world rotation matrix:

   ```python
   R = Rz(yaw) @ Ry(pitch) @ Rx(roll)
   ```

3. Rotate body-frame acceleration into the world frame:

   ```python
   acc_world_g = R @ acc_body_g
   ```

4. Subtract the world Z-axis support-force/gravity component:

   ```python
   linear_acc_world_g = acc_world_g - [0, 0, gravity_sign]
   ```

   The default call uses `gravity_sign=1.0`, so a horizontally placed, still sensor is expected to have approximately `+1g` on the world Z axis before compensation.

5. Convert from `g` to `m/s^2` with `9.81`.
6. Estimate a small acceleration bias from the first `bias_seconds` seconds, default `1.0` second, and subtract it.
7. Apply an acceleration deadband, default `0.15 m/s^2`, to reduce small stationary noise.
8. Integrate acceleration to velocity with a small decay factor, default `vel_decay=0.995`, to limit drift.

This velocity estimate is useful for short-duration motion checks and relative comparisons. It will drift over time because low-cost IMU acceleration, attitude error, and numerical integration all accumulate error. Keep the sensor still for the first second when possible so the initial bias estimate is meaningful.

## Packet Protocol

The ESP32-to-PC binary packet format is defined in `main.py`, `pc_reader.py`, and `esp_bridge.py`:

```python
PACKET_FORMAT = "<2sBBBI10hB"
```

Field layout:

| Field | Type | Description |
| --- | --- | --- |
| sync | `2s` | Fixed bytes `A5 5A` |
| version | `B` | Currently `2` |
| din_mask | `B` | 4-channel digital input bit mask |
| dout_mask | `B` | 2-channel digital output bit mask |
| time_ms | `I` | ESP32 `ticks_ms()` value |
| raw_values | `10h` | acc xyz, gyro xyz, angle xyz, temperature |
| checksum | `B` | Low 8 bits of the sum of all previous bytes |

Raw value scaling:

| Data | Scaling |
| --- | --- |
| Acceleration | `raw * 16 / 32768`, unit `g` |
| Gyro | `raw * 2000 / 32768`, unit `deg/s` |
| Angle | `raw * 180 / 32768`, unit `deg` |
| Temperature | `raw / 100`, unit `deg C` |

## Common Parameters

The most useful hardware and timing parameters are in `main.py`:

```python
IMU_RX_PIN = 16
IMU_TX_PIN = 17
DIN_PINS = (25, 26, 27, 32)
DOUT_PINS = (18, 19)
IMU_BAUD = 921600
SEND_PERIOD_MS = 20
SWITCH_PULSE_MS = 2000
USE_REAL_DOUT = True
```

Before testing a real relay, you can set:

```python
USE_REAL_DOUT = False
```

In this mode, the PC can still see `DOUT` state changes, but the ESP32 will not actually drive GPIO18/GPIO19.

## Troubleshooting

### Serial Port Not Found

On Linux, list possible serial ports with:

```bash
ls /dev/ttyUSB* /dev/ttyACM*
```

If you have permission issues, add your user to the `dialout` group or temporarily run the serial tool with `sudo`.

### Frequency Is Not 50 Hz

Check the following:

- The ESP32 has entered the `main.py` loop.
- The PC-side baud rate is `115200`.
- The IMU-to-ESP32 UART baud rate is `921600`.
- The USB serial cable and power supply are stable.

### Data Is All Zero Or Angle Does Not Update

Check the IMU wiring and output format. The ESP32 parser currently handles WIT frames:

- `0x51`: acceleration
- `0x52`: gyro
- `0x53`: angle

The program treats an angle frame as the point where one full IMU update is complete.

### OPEN/CLOSE Direction Is Reversed

Depending on the relay and motor wiring, swap the relay inputs connected to GPIO18/GPIO19, or adjust the OPEN/CLOSE output logic in `set_gripper()`.

## Suggested Workflow

1. Use `pc_reader.py` to confirm the PC receives stable 50 Hz packets.
2. Type `o`, `c`, and `s` to confirm `DOUT` state changes.
3. Enable real relay output with `USE_REAL_DOUT = True` when ready.
4. Use `visualise.py` to sample data and generate CSV/plot outputs for checking noise, frequency, and angle stability.