Minimalist haptics with esp32 firmware + python OSC server for VRC

esp32_haptics_example.cpp

#include <Arduino.h>
#include <WiFi.h>
#include <WiFiUdp.h>

const char* ssid = "SSID";
const char* password = "PASSWORD";
WiFiUDP udp;
bool blink = false;
const uint8_t motors_pins[] = { 0, 1, 2, 3, 4, 5 };

void setup() {
    // setup pins
    pinMode(LED_BUILTIN, OUTPUT);
    for (int i = 0; i < sizeof(motors_pins); ++i) {
        ledcAttachChannel(motors_pins[i], 8000, 8, i);
    }

    WiFi.mode(WIFI_STA);
    WiFi.setSleep(false); // should fix high latency
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
        digitalWrite(LED_BUILTIN, (blink = !blink));
        delay(500);
    }
    digitalWrite(LED_BUILTIN, (blink = 0));

    // server
    udp.begin(1234);

    Serial.println("started, ready");
}

void loop() {
    int size = udp.parsePacket();
    if (size == 0) return;

    byte data[size];
    size = udp.read(data, size);
    size -= 1;

    switch (data[0]) {

    case '/': { // blink test
        digitalWrite(LED_BUILTIN, (blink = !blink));
        break;
    }

    case 'm': { // send motor value
        struct { uint8_t motor_id; uint8_t strength; }* data;
        if (size != sizeof(*data)) goto signal_error;
        data = (typeof(data))raw;
        if (data->motor_id >= sizeof(motors_pins)) goto signal_error;
        ledcWrite(data->motor_id, data->strength);
        break;
    }


    default: {
        goto signal_error;
    }
    }

    return;

signal_error:
    printf("error command: %c\n", data[0]);
}

osc_haptics_example.py

#!/usr/bin/env python3
import socket
from struct import pack
from pythonosc import dispatcher, osc_server


hostname = socket.gethostbyname("esp32-XXXXXX") # resolve one = efficient
hostport = 1234
if hostname is None:
    raise Exception("Device not found")

motors = {
        0: 'face0',
        1: 'face1',
        2: 'face2',
        3: 'face3',
        4: 'face4',
        5: 'face5',
        }

# invert dict (name -> address) and add prefix
motors = { f'haptics-{name}': addr
        for addr, name in motors.items() }

print("start esp32 socket")
sout = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

def on_haptics(path, proximity):
    # path ex: /avatar/parameters/haptics-face1
    name = path.split('/', 3)[-1]
    motor = motors.get(name)
    if motor is None: return # unregistered = ignored

    strength = 255 * (1 - proximity) # proximity 0 = max brrr 255
    print(f'sent_raw ESP: {motor} <- {strength}')
    data = b"m" + pack("BB", motor, strength)
    sout.sendto(data, (hostname, hostport))


print('start osc server')
dispatcher = dispatcher.Dispatcher()
dispatcher.map("/avatar/parameters/haptics-*", on_haptics)

server = osc_server.ThreadingOSCUDPServer(('127.0.0.1', 9001), dispatcher)
server.serve_forever()