m5 spider bot

#include <M5Unified.h>
#include <Wire.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>

// ---------- Servo HAT ----------
static const uint8_t HAT_ADDR = 0x38;   // 8Servos HAT
static const uint8_t REG_ANGLE = 0x00;  // 0x00..0x07 -> angle 0..180
static const int SDA_PIN = 0;           // M5StickC S3 HAT bus
static const int SCL_PIN = 1;

// Per-servo trim so a mechanically neutral spider really sits at 90 deg.
static const int8_t TRIM[8] = {0, 0, 0, 0, 0, 0, 0, 0};
// Flip direction for mirrored legs (right side is mounted mirrored).
static const int8_t DIR[8] = {1, -1, 1, -1, 1, -1, 1, -1};

static uint8_t current[8];
static uint8_t target[8];

static void hatWrite(uint8_t ch, uint8_t angle) {
  Wire.beginTransmission(HAT_ADDR);
  Wire.write(REG_ANGLE + ch);
  Wire.write(angle);
  Wire.endTransmission();
}

static void setJoint(uint8_t ch, int angle) {
  int a = 90 + DIR[ch] * (angle - 90) + TRIM[ch];
  a = constrain(a, 5, 175);
  target[ch] = (uint8_t)a;
}

// ---------- Motion engine ----------
static int gSpeed = 70;          // 10..100
static bool gStopped = false;

// Move all joints toward target at a rate derived from gSpeed.
static void servoTick() {
  int step = map(gSpeed, 10, 100, 1, 6);
  for (uint8_t i = 0; i < 8; i++) {
    if (current[i] == target[i]) continue;
    int d = (int)target[i] - (int)current[i];
    if (abs(d) <= step) current[i] = target[i];
    else current[i] += (d > 0 ? step : -step);
    hatWrite(i, current[i]);
  }
}

// Blocking-ish helper: drive to targets, honouring STOP.
static void settle(uint16_t ms) {
  uint32_t end = millis() + ms;
  while (millis() < end) {
    servoTick();
    delay(12);
    if (gStopped) return;
  }
}

// Pose helpers ------------------------------------------------------------
static void poseStand() {
  for (uint8_t l = 0; l < 4; l++) {
    setJoint(l * 2, 90);      // hip centred
    setJoint(l * 2 + 1, 60);  // knee down -> body lifted
  }
}

static void poseSit() {
  for (uint8_t l = 0; l < 4; l++) {
    setJoint(l * 2, 90);
    setJoint(l * 2 + 1, 130);
  }
}

// One half-step of a diagonal (trot) gait.
// group 0 = FL+RR swing, group 1 = FR+RL swing. hipDir sets travel direction.
static void trotHalfStep(uint8_t group, int hipSwing) {
  const uint8_t g[2][2] = {{0, 3}, {1, 2}};  // leg indexes per group
  uint16_t dwell = map(gSpeed, 10, 100, 320, 90);

  // lift swing legs
  for (uint8_t k = 0; k < 2; k++) setJoint(g[group][k] * 2 + 1, 30);
  settle(dwell / 2);
  // swing them forward, push the stance legs backward
  for (uint8_t k = 0; k < 2; k++) setJoint(g[group][k] * 2, 90 + hipSwing);
  for (uint8_t k = 0; k < 2; k++) setJoint(g[1 - group][k] * 2, 90 - hipSwing);
  settle(dwell / 2);
  // put swing legs down
  for (uint8_t k = 0; k < 2; k++) setJoint(g[group][k] * 2 + 1, 60);
  settle(dwell / 2);
}

static void turnHalfStep(uint8_t group, int spin) {
  const uint8_t g[2][2] = {{0, 3}, {1, 2}};
  uint16_t dwell = map(gSpeed, 10, 100, 320, 90);
  for (uint8_t k = 0; k < 2; k++) setJoint(g[group][k] * 2 + 1, 30);
  settle(dwell / 2);
  // all hips rotate the same way -> body yaws
  for (uint8_t i = 0; i < 4; i++) setJoint(i * 2, 90 + spin);
  settle(dwell / 2);
  for (uint8_t k = 0; k < 2; k++) setJoint(g[group][k] * 2 + 1, 60);
  settle(dwell / 2);
}

static void poseWave() {
  poseStand();
  settle(250);
  for (uint8_t i = 0; i < 3 && !gStopped; i++) {
    setJoint(1, 10);   // FL knee up
    setJoint(0, 130);  // FL hip out
    settle(220);
    setJoint(0, 60);
    settle(220);
  }
  poseStand();
  settle(250);
}

static void poseDance() {
  for (uint8_t i = 0; i < 4 && !gStopped; i++) {
    for (uint8_t l = 0; l < 4; l++) setJoint(l * 2 + 1, (i % 2) ? 40 : 80);
    settle(200);
  }
  poseStand();
  settle(200);
}

// ---------- Command state ----------
static int driveX = 0, driveY = 0;   // -100..100
static uint8_t phase = 0;
static String activeGait = "";

static void applyDrive() {
  if (gStopped) return;
  if (driveX == 0 && driveY == 0) return;

  int mag = max(abs(driveX), abs(driveY));
  int swing = map(mag, 0, 100, 5, 35);

  if (abs(driveY) >= abs(driveX)) {
    trotHalfStep(phase, driveY > 0 ? swing : -swing);
  } else {
    turnHalfStep(phase, driveX > 0 ? swing : -swing);
  }
  phase ^= 1;
}

// ---------- BLE / NUS ----------
#define NUS_SERVICE "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
#define NUS_RX      "6e400002-b5a3-f393-e0a9-e50e24dcca9e"  // phone -> robot (write)
#define NUS_TX      "6e400003-b5a3-f393-e0a9-e50e24dcca9e"  // robot -> phone (notify)

static BLECharacteristic *txChar = nullptr;
static bool bleConnected = false;

static void notify(const String &s) {
  if (txChar && bleConnected) {
    String line = s + "\n";
    txChar->setValue((uint8_t *)line.c_str(), line.length());
    txChar->notify();
  }
}

static void handleLine(String line) {
  line.trim();
  if (!line.length()) return;

  if (line == "STOP") {
    gStopped = true;
    driveX = driveY = 0;
    activeGait = "";
    for (uint8_t i = 0; i < 8; i++) target[i] = current[i];
    notify("OK STOP");
    return;
  }

  gStopped = false;

  if (line.startsWith("M,")) {
    int c1 = line.indexOf(',', 2);
    int c2 = line.indexOf(',', c1 + 1);
    driveX = constrain(line.substring(2, c1).toInt(), -100, 100);
    driveY = constrain(line.substring(c1 + 1, c2).toInt(), -100, 100);
    if (c2 > 0) gSpeed = constrain(line.substring(c2 + 1).toInt(), 10, 100);
    if (driveX == 0 && driveY == 0) poseStand();
    activeGait = "";
    return;
  }

  if (line.startsWith("S,")) {
    gSpeed = constrain(line.substring(2).toInt(), 10, 100);
    notify("OK S " + String(gSpeed));
    return;
  }

  if (line.startsWith("G,")) {
    String g = line.substring(2);
    g.trim();
    driveX = driveY = 0;
    activeGait = "";
    if (g == "stand") poseStand();
    else if (g == "sit") poseSit();
    else if (g == "wave") poseWave();
    else if (g == "dance") poseDance();
    else if (g == "walk") { driveY = 100; }
    else if (g == "back") { driveY = -100; }
    else if (g == "left") { driveX = -100; }
    else if (g == "right") { driveX = 100; }
    notify("OK G " + g);
    return;
  }

  notify("ERR " + line);
}

class ServerCB : public BLEServerCallbacks {
  void onConnect(BLEServer *s) override {
    bleConnected = true;
    M5.Display.fillScreen(TFT_BLACK);
    M5.Display.setCursor(4, 4);
    M5.Display.print("LINKED");
  }
  void onDisconnect(BLEServer *s) override {
    bleConnected = false;
    gStopped = true;
    driveX = driveY = 0;
    s->getAdvertising()->start();
    M5.Display.fillScreen(TFT_BLACK);
    M5.Display.setCursor(4, 4);
    M5.Display.print("ADVERTISING");
  }
};

class RxCB : public BLECharacteristicCallbacks {
  String buf;
  void onWrite(BLECharacteristic *c) override {
    String v = String(c->getValue().c_str());
    for (unsigned int i = 0; i < v.length(); i++) {
      char ch = v[i];
      if (ch == '\n' || ch == '\r') {
        handleLine(buf);
        buf = "";
      } else if (buf.length() < 64) {
        buf += ch;
      }
    }
  }
};

// ---------- Arduino entry points ----------
void setup() {
  auto cfg = M5.config();
  M5.begin(cfg);
  M5.Display.setRotation(1);
  M5.Display.setTextSize(2);
  M5.Display.fillScreen(TFT_BLACK);
  M5.Display.setCursor(4, 4);
  M5.Display.print("ARACHNE");

  Wire.begin(SDA_PIN, SCL_PIN, 100000);

  for (uint8_t i = 0; i < 8; i++) {
    current[i] = 90;
    target[i] = 90;
    hatWrite(i, 90);
    delay(40);
  }
  poseStand();
  settle(600);

  BLEDevice::init("Arachne Spider");
  BLEServer *server = BLEDevice::createServer();
  server->setCallbacks(new ServerCB());
  BLEService *svc = server->createService(NUS_SERVICE);

  txChar = svc->createCharacteristic(NUS_TX, BLECharacteristic::PROPERTY_NOTIFY);
  txChar->addDescriptor(new BLE2902());

  BLECharacteristic *rxChar = svc->createCharacteristic(
      NUS_RX, BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
  rxChar->setCallbacks(new RxCB());

  svc->start();
  BLEAdvertising *adv = BLEDevice::getAdvertising();
  adv->addServiceUUID(NUS_SERVICE);
  adv->setScanResponse(true);
  adv->start();

  M5.Display.setCursor(4, 30);
  M5.Display.print("ADVERTISING");
}

void loop() {
  M5.update();
  if (M5.BtnA.wasPressed()) {  // panic button on the stick
    gStopped = true;
    driveX = driveY = 0;
  }
  applyDrive();
  servoTick();
  delay(10);
}