Untitled

// ============================================================
// XIAO C6 #3 — ESC + VESC Controller
// Board: Seeed XIAO ESP32-C6
// MAC: 58:E6:C5:01:8D:38
//
// - Reads incoming PWM signal from ESC
// - Reads VESC telemetry (RPM, voltage, current, temp)
// - Outputs throttle PWM to ESC from LabVIEW commands
// - Sends all ESC/VESC data to XIAO C6 #2 via ESP-NOW
//
// XIAO C6 Pin Mapping:
//   D6 = GPIO16 (VESC TX out)
//   D7 = GPIO17 (VESC RX in)
//   D8 = GPIO19 (PWM OUT to ESC)
//   D9 = GPIO20 (PWM IN from ESC monitor)
// ============================================================

#include <esp_now.h>
#include <WiFi.h>
#include <esp_wifi.h>
#include <ESP32Servo.h>
#include <VescUart.h>
#include <HardwareSerial.h>

// ---- Pin Definitions (XIAO C6) ----
#define PWM_OUT_PIN  19   // D8 → ESC signal input
#define PWM_IN_PIN   20   // D9 ← ESC PWM signal monitor

// ---- VESC UART (Serial1) ----
#define VESC_RX_PIN  17   // D7
#define VESC_TX_PIN  16   // D6

// ---- Motor pole pairs — update to match your motor ----
#define MOTOR_POLE_PAIRS 7

// ---- Safety ----
#define PWM_MIN_US   1000
#define PWM_MAX_US   2000
#define PWM_ARM_US   1000
#define TIMEOUT_MS   2000

// ---- Receiver MAC (XIAO C6 #2) ----
uint8_t receiverMAC[] = {0x58, 0xE6, 0xC5, 0x00, 0x94, 0xCC};

// ---- Servo/ESC Output ----
Servo esc;

// ---- VESC ----
VescUart vesc;

// ---- PWM Input Reading ----
volatile unsigned long pwmRiseTime   = 0;
volatile unsigned long pwmPulseWidth = 0;
volatile bool pwmNewReading = false;

void IRAM_ATTR pwmFallingEdge();

void IRAM_ATTR pwmRisingEdge() {
  pwmRiseTime = micros();
  attachInterrupt(digitalPinToInterrupt(PWM_IN_PIN), pwmFallingEdge, FALLING);
}

void IRAM_ATTR pwmFallingEdge() {
  pwmPulseWidth = micros() - pwmRiseTime;
  pwmNewReading = true;
  attachInterrupt(digitalPinToInterrupt(PWM_IN_PIN), pwmRisingEdge, RISING);
}

// ---- Command Packet (received from XIAO #2) ----
typedef struct __attribute__((packed)) {
  uint8_t source;
  float   throttle_pct;
  uint8_t estop;
} CommandPacket;

// ---- Data Packet (sent to XIAO #2) ----
typedef struct __attribute__((packed)) {
  uint8_t  source;
  float    throttle_pct;
  uint32_t pwm_us;
  float    rpm;
  float    motor_voltage;
  float    motor_current;
  float    motor_power;
  float    motor_temp;
  bool     vesc_valid;
} EscPacket;

// ---- State ----
float currentThrottle = 0.0;
bool  estopActive     = false;
bool  prevEstopState  = false;
String estopSource    = "none";
unsigned long lastCommandTime = 0;

EscPacket outPacket;
esp_now_peer_info_t receiverPeer;

// ---- Receive Callback ----
void onDataReceived(const esp_now_recv_info *recvInfo, const uint8_t *data, int len) {
  if (len != sizeof(CommandPacket)) return;
  CommandPacket cmd;
  memcpy(&cmd, data, sizeof(cmd));
  if (cmd.source != 10) return;

  lastCommandTime = millis();

  if (cmd.estop) {
    estopActive     = true;
    currentThrottle = 0.0;
    estopSource     = "LabVIEW";
  } else {
    estopActive     = false;
    estopSource     = "none";
    currentThrottle = constrain(cmd.throttle_pct, 0.0, 100.0);
  }
}

// ---- Send Callback ----
void onDataSent(const wifi_tx_info_t *txInfo, esp_now_send_status_t status) {
  if (status != ESP_NOW_SEND_SUCCESS) {
    Serial.println("ESC board: Send FAILED");
  }
}

int throttleToPWM(float pct) {
  return (int)(PWM_MIN_US + (pct / 100.0) * (PWM_MAX_US - PWM_MIN_US));
}

void setup() {
  Serial.begin(115200);
  delay(200);

  // -- ESC PWM Output --
  esc.attach(PWM_OUT_PIN, PWM_MIN_US, PWM_MAX_US);
  esc.writeMicroseconds(PWM_ARM_US);
  Serial.println("ESC arming...");
  delay(3000);
  Serial.println("ESC armed");

  // -- PWM Input --
  pinMode(PWM_IN_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(PWM_IN_PIN), pwmRisingEdge, RISING);
  Serial.println("PWM monitor ready");

  // -- VESC UART --
  Serial1.begin(115200, SERIAL_8N1, VESC_RX_PIN, VESC_TX_PIN);
  vesc.setSerialPort(&Serial1);
  Serial.println("VESC UART ready");

  // -- ESP-NOW with legacy protocol for C6 compatibility --
  WiFi.mode(WIFI_STA);
  esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N);
  esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE);

  Serial.print("ESC board MAC: ");
  Serial.println(WiFi.macAddress());

  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW init failed");
    while (true) delay(1000);
  }

  esp_now_register_recv_cb(onDataReceived);
  esp_now_register_send_cb(onDataSent);

  memcpy(receiverPeer.peer_addr, receiverMAC, 6);
  receiverPeer.channel = 1;
  receiverPeer.encrypt = false;

  if (esp_now_add_peer(&receiverPeer) != ESP_OK) {
    Serial.println("Failed to add peer");
    while (true) delay(1000);
  }

  outPacket.source = 1;
  Serial.println("ESC board ready");
}

void loop() {
  // -- Watchdog E-stop --
  if (millis() - lastCommandTime > TIMEOUT_MS && lastCommandTime > 0) {
    if (!estopActive) {
      estopActive     = true;
      currentThrottle = 0.0;
      estopSource     = "timeout";
    }
  }

  // -- Detect estop state changes --
  if (estopActive && !prevEstopState) {
    Serial.println("╔══════════════════════════════╗");
    Serial.printf ("║  !!! ESTOP ACTIVE !!!        ║\n");
    Serial.printf ("║  Source: %-20s║\n", estopSource.c_str());
    Serial.println("║  PWM locked to 1000us (0%)   ║");
    Serial.println("╚══════════════════════════════╝");
  } else if (!estopActive && prevEstopState) {
    Serial.println("╔══════════════════════════════╗");
    Serial.println("║  ESTOP CLEARED               ║");
    Serial.printf ("║  Resuming throttle: %.1f%%     ║\n", currentThrottle);
    Serial.println("╚══════════════════════════════╝");
  }
  prevEstopState = estopActive;

  // -- Apply throttle to ESC --
  int pwmOut = estopActive ? PWM_MIN_US : throttleToPWM(currentThrottle);
  esc.writeMicroseconds(pwmOut);

  // -- Read incoming PWM --
  if (pwmNewReading) {
    noInterrupts();
    unsigned long pw = pwmPulseWidth;
    pwmNewReading = false;
    interrupts();
    pw = constrain(pw, 1000, 2000);
    outPacket.pwm_us       = pw;
    outPacket.throttle_pct = (pw - 1000.0) / 10.0;
  }

  // -- Read VESC telemetry --
  if (vesc.getVescValues()) {
    outPacket.rpm           = abs(vesc.data.rpm) / MOTOR_POLE_PAIRS;
    outPacket.motor_voltage = vesc.data.inpVoltage;
    outPacket.motor_current = vesc.data.avgInputCurrent;
    outPacket.motor_power   = vesc.data.inpVoltage * vesc.data.avgInputCurrent;
    outPacket.motor_temp    = vesc.data.tempMosfet;
    outPacket.vesc_valid    = true;
  } else {
    outPacket.vesc_valid = false;
  }

  // -- Send to receiver --
  esp_now_send(receiverMAC, (uint8_t *)&outPacket, sizeof(outPacket));

  // -- Debug every 2 seconds --
  static unsigned long lastDebug = 0;
  if (millis() - lastDebug >= 2000) {
    Serial.println("------ ESC Board STATUS ------");
    Serial.printf("E-Stop:     %s (source: %s)\n",
      estopActive ? "ACTIVE" : "clear", estopSource.c_str());
    Serial.printf("Throttle:   %.1f%%\n", currentThrottle);
    Serial.printf("PWM out:    %d us\n",  pwmOut);
    Serial.printf("PWM in:     %lu us\n", outPacket.pwm_us);
    Serial.printf("RPM:        %.0f\n",   outPacket.rpm);
    Serial.printf("VESC:       %s\n",     outPacket.vesc_valid ? "OK" : "NO SIGNAL");
    Serial.println("------------------------------");
    lastDebug = millis();
  }

  delay(20);
}