diff --git a/.gitignore b/.gitignore index ad75bf1..bb117f2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,2 @@ -Oliver/logs/ +Oliver/logs/ .venv/ \ No newline at end of file diff --git a/Angle_4_click b/Angle_4_click new file mode 160000 index 0000000..35ecd67 --- /dev/null +++ b/Angle_4_click @@ -0,0 +1 @@ +Subproject commit 35ecd6735a8d3cf0b0fc1bc5544a8d693c773657 diff --git a/Documents/AEAT_8800_Q24.pdf b/Documents/AEAT_8800_Q24.pdf new file mode 100644 index 0000000..f227bce Binary files /dev/null and b/Documents/AEAT_8800_Q24.pdf differ diff --git a/Oliver/master/master.ino b/Oliver/master/master.ino index a792f3b..d0a56af 100644 --- a/Oliver/master/master.ino +++ b/Oliver/master/master.ino @@ -1,469 +1,469 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* - * Oliver Master Gateway - Robust Discovery Edition - * Hardware: XIAO ESP32C6 - * Features: - * - 30s blocking discovery for all 5 slaves - * - Continuous re-discovery for missing slaves - * - BLE command interface for manual re-discovery - * - Detailed status reporting - * Author: Swaraj Dangare - */ - -#define NUM_SLAVES 1 // FIXED: Was 4, now 5 -#define WIFI_CHANNEL 11 // ESP-NOW channel (use 1, 6, or 11 to isolate from other Oliver sets) -#define SERVICE_UUID "6ab88bb9-cf50-4564-b1c4-f53be2abc53f" -#define CHARACTERISTIC_UUID "1d4cd358-172d-4c33-b0b2-ddce9a071aab" -#define COMMAND_UUID "308a0c43-80f0-4b01-81e5-bb2798eb92f9" - -typedef struct __attribute__((packed)) { - uint8_t id; - int value; // Angle * 10000 - uint32_t packetIdx; - uint8_t agc; // AS5047D AGC value (0-255) - uint16_t mag; // AS5047D CORDIC magnitude (14-bit) - uint8_t magl; // Magnetic field too low (0 or 1) - uint8_t magh; // Magnetic field too high (0 or 1) - uint8_t cof; // CORDIC overflow (0 or 1) -} Payload; - -uint8_t slaveMACs[NUM_SLAVES][6]; -bool slaveFound[NUM_SLAVES] = {false}; -Payload slaves[NUM_SLAVES]; -uint32_t gatewayPacketIdx = 0; -uint32_t lastSeenTime[NUM_SLAVES] = {0}; // Track last response time - -BLECharacteristic *pChar; -BLECharacteristic *pCommandChar; -bool pcConnected = false; -bool rediscoverRequested = false; -bool readRequested = false; -bool slaveResponded[NUM_SLAVES] = {false}; - -// ---------------- MASTER ENCODER SETTINGS ---------------- -#define ANGLECOM 0x3FFF -#define DIAAGC_REG 0x3FFC -#define MAG_REG 0x3FFD -#define RD 0x40 -#define NUM_BLOCKS 16 -#define SAMPLES_PER_BLOCK 256 - -const int PIN_CS = D7; -const int PIN_SCK = D1; -const int PIN_MISO = D0; -const int PIN_MOSI = D10; - -SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); - -// Master encoder data -Payload masterData = {255, 0, 0, 0, 0, 0, 0, 0}; // id=255 for master - -// ESP-NOW Receive Callback -void onEspNowRecv(const esp_now_recv_info_t *info, const uint8_t *data, - int len) { - if (len == 1) { // Discovery Response - uint8_t id = data[0]; - if (id < NUM_SLAVES) { - if (!slaveFound[id]) { - memcpy(slaveMACs[id], info->src_addr, 6); - slaveFound[id] = true; - esp_now_peer_info_t peer{}; - memcpy(peer.peer_addr, info->src_addr, 6); - peer.channel = WIFI_CHANNEL; - peer.encrypt = false; - esp_now_add_peer(&peer); - // Safe to print here - quick message - Serial.printf( - "[DISCOVERY] Found Slave %d: %02X:%02X:%02X:%02X:%02X:%02X\n", id, - info->src_addr[0], info->src_addr[1], info->src_addr[2], - info->src_addr[3], info->src_addr[4], info->src_addr[5]); - } - lastSeenTime[id] = millis(); - } - } else if (len == sizeof(Payload)) { // Data Response - Payload p; - memcpy(&p, data, sizeof(p)); - if (p.id < NUM_SLAVES) { - slaves[p.id] = p; - lastSeenTime[p.id] = millis(); - slaveResponded[p.id] = true; - } - } -} - -// ---------------- MASTER ENCODER FUNCTIONS ---------------- -uint16_t evenParityBit(uint16_t x) { - x &= 0x7FFF; - return __builtin_parity(x); -} - -uint16_t makeReadCmd(uint16_t addr) { - uint16_t cmd = (1 << 14) | (addr & 0x3FFF); - cmd |= (evenParityBit(cmd) << 15); - return cmd; -} - -uint16_t AS5047D_Read() { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(PIN_CS, LOW); - SPI.transfer16(makeReadCmd(ANGLECOM)); - digitalWrite(PIN_CS, HIGH); - delayMicroseconds(1); - digitalWrite(PIN_CS, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(PIN_CS, HIGH); - SPI.endTransaction(); - return result; -} - -uint16_t readRegister(uint16_t addr) { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(PIN_CS, LOW); - SPI.transfer16(makeReadCmd(addr)); - digitalWrite(PIN_CS, HIGH); - delayMicroseconds(1); - digitalWrite(PIN_CS, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(PIN_CS, HIGH); - SPI.endTransaction(); - return result & 0x3FFF; -} - -double getRobustMean(uint16_t *samples, int size) { - double sum = 0; - for (int i = 0; i < size; i++) - sum += (samples[i] & 0x3FFF); - double initialMean = sum / size; - - double robustSum = 0; - int count = 0; - for (int i = 0; i < size; i++) { - uint16_t val = samples[i] & 0x3FFF; - if (abs((double)val - initialMean) < 1.5) { - robustSum += val; - count++; - } - } - return (count > 0) ? (robustSum / count) : initialMean; -} - -double getMedian(double *values, int size) { - std::sort(values, values + size); - return values[size / 2]; -} - -double getUltraPrecisionReading() { - double blockMeans[NUM_BLOCKS]; - uint16_t blockSamples[SAMPLES_PER_BLOCK]; - - for (int b = 0; b < NUM_BLOCKS; b++) { - for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { - uint16_t raw = AS5047D_Read(); - if (((raw >> 15) & 1) == evenParityBit(raw)) { - blockSamples[s] = raw; - } else { - s--; - } - } - blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); - delayMicroseconds(50); - } - double finalCounts = getMedian(blockMeans, NUM_BLOCKS); - return (finalCounts * 360.0) / 16384.0; -} - -// BLE Callbacks -class ServerCallbacks : public BLEServerCallbacks { - void onConnect(BLEServer *) { - pcConnected = true; - Serial.println("[BLE] Client Connected"); - } - void onDisconnect(BLEServer *) { - pcConnected = false; - Serial.println("[BLE] Client Disconnected"); - BLEDevice::startAdvertising(); - } -}; - -// Command Handler -class CommandCallbacks : public BLECharacteristicCallbacks { - void onWrite(BLECharacteristic *pChar) { - String value = - pChar->getValue().c_str(); // Convert std::string to Arduino String - if (value == "READ") { - readRequested = true; - for (int i = 0; i < NUM_SLAVES; i++) - slaveResponded[i] = false; - } else if (value == "REDISCOVER") { - rediscoverRequested = true; - Serial.println("[CMD] Re-discovery requested from PC"); - } - } -}; - -// Discovery Function -void discoverSlaves(uint32_t timeoutMs) { - uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - uint32_t startTime = millis(); - - Serial.printf( - "[DISCOVERY] Starting discovery for %d slaves (timeout: %dms)...\n", - NUM_SLAVES, timeoutMs); - - while (millis() - startTime < timeoutMs) { - // Count found slaves - int foundCount = 0; - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i]) - foundCount++; - } - - // Exit early if all found - if (foundCount == NUM_SLAVES) { - Serial.println("[DISCOVERY] All slaves found!"); - break; - } - - // Send broadcast - uint8_t ping = 0xFF; - esp_now_send(broadcastAddr, &ping, 1); - - // Print status every 2 seconds - static uint32_t lastPrint = 0; - if (millis() - lastPrint > 2000) { - lastPrint = millis(); - Serial.printf("[DISCOVERY] Progress: %d/%d slaves found | Missing: ", - foundCount, NUM_SLAVES); - for (int i = 0; i < NUM_SLAVES; i++) { - if (!slaveFound[i]) - Serial.printf("S%d ", i); - } - Serial.println(); - } - - delay(200); // Broadcast every 200ms (was 500ms) - } - - // Final report - int finalCount = 0; - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i]) - finalCount++; - } - - Serial.println("\n" + String('=', 50)); - Serial.printf("[DISCOVERY] Complete: %d/%d slaves discovered\n", finalCount, - NUM_SLAVES); - if (finalCount < NUM_SLAVES) { - Serial.print("[WARNING] Missing slaves: "); - for (int i = 0; i < NUM_SLAVES; i++) { - if (!slaveFound[i]) - Serial.printf("S%d ", i); - } - Serial.println("\n[INFO] Will retry in background..."); - } - Serial.println(String('=', 50) + "\n"); -} - -void setup() { - Serial.begin(115200); - delay(1000); // Give serial time to initialize - - Serial.println("\n\n=== OLIVER MASTER GATEWAY ==="); - Serial.println("Hardware: XIAO ESP32C6"); - Serial.printf("Firmware: Robust Discovery v2.0\n\n"); - - // WiFi Init - WiFi.mode(WIFI_STA); - WiFi.disconnect(); - Serial.printf("[WIFI] MAC Address: %s\n", WiFi.macAddress().c_str()); - - // Master Encoder SPI Init - pinMode(PIN_CS, OUTPUT); - digitalWrite(PIN_CS, HIGH); - SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); - Serial.println("[SPI] Master encoder initialized"); - - // Force WiFi channel - esp_wifi_set_promiscuous(true); - esp_wifi_set_channel(WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE); - esp_wifi_set_promiscuous(false); - Serial.printf("[WIFI] Channel %d locked\n", WIFI_CHANNEL); - - // ESP-NOW Init - if (esp_now_init() != ESP_OK) { - Serial.println("[ERROR] ESP-NOW Init Failed!"); - return; - } - esp_now_register_recv_cb(onEspNowRecv); - Serial.println("[ESP-NOW] Initialized"); - - // Add Broadcast Peer - uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - esp_now_peer_info_t bcast{}; - memcpy(bcast.peer_addr, broadcastAddr, 6); - bcast.channel = WIFI_CHANNEL; - bcast.encrypt = false; - esp_now_add_peer(&bcast); - Serial.println("[ESP-NOW] Broadcast peer added\n"); - - // Initial Discovery (30 seconds, blocking) - discoverSlaves(30000); - - // BLE Init - Serial.println("[BLE] Initializing..."); - BLEDevice::init("Oliver_3"); - BLEServer *pServer = BLEDevice::createServer(); - pServer->setCallbacks(new ServerCallbacks()); - - BLEService *pService = pServer->createService(SERVICE_UUID); - - // Data characteristic (notify) - pChar = pService->createCharacteristic(CHARACTERISTIC_UUID, - BLECharacteristic::PROPERTY_NOTIFY); - pChar->addDescriptor(new BLE2902()); - - // Command characteristic (write) - pCommandChar = pService->createCharacteristic( - COMMAND_UUID, BLECharacteristic::PROPERTY_WRITE); - pCommandChar->setCallbacks(new CommandCallbacks()); - - pService->start(); - - BLEAdvertising *pAdvertising = BLEDevice::getAdvertising(); - pAdvertising->addServiceUUID(SERVICE_UUID); - pAdvertising->setScanResponse(true); - BLEDevice::startAdvertising(); - - Serial.println("[BLE] Advertising as 'Oliver_3'"); - Serial.println("\n=== GATEWAY READY ===\n"); -} - -void loop() { - static uint32_t lastRediscover = 0; - - // Handle manual re-discovery request - if (rediscoverRequested) { - rediscoverRequested = false; - Serial.println("\n[CMD] Manual re-discovery triggered!"); - for (int i = 0; i < NUM_SLAVES; i++) { - if (!slaveFound[i]) { - Serial.printf("[REDISCOVER] Will search for S%d\n", i); - } - } - discoverSlaves(15000); - } - - // Automatic re-discovery every 10 seconds for missing slaves - if (millis() - lastRediscover > 10000) { - lastRediscover = millis(); - int missingCount = 0; - for (int i = 0; i < NUM_SLAVES; i++) { - if (!slaveFound[i]) - missingCount++; - } - - if (missingCount > 0) { - Serial.printf("[AUTO-REDISCOVER] Searching for %d missing slaves...\n", - missingCount); - uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - for (int attempt = 0; attempt < 5; attempt++) { - uint8_t ping = 0xFF; - esp_now_send(broadcastAddr, &ping, 1); - delay(200); - } - } - } - - // ── On-demand read: triggered by BLE "READ" command from PC ── - if (readRequested) { - readRequested = false; - gatewayPacketIdx++; - - // 1. Read master's own encoder + diagnostics - double masterAngle = getUltraPrecisionReading(); - masterData.value = (int)(masterAngle * 10000.0); - masterData.packetIdx = gatewayPacketIdx; - - uint16_t diaagc = readRegister(DIAAGC_REG); - uint16_t mag = readRegister(MAG_REG); - masterData.agc = diaagc & 0xFF; - masterData.mag = mag & 0x3FFF; - masterData.magl = (diaagc >> 8) & 0x01; - masterData.magh = (diaagc >> 10) & 0x01; - masterData.cof = (diaagc >> 9) & 0x01; - - // 2. Request data from all discovered slaves - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i]) { - uint8_t req = i; - esp_now_send(slaveMACs[i], &req, 1); - } - } - - // 3. Wait for slave responses (up to 200ms timeout) - uint32_t waitStart = millis(); - while (millis() - waitStart < 200) { - bool allResponded = true; - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i] && !slaveResponded[i]) { - allResponded = false; - break; - } - } - if (allResponded) - break; - delay(1); - } - - // 4. Build BLE message (same 7-field format) - String bleMsg = String(gatewayPacketIdx); - - bleMsg += "|M0:" + String(masterData.value) + "," + - String(masterData.packetIdx) + "," + - String(masterData.agc) + "," + String(masterData.mag) + "," + - String(masterData.magl) + "," + String(masterData.magh) + "," + - String(masterData.cof); - - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i] && slaveResponded[i]) { - bleMsg += "|S" + String(i) + ":" + String(slaves[i].value) + "," + - String(slaves[i].packetIdx) + "," + - String(slaves[i].agc) + "," + String(slaves[i].mag) + "," + - String(slaves[i].magl) + "," + String(slaves[i].magh) + "," + - String(slaves[i].cof); - } else { - bleMsg += "|S" + String(i) + ":OFFLINE,0"; - } - } - - // 5. Send BLE notification - if (pcConnected && pChar) { - pChar->setValue(bleMsg.c_str()); - pChar->notify(); - } - - // 6. Serial log - Serial.printf("[READ #%u] M0=%d", gatewayPacketIdx, masterData.value); - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i] && slaveResponded[i]) - Serial.printf(" | S%d=%d", i, slaves[i].value); - else - Serial.printf(" | S%d=OFFLINE", i); - } - Serial.println(); - } - - delay(1); +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* + * Oliver Master Gateway - Robust Discovery Edition + * Hardware: XIAO ESP32C6 + * Features: + * - 30s blocking discovery for all 5 slaves + * - Continuous re-discovery for missing slaves + * - BLE command interface for manual re-discovery + * - Detailed status reporting + * Author: Swaraj Dangare + */ + +#define NUM_SLAVES 1 // FIXED: Was 4, now 5 +#define WIFI_CHANNEL 11 // ESP-NOW channel (use 1, 6, or 11 to isolate from other Oliver sets) +#define SERVICE_UUID "6ab88bb9-cf50-4564-b1c4-f53be2abc53f" +#define CHARACTERISTIC_UUID "1d4cd358-172d-4c33-b0b2-ddce9a071aab" +#define COMMAND_UUID "308a0c43-80f0-4b01-81e5-bb2798eb92f9" + +typedef struct __attribute__((packed)) { + uint8_t id; + int value; // Angle * 10000 + uint32_t packetIdx; + uint8_t agc; // AS5047D AGC value (0-255) + uint16_t mag; // AS5047D CORDIC magnitude (14-bit) + uint8_t magl; // Magnetic field too low (0 or 1) + uint8_t magh; // Magnetic field too high (0 or 1) + uint8_t cof; // CORDIC overflow (0 or 1) +} Payload; + +uint8_t slaveMACs[NUM_SLAVES][6]; +bool slaveFound[NUM_SLAVES] = {false}; +Payload slaves[NUM_SLAVES]; +uint32_t gatewayPacketIdx = 0; +uint32_t lastSeenTime[NUM_SLAVES] = {0}; // Track last response time + +BLECharacteristic *pChar; +BLECharacteristic *pCommandChar; +bool pcConnected = false; +bool rediscoverRequested = false; +bool readRequested = false; +bool slaveResponded[NUM_SLAVES] = {false}; + +// ---------------- MASTER ENCODER SETTINGS ---------------- +#define ANGLECOM 0x3FFF +#define DIAAGC_REG 0x3FFC +#define MAG_REG 0x3FFD +#define RD 0x40 +#define NUM_BLOCKS 16 +#define SAMPLES_PER_BLOCK 256 + +const int PIN_CS = D7; +const int PIN_SCK = D1; +const int PIN_MISO = D0; +const int PIN_MOSI = D10; + +SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); + +// Master encoder data +Payload masterData = {255, 0, 0, 0, 0, 0, 0, 0}; // id=255 for master + +// ESP-NOW Receive Callback +void onEspNowRecv(const esp_now_recv_info_t *info, const uint8_t *data, + int len) { + if (len == 1) { // Discovery Response + uint8_t id = data[0]; + if (id < NUM_SLAVES) { + if (!slaveFound[id]) { + memcpy(slaveMACs[id], info->src_addr, 6); + slaveFound[id] = true; + esp_now_peer_info_t peer{}; + memcpy(peer.peer_addr, info->src_addr, 6); + peer.channel = WIFI_CHANNEL; + peer.encrypt = false; + esp_now_add_peer(&peer); + // Safe to print here - quick message + Serial.printf( + "[DISCOVERY] Found Slave %d: %02X:%02X:%02X:%02X:%02X:%02X\n", id, + info->src_addr[0], info->src_addr[1], info->src_addr[2], + info->src_addr[3], info->src_addr[4], info->src_addr[5]); + } + lastSeenTime[id] = millis(); + } + } else if (len == sizeof(Payload)) { // Data Response + Payload p; + memcpy(&p, data, sizeof(p)); + if (p.id < NUM_SLAVES) { + slaves[p.id] = p; + lastSeenTime[p.id] = millis(); + slaveResponded[p.id] = true; + } + } +} + +// ---------------- MASTER ENCODER FUNCTIONS ---------------- +uint16_t evenParityBit(uint16_t x) { + x &= 0x7FFF; + return __builtin_parity(x); +} + +uint16_t makeReadCmd(uint16_t addr) { + uint16_t cmd = (1 << 14) | (addr & 0x3FFF); + cmd |= (evenParityBit(cmd) << 15); + return cmd; +} + +uint16_t AS5047D_Read() { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(PIN_CS, LOW); + SPI.transfer16(makeReadCmd(ANGLECOM)); + digitalWrite(PIN_CS, HIGH); + delayMicroseconds(1); + digitalWrite(PIN_CS, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(PIN_CS, HIGH); + SPI.endTransaction(); + return result; +} + +uint16_t readRegister(uint16_t addr) { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(PIN_CS, LOW); + SPI.transfer16(makeReadCmd(addr)); + digitalWrite(PIN_CS, HIGH); + delayMicroseconds(1); + digitalWrite(PIN_CS, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(PIN_CS, HIGH); + SPI.endTransaction(); + return result & 0x3FFF; +} + +double getRobustMean(uint16_t *samples, int size) { + double sum = 0; + for (int i = 0; i < size; i++) + sum += (samples[i] & 0x3FFF); + double initialMean = sum / size; + + double robustSum = 0; + int count = 0; + for (int i = 0; i < size; i++) { + uint16_t val = samples[i] & 0x3FFF; + if (abs((double)val - initialMean) < 1.5) { + robustSum += val; + count++; + } + } + return (count > 0) ? (robustSum / count) : initialMean; +} + +double getMedian(double *values, int size) { + std::sort(values, values + size); + return values[size / 2]; +} + +double getUltraPrecisionReading() { + double blockMeans[NUM_BLOCKS]; + uint16_t blockSamples[SAMPLES_PER_BLOCK]; + + for (int b = 0; b < NUM_BLOCKS; b++) { + for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { + uint16_t raw = AS5047D_Read(); + if (((raw >> 15) & 1) == evenParityBit(raw)) { + blockSamples[s] = raw; + } else { + s--; + } + } + blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); + delayMicroseconds(50); + } + double finalCounts = getMedian(blockMeans, NUM_BLOCKS); + return (finalCounts * 360.0) / 16384.0; +} + +// BLE Callbacks +class ServerCallbacks : public BLEServerCallbacks { + void onConnect(BLEServer *) { + pcConnected = true; + Serial.println("[BLE] Client Connected"); + } + void onDisconnect(BLEServer *) { + pcConnected = false; + Serial.println("[BLE] Client Disconnected"); + BLEDevice::startAdvertising(); + } +}; + +// Command Handler +class CommandCallbacks : public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *pChar) { + String value = + pChar->getValue().c_str(); // Convert std::string to Arduino String + if (value == "READ") { + readRequested = true; + for (int i = 0; i < NUM_SLAVES; i++) + slaveResponded[i] = false; + } else if (value == "REDISCOVER") { + rediscoverRequested = true; + Serial.println("[CMD] Re-discovery requested from PC"); + } + } +}; + +// Discovery Function +void discoverSlaves(uint32_t timeoutMs) { + uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + uint32_t startTime = millis(); + + Serial.printf( + "[DISCOVERY] Starting discovery for %d slaves (timeout: %dms)...\n", + NUM_SLAVES, timeoutMs); + + while (millis() - startTime < timeoutMs) { + // Count found slaves + int foundCount = 0; + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i]) + foundCount++; + } + + // Exit early if all found + if (foundCount == NUM_SLAVES) { + Serial.println("[DISCOVERY] All slaves found!"); + break; + } + + // Send broadcast + uint8_t ping = 0xFF; + esp_now_send(broadcastAddr, &ping, 1); + + // Print status every 2 seconds + static uint32_t lastPrint = 0; + if (millis() - lastPrint > 2000) { + lastPrint = millis(); + Serial.printf("[DISCOVERY] Progress: %d/%d slaves found | Missing: ", + foundCount, NUM_SLAVES); + for (int i = 0; i < NUM_SLAVES; i++) { + if (!slaveFound[i]) + Serial.printf("S%d ", i); + } + Serial.println(); + } + + delay(200); // Broadcast every 200ms (was 500ms) + } + + // Final report + int finalCount = 0; + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i]) + finalCount++; + } + + Serial.println("\n" + String('=', 50)); + Serial.printf("[DISCOVERY] Complete: %d/%d slaves discovered\n", finalCount, + NUM_SLAVES); + if (finalCount < NUM_SLAVES) { + Serial.print("[WARNING] Missing slaves: "); + for (int i = 0; i < NUM_SLAVES; i++) { + if (!slaveFound[i]) + Serial.printf("S%d ", i); + } + Serial.println("\n[INFO] Will retry in background..."); + } + Serial.println(String('=', 50) + "\n"); +} + +void setup() { + Serial.begin(115200); + delay(1000); // Give serial time to initialize + + Serial.println("\n\n=== OLIVER MASTER GATEWAY ==="); + Serial.println("Hardware: XIAO ESP32C6"); + Serial.printf("Firmware: Robust Discovery v2.0\n\n"); + + // WiFi Init + WiFi.mode(WIFI_STA); + WiFi.disconnect(); + Serial.printf("[WIFI] MAC Address: %s\n", WiFi.macAddress().c_str()); + + // Master Encoder SPI Init + pinMode(PIN_CS, OUTPUT); + digitalWrite(PIN_CS, HIGH); + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); + Serial.println("[SPI] Master encoder initialized"); + + // Force WiFi channel + esp_wifi_set_promiscuous(true); + esp_wifi_set_channel(WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE); + esp_wifi_set_promiscuous(false); + Serial.printf("[WIFI] Channel %d locked\n", WIFI_CHANNEL); + + // ESP-NOW Init + if (esp_now_init() != ESP_OK) { + Serial.println("[ERROR] ESP-NOW Init Failed!"); + return; + } + esp_now_register_recv_cb(onEspNowRecv); + Serial.println("[ESP-NOW] Initialized"); + + // Add Broadcast Peer + uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + esp_now_peer_info_t bcast{}; + memcpy(bcast.peer_addr, broadcastAddr, 6); + bcast.channel = WIFI_CHANNEL; + bcast.encrypt = false; + esp_now_add_peer(&bcast); + Serial.println("[ESP-NOW] Broadcast peer added\n"); + + // Initial Discovery (30 seconds, blocking) + discoverSlaves(30000); + + // BLE Init + Serial.println("[BLE] Initializing..."); + BLEDevice::init("Oliver_3"); + BLEServer *pServer = BLEDevice::createServer(); + pServer->setCallbacks(new ServerCallbacks()); + + BLEService *pService = pServer->createService(SERVICE_UUID); + + // Data characteristic (notify) + pChar = pService->createCharacteristic(CHARACTERISTIC_UUID, + BLECharacteristic::PROPERTY_NOTIFY); + pChar->addDescriptor(new BLE2902()); + + // Command characteristic (write) + pCommandChar = pService->createCharacteristic( + COMMAND_UUID, BLECharacteristic::PROPERTY_WRITE); + pCommandChar->setCallbacks(new CommandCallbacks()); + + pService->start(); + + BLEAdvertising *pAdvertising = BLEDevice::getAdvertising(); + pAdvertising->addServiceUUID(SERVICE_UUID); + pAdvertising->setScanResponse(true); + BLEDevice::startAdvertising(); + + Serial.println("[BLE] Advertising as 'Oliver_3'"); + Serial.println("\n=== GATEWAY READY ===\n"); +} + +void loop() { + static uint32_t lastRediscover = 0; + + // Handle manual re-discovery request + if (rediscoverRequested) { + rediscoverRequested = false; + Serial.println("\n[CMD] Manual re-discovery triggered!"); + for (int i = 0; i < NUM_SLAVES; i++) { + if (!slaveFound[i]) { + Serial.printf("[REDISCOVER] Will search for S%d\n", i); + } + } + discoverSlaves(15000); + } + + // Automatic re-discovery every 10 seconds for missing slaves + if (millis() - lastRediscover > 10000) { + lastRediscover = millis(); + int missingCount = 0; + for (int i = 0; i < NUM_SLAVES; i++) { + if (!slaveFound[i]) + missingCount++; + } + + if (missingCount > 0) { + Serial.printf("[AUTO-REDISCOVER] Searching for %d missing slaves...\n", + missingCount); + uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + for (int attempt = 0; attempt < 5; attempt++) { + uint8_t ping = 0xFF; + esp_now_send(broadcastAddr, &ping, 1); + delay(200); + } + } + } + + // ── On-demand read: triggered by BLE "READ" command from PC ── + if (readRequested) { + readRequested = false; + gatewayPacketIdx++; + + // 1. Read master's own encoder + diagnostics + double masterAngle = getUltraPrecisionReading(); + masterData.value = (int)(masterAngle * 10000.0); + masterData.packetIdx = gatewayPacketIdx; + + uint16_t diaagc = readRegister(DIAAGC_REG); + uint16_t mag = readRegister(MAG_REG); + masterData.agc = diaagc & 0xFF; + masterData.mag = mag & 0x3FFF; + masterData.magl = (diaagc >> 8) & 0x01; + masterData.magh = (diaagc >> 10) & 0x01; + masterData.cof = (diaagc >> 9) & 0x01; + + // 2. Request data from all discovered slaves + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i]) { + uint8_t req = i; + esp_now_send(slaveMACs[i], &req, 1); + } + } + + // 3. Wait for slave responses (up to 200ms timeout) + uint32_t waitStart = millis(); + while (millis() - waitStart < 200) { + bool allResponded = true; + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i] && !slaveResponded[i]) { + allResponded = false; + break; + } + } + if (allResponded) + break; + delay(1); + } + + // 4. Build BLE message (same 7-field format) + String bleMsg = String(gatewayPacketIdx); + + bleMsg += "|M0:" + String(masterData.value) + "," + + String(masterData.packetIdx) + "," + + String(masterData.agc) + "," + String(masterData.mag) + "," + + String(masterData.magl) + "," + String(masterData.magh) + "," + + String(masterData.cof); + + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i] && slaveResponded[i]) { + bleMsg += "|S" + String(i) + ":" + String(slaves[i].value) + "," + + String(slaves[i].packetIdx) + "," + + String(slaves[i].agc) + "," + String(slaves[i].mag) + "," + + String(slaves[i].magl) + "," + String(slaves[i].magh) + "," + + String(slaves[i].cof); + } else { + bleMsg += "|S" + String(i) + ":OFFLINE,0"; + } + } + + // 5. Send BLE notification + if (pcConnected && pChar) { + pChar->setValue(bleMsg.c_str()); + pChar->notify(); + } + + // 6. Serial log + Serial.printf("[READ #%u] M0=%d", gatewayPacketIdx, masterData.value); + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i] && slaveResponded[i]) + Serial.printf(" | S%d=%d", i, slaves[i].value); + else + Serial.printf(" | S%d=OFFLINE", i); + } + Serial.println(); + } + + delay(1); } \ No newline at end of file diff --git a/Oliver/oliver.py b/Oliver/oliver.py index 0259659..c311d1a 100644 --- a/Oliver/oliver.py +++ b/Oliver/oliver.py @@ -1,559 +1,559 @@ -""" -Oliver Encoder API -================== -Clean async API for reading encoder values from the Oliver BLE gateway. - -Usage: - import asyncio - from blade_offset_probe import OliverAPI - - async def main(): - probe = OliverAPI() - await probe.connect() - - # Request a fresh encoder reading on-demand; (M0_deg, S0_deg) - m0, s0 = await probe.get_instantaneous_encoder_values() - - # Take a precision measurement (collects 5 samples); returns (M0_median, S0_median) - m0_med, s0_med = await probe.get_encoder_values() - - # Set current position as zero reference - probe.set_zero() - - # Take another measurement (now relative to zero) - result = await probe.measure() - - # Disconnect cleanly - await probe.disconnect() - - asyncio.run(main()) - -Author: Swaraj Dangare -""" - -import asyncio -import time -import logging -import numpy as np -from dataclasses import dataclass, field -from typing import Optional -from bleak import BleakClient, BleakScanner - -# Set False to disable all prints and debug/info logging for the entire file. -VERBOSE = True - -# ── Logging ────────────────────────────────────────────────────────────────── -logger = logging.getLogger("oliver") -logger.setLevel(logging.DEBUG if VERBOSE else logging.WARNING) - -# ── BLE identifiers ───────────────────────────────────────────────────────── -DEVICE_NAME = "Oliver_3" -CHARACTERISTIC_UUID = "1d4cd358-172d-4c33-b0b2-ddce9a071aab" -COMMAND_UUID = "308a0c43-80f0-4b01-81e5-bb2798eb92f9" - -ENCODERS = ("M0", "S0") -SCAN_TIMEOUT_S = 15.0 -MEASURE_TIMEOUT_S = 30.0 -SAMPLES_PER_MEASUREMENT = 5 -REACH_MM = 183.0 # lever arm for µm precision calc - - -# ── Data classes ───────────────────────────────────────────────────────────── -@dataclass -class EncoderReading: - """Single encoder snapshot.""" - name: str - angle_deg: float - raw_angle_deg: float - pkt: int = 0 - agc: int = 0 - mag: int = 0 - status: str = "OK" - - -@dataclass -class MeasurementResult: - """Result of a multi-sample precision measurement.""" - measurement_id: int - encoders: dict = field(default_factory=dict) - timestamp: float = 0.0 - - # Per-encoder nested data - # encoders = { - # "M0": {"samples": [...], "median": float, "jitter": float, "precision_um": float, "status": str}, - # "S0": { ... }, - # } - - -# ── Packet parser ──────────────────────────────────────────────────────────── -def _parse_encoder_field(part: str, gateway_idx: int) -> tuple[str, Optional[tuple[float, dict]]]: - """ - Parse a single encoder field from the BLE payload. - Format: 'XX:angle,pkt[,agc,mag,magl,magh,cof]' - Returns (name, None) for OFFLINE encoders. - Returns (name, (angle_deg, metadata_dict)) on success. - """ - name_str, data = part.split(":") - fields = data.split(",") - - if fields[0] == "OFFLINE": - return name_str, None - - angle = float(fields[0]) / 10000.0 - pkt = int(fields[1]) if len(fields) > 1 else 0 - - if len(fields) >= 7: - agc, mag = int(fields[2]), int(fields[3]) - magl, magh, cof = fields[4], fields[5], fields[6] - else: - agc, mag, magl, magh, cof = 0, 0, "0", "0", "0" - - status = "OK" - if cof == "1": - status = "CORDIC_ERR" - elif magl == "1": - status = "FIELD_LOW" - elif magh == "1": - status = "FIELD_HIGH" - - meta = { - "g_idx": gateway_idx, "pkt": pkt, "agc": agc, "mag": mag, - "magl": magl, "magh": magh, "cof": cof, "status": status, - } - return name_str, (angle, meta) - - -# ── Main API class ─────────────────────────────────────────────────────────── -class OliverAPI: - """ - Async API for the Oliver BLE encoder gateway. - - All public methods are safe to call from any async context. - The object manages its own BLE connection lifecycle. - """ - - def __init__(self): - # Connection state - self._client: Optional[BleakClient] = None - self._device = None - self._connected = False - - # Live encoder data (updated on every BLE notification) - self._latest_raw: dict[str, Optional[float]] = {e: None for e in ENCODERS} - self._latest_meta: dict[str, dict] = {e: {} for e in ENCODERS} - self._last_gateway_idx = -1 - self._missed_packets = 0 - self._last_notify_time: float = 0.0 - - # Notification event (set on every _on_notify, used for on-demand reads) - self._notify_received: asyncio.Event = asyncio.Event() - - # Zero offsets - self._zero_offsets: dict[str, float] = {e: 0.0 for e in ENCODERS} - - # Measurement collection state - self._measurement_id = 0 - self._collecting = False - self._collect_samples: dict[str, list[float]] = {e: [] for e in ENCODERS} - self._collect_meta: dict[str, list[dict]] = {e: [] for e in ENCODERS} - self._collect_done: Optional[asyncio.Event] = None - - # Timing (seconds) of last connect and last get_encoder_values / get_instantaneous call - self._last_connect_time_s: float = 0.0 - self._last_get_encoder_values_time_s: float = 0.0 - self._last_get_instantaneous_encoder_values_time_s: float = 0.0 - - # ── Properties ─────────────────────────────────────────────────────── - - @property - def is_connected(self) -> bool: - """True when BLE link is active.""" - return self._connected and self._client is not None and self._client.is_connected - - @property - def missed_packets(self) -> int: - return self._missed_packets - - @property - def last_gateway_idx(self) -> int: - return self._last_gateway_idx - - @property - def last_connect_time_s(self) -> float: - """Time in seconds taken by the last connect() (or reconnect()) that did a full connect.""" - return self._last_connect_time_s - - @property - def last_get_encoder_values_time_s(self) -> float: - """Time in seconds taken by the last get_encoder_values() call from start to return.""" - return self._last_get_encoder_values_time_s - - @property - def last_get_instantaneous_encoder_values_time_s(self) -> float: - """Time in seconds taken by the last get_instantaneous_encoder_values() call from start to return.""" - return self._last_get_instantaneous_encoder_values_time_s - - # ── Connect / Disconnect / Reconnect ───────────────────────────────── - - async def connect(self, timeout: float = SCAN_TIMEOUT_S) -> bool: - """ - Scan for the gateway and connect. - Returns True on success, False if the device was not found. - """ - if self.is_connected: - logger.info("Already connected.") - return True - - t0 = time.perf_counter() - logger.info("Scanning for %s …", DEVICE_NAME) - self._device = await BleakScanner.find_device_by_filter( - lambda d, _ad: d.name == DEVICE_NAME, timeout=timeout, - ) - if self._device is None: - logger.error("Gateway not found (scanned for %.0fs).", timeout) - return False - - logger.info("Found %s (%s). Connecting …", DEVICE_NAME, self._device.address) - self._client = BleakClient(self._device, disconnected_callback=self._on_disconnect) - await self._client.connect() - await self._client.start_notify(CHARACTERISTIC_UUID, self._on_notify) - self._connected = True - self._last_connect_time_s = time.perf_counter() - t0 - logger.info("Connected to %s (%.2f s).", DEVICE_NAME, self._last_connect_time_s) - return True - - async def disconnect(self): - """Cleanly close the BLE connection.""" - if self._client is not None: - try: - await self._client.disconnect() - except Exception: - pass - self._connected = False - logger.info("Disconnected.") - - async def reconnect(self, timeout: float = SCAN_TIMEOUT_S) -> bool: - """ - Drop existing connection (if any) and reconnect from scratch. - Returns True on success. - """ - logger.info("Reconnecting …") - await self.disconnect() - await asyncio.sleep(1.0) # give BLE stack time to clean up - return await self.connect(timeout=timeout) - - def _on_disconnect(self, _client): - self._connected = False - logger.warning("BLE disconnected unexpectedly.") - - # ── Live encoder values ────────────────────────────────────────────── - - async def get_instantaneous_encoder_values(self) -> tuple[Optional[float], Optional[float]]: - """ - Request a fresh encoder reading from the gateway and return - (M0, S0) angles in degrees (zero-adjusted). - - Sends a "READ" command and waits for the notification response. - If not connected to BLE, reconnects first. Returns (None, None) if reconnect fails. - """ - t0 = time.perf_counter() - if not self.is_connected: - ok = await self.reconnect() - if not ok: - self._last_get_instantaneous_encoder_values_time_s = time.perf_counter() - t0 - return (None, None) - - self._notify_received.clear() - await self.send_command("READ") - try: - await asyncio.wait_for(self._notify_received.wait(), timeout=5.0) - except asyncio.TimeoutError: - logger.warning("READ response timed out.") - self._last_get_instantaneous_encoder_values_time_s = time.perf_counter() - t0 - return (None, None) - - m0_raw = self._latest_raw["M0"] - s0_raw = self._latest_raw["S0"] - m0 = (m0_raw - self._zero_offsets["M0"]) if m0_raw is not None else None - s0 = (s0_raw - self._zero_offsets["S0"]) if s0_raw is not None else None - self._last_get_instantaneous_encoder_values_time_s = time.perf_counter() - t0 - return (m0, s0) - - # ── Precision measurement ──────────────────────────────────────────── - - async def get_encoder_values( - self, - num_samples: int = SAMPLES_PER_MEASUREMENT, - timeout: float = MEASURE_TIMEOUT_S, - ) -> tuple[Optional[float], Optional[float]]: - """ - Collect *num_samples* on-demand readings from every encoder, - compute medians, and return (M0_median_deg, S0_median_deg). - - Sends a "READ" command for each sample and waits for the response. - If not connected to BLE, reconnects first. Returns (None, None) if reconnect fails. - Raises TimeoutError if samples are not received in time. - """ - t0 = time.perf_counter() - if not self.is_connected: - ok = await self.reconnect() - if not ok: - self._last_get_encoder_values_time_s = time.perf_counter() - t0 - return (None, None) - - self._measurement_id += 1 - mid = self._measurement_id - - # Prepare collection buffers - self._collect_samples = {e: [] for e in ENCODERS} - self._collect_meta = {e: [] for e in ENCODERS} - self._collecting = True - per_sample_timeout = timeout / num_samples - - logger.info("Measurement #%d: collecting %d samples …", mid, num_samples) - - try: - for i in range(num_samples): - self._notify_received.clear() - await self.send_command("READ") - try: - await asyncio.wait_for(self._notify_received.wait(), timeout=per_sample_timeout) - except asyncio.TimeoutError: - self._collecting = False - self._last_get_encoder_values_time_s = time.perf_counter() - t0 - raise TimeoutError( - f"Measurement #{mid} timed out at sample {i+1}/{num_samples} " - f"after {time.perf_counter() - t0:.1f}s." - ) - finally: - self._collecting = False - - # Build result - result = MeasurementResult(measurement_id=mid, timestamp=time.time()) - for enc in ENCODERS: - samples = self._collect_samples[enc] - enc_data: dict = {"samples": samples, "count": len(samples)} - - if len(samples) >= 1: - median = float(np.median(samples)) - jitter = (max(samples) - min(samples)) if len(samples) >= 2 else 0.0 - precision_um = REACH_MM * (jitter * np.pi / 180.0) * 1000.0 - enc_data.update(median=median, jitter=jitter, precision_um=precision_um) - else: - enc_data.update(median=None, jitter=None, precision_um=None) - - # Attach last known status - metas = self._collect_meta[enc] - enc_data["status"] = metas[-1].get("status", "OK") if metas else "NO_DATA" - result.encoders[enc] = enc_data - - self._last_get_encoder_values_time_s = time.perf_counter() - t0 - logger.info("Measurement #%d complete (%.2f s).", mid, self._last_get_encoder_values_time_s) - m0_median = result.encoders["M0"].get("median") - s0_median = result.encoders["S0"].get("median") - return (m0_median, s0_median) - - # ── Zero reference ─────────────────────────────────────────────────── - - def set_zero(self) -> dict[str, float]: - """ - Capture the current raw angles as the zero reference. - All subsequent get_encoder_values() and measure() calls will be - relative to this position. - - Returns a dict of the offsets that were applied, e.g.: - {"M0": 166.990, "S0": 125.395} - - Raises RuntimeError if no data has been received yet. - """ - applied: dict[str, float] = {} - for enc in ENCODERS: - raw = self._latest_raw[enc] - if raw is not None: - self._zero_offsets[enc] = raw - applied[enc] = raw - - if not applied: - raise RuntimeError("No encoder data received yet — cannot set zero.") - - logger.info("Zero set: %s", {k: f"{v:.5f}°" for k, v in applied.items()}) - return applied - - def clear_zero(self): - """Remove any zero offset (revert to absolute angles).""" - self._zero_offsets = {e: 0.0 for e in ENCODERS} - logger.info("Zero offsets cleared.") - - # ── Send BLE command to gateway ────────────────────────────────────── - - async def send_command(self, cmd: str) -> bool: - """ - Send an arbitrary string command to the gateway's command characteristic. - Returns True on success. - """ - if not self.is_connected: - raise RuntimeError("Not connected.") - try: - await self._client.write_gatt_char(COMMAND_UUID, cmd.encode()) - logger.info("Sent command: %s", cmd) - return True - except Exception as exc: - logger.error("Command failed: %s", exc) - return False - - # ── Internal BLE notification handler ──────────────────────────────── - - def _on_notify(self, _sender, data: bytearray): - """Called by bleak on every BLE notification.""" - try: - payload = data.decode() - parts = payload.split("|") - g_idx = int(parts[0]) - - # Track missed packets - if self._last_gateway_idx != -1 and g_idx - self._last_gateway_idx > 1: - self._missed_packets += g_idx - self._last_gateway_idx - 1 - self._last_gateway_idx = g_idx - self._last_notify_time = time.time() - - # ── Parse Master (M0) ── - _, m_result = _parse_encoder_field(parts[1], g_idx) - if m_result is not None: - raw_ang, meta = m_result - self._latest_raw["M0"] = raw_ang - self._latest_meta["M0"] = meta - - # ── Parse Slave (S0) ── - s_result = None - if len(parts) > 2: - _, s_result = _parse_encoder_field(parts[2], g_idx) - if s_result is not None: - raw_ang, meta = s_result - self._latest_raw["S0"] = raw_ang - self._latest_meta["S0"] = meta - - # ── Collecting samples for a measurement? ── - if self._collecting: - for enc, result in [("M0", m_result), ("S0", s_result)]: - if result is None: - continue - raw_ang, meta = result - adj = raw_ang - self._zero_offsets[enc] - if enc == "M0" and meta["status"] == "OK": - meta["status"] = "MASTER" - self._collect_samples[enc].append(adj) - self._collect_meta[enc].append(meta) - - # Signal that a notification was received - self._notify_received.set() - - except Exception as exc: - logger.debug("Parse error: %s | payload=%s", exc, data) - - -# ── Convenience: run interactively if executed directly ────────────────────── -async def _interactive_demo(): - """Quick interactive session for testing.""" - logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(name)s] %(message)s") - - probe = OliverAPI() - ok = await probe.connect() - if not ok: - if VERBOSE: - print("✗ Could not find gateway.") - return - - if VERBOSE: - print("\n" + "=" * 70) - print(" Oliver API — Interactive Demo") - print(" Commands: Enter = measure | z = set zero | v = instantaneous") - print(" c = connect | d = disconnect | r = reconnect | q = quit") - print("=" * 70 + "\n") - - import sys, select - - try: - if sys.platform != "win32": - import tty, termios - old_settings = termios.tcgetattr(sys.stdin) - tty.setcbreak(sys.stdin.fileno()) - else: - old_settings = None - - while True: - # Non-blocking key check - if sys.platform != "win32" and select.select([sys.stdin], [], [], 0)[0]: - key = sys.stdin.read(1) - else: - key = None - - if key: - if key.lower() == "q": - break - - elif key.lower() == "z": - try: - offsets = probe.set_zero() - if VERBOSE: - print(f"✓ Zero set: {offsets}") - except RuntimeError as e: - if VERBOSE: - print(f"✗ {e}") - - elif key.lower() == "c": - ok = await probe.connect() - if VERBOSE: - print("✓ Connected." if ok else "✗ Connect failed.") - - elif key.lower() == "d": - await probe.disconnect() - if VERBOSE: - print("✓ Disconnected.") - - elif key.lower() == "r": - ok = await probe.reconnect() - if VERBOSE: - print("✓ Reconnected." if ok else "✗ Reconnect failed.") - - elif key == "\n": - try: - m0_med, s0_med = await probe.get_encoder_values() - if VERBOSE: - print(f"\n{'=' * 80}") - print(" Measurement (M0_median, S0_median)") - print(f"{'=' * 80}") - if m0_med is not None: - print(f" M0 {m0_med:12.5f}°") - else: - print(" M0 no data") - if s0_med is not None: - print(f" S0 {s0_med:12.5f}°") - else: - print(" S0 no data") - print(f"{'=' * 80}\n") - except TimeoutError as e: - if VERBOSE: - print(f"✗ {e}") - - elif key == "v": - m0, s0 = await probe.get_instantaneous_encoder_values() - if VERBOSE: - print(f" M0: {m0:.5f}°" if m0 is not None else " M0: no data") - print(f" S0: {s0:.5f}°" if s0 is not None else " S0: no data") - print(f" (%.3f s)" % probe.last_get_instantaneous_encoder_values_time_s) - - await asyncio.sleep(0.1) - - except KeyboardInterrupt: - pass - finally: - if sys.platform != "win32" and old_settings: - import termios - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) - await probe.disconnect() - if VERBOSE: - print("\nDone.") - - -if __name__ == "__main__": - asyncio.run(_interactive_demo()) +""" +Oliver Encoder API +================== +Clean async API for reading encoder values from the Oliver BLE gateway. + +Usage: + import asyncio + from blade_offset_probe import OliverAPI + + async def main(): + probe = OliverAPI() + await probe.connect() + + # Request a fresh encoder reading on-demand; (M0_deg, S0_deg) + m0, s0 = await probe.get_instantaneous_encoder_values() + + # Take a precision measurement (collects 5 samples); returns (M0_median, S0_median) + m0_med, s0_med = await probe.get_encoder_values() + + # Set current position as zero reference + probe.set_zero() + + # Take another measurement (now relative to zero) + result = await probe.measure() + + # Disconnect cleanly + await probe.disconnect() + + asyncio.run(main()) + +Author: Swaraj Dangare +""" + +import asyncio +import time +import logging +import numpy as np +from dataclasses import dataclass, field +from typing import Optional +from bleak import BleakClient, BleakScanner + +# Set False to disable all prints and debug/info logging for the entire file. +VERBOSE = True + +# ── Logging ────────────────────────────────────────────────────────────────── +logger = logging.getLogger("oliver") +logger.setLevel(logging.DEBUG if VERBOSE else logging.WARNING) + +# ── BLE identifiers ───────────────────────────────────────────────────────── +DEVICE_NAME = "Oliver_3" +CHARACTERISTIC_UUID = "1d4cd358-172d-4c33-b0b2-ddce9a071aab" +COMMAND_UUID = "308a0c43-80f0-4b01-81e5-bb2798eb92f9" + +ENCODERS = ("M0", "S0") +SCAN_TIMEOUT_S = 15.0 +MEASURE_TIMEOUT_S = 30.0 +SAMPLES_PER_MEASUREMENT = 5 +REACH_MM = 183.0 # lever arm for µm precision calc + + +# ── Data classes ───────────────────────────────────────────────────────────── +@dataclass +class EncoderReading: + """Single encoder snapshot.""" + name: str + angle_deg: float + raw_angle_deg: float + pkt: int = 0 + agc: int = 0 + mag: int = 0 + status: str = "OK" + + +@dataclass +class MeasurementResult: + """Result of a multi-sample precision measurement.""" + measurement_id: int + encoders: dict = field(default_factory=dict) + timestamp: float = 0.0 + + # Per-encoder nested data + # encoders = { + # "M0": {"samples": [...], "median": float, "jitter": float, "precision_um": float, "status": str}, + # "S0": { ... }, + # } + + +# ── Packet parser ──────────────────────────────────────────────────────────── +def _parse_encoder_field(part: str, gateway_idx: int) -> tuple[str, Optional[tuple[float, dict]]]: + """ + Parse a single encoder field from the BLE payload. + Format: 'XX:angle,pkt[,agc,mag,magl,magh,cof]' + Returns (name, None) for OFFLINE encoders. + Returns (name, (angle_deg, metadata_dict)) on success. + """ + name_str, data = part.split(":") + fields = data.split(",") + + if fields[0] == "OFFLINE": + return name_str, None + + angle = float(fields[0]) / 10000.0 + pkt = int(fields[1]) if len(fields) > 1 else 0 + + if len(fields) >= 7: + agc, mag = int(fields[2]), int(fields[3]) + magl, magh, cof = fields[4], fields[5], fields[6] + else: + agc, mag, magl, magh, cof = 0, 0, "0", "0", "0" + + status = "OK" + if cof == "1": + status = "CORDIC_ERR" + elif magl == "1": + status = "FIELD_LOW" + elif magh == "1": + status = "FIELD_HIGH" + + meta = { + "g_idx": gateway_idx, "pkt": pkt, "agc": agc, "mag": mag, + "magl": magl, "magh": magh, "cof": cof, "status": status, + } + return name_str, (angle, meta) + + +# ── Main API class ─────────────────────────────────────────────────────────── +class OliverAPI: + """ + Async API for the Oliver BLE encoder gateway. + + All public methods are safe to call from any async context. + The object manages its own BLE connection lifecycle. + """ + + def __init__(self): + # Connection state + self._client: Optional[BleakClient] = None + self._device = None + self._connected = False + + # Live encoder data (updated on every BLE notification) + self._latest_raw: dict[str, Optional[float]] = {e: None for e in ENCODERS} + self._latest_meta: dict[str, dict] = {e: {} for e in ENCODERS} + self._last_gateway_idx = -1 + self._missed_packets = 0 + self._last_notify_time: float = 0.0 + + # Notification event (set on every _on_notify, used for on-demand reads) + self._notify_received: asyncio.Event = asyncio.Event() + + # Zero offsets + self._zero_offsets: dict[str, float] = {e: 0.0 for e in ENCODERS} + + # Measurement collection state + self._measurement_id = 0 + self._collecting = False + self._collect_samples: dict[str, list[float]] = {e: [] for e in ENCODERS} + self._collect_meta: dict[str, list[dict]] = {e: [] for e in ENCODERS} + self._collect_done: Optional[asyncio.Event] = None + + # Timing (seconds) of last connect and last get_encoder_values / get_instantaneous call + self._last_connect_time_s: float = 0.0 + self._last_get_encoder_values_time_s: float = 0.0 + self._last_get_instantaneous_encoder_values_time_s: float = 0.0 + + # ── Properties ─────────────────────────────────────────────────────── + + @property + def is_connected(self) -> bool: + """True when BLE link is active.""" + return self._connected and self._client is not None and self._client.is_connected + + @property + def missed_packets(self) -> int: + return self._missed_packets + + @property + def last_gateway_idx(self) -> int: + return self._last_gateway_idx + + @property + def last_connect_time_s(self) -> float: + """Time in seconds taken by the last connect() (or reconnect()) that did a full connect.""" + return self._last_connect_time_s + + @property + def last_get_encoder_values_time_s(self) -> float: + """Time in seconds taken by the last get_encoder_values() call from start to return.""" + return self._last_get_encoder_values_time_s + + @property + def last_get_instantaneous_encoder_values_time_s(self) -> float: + """Time in seconds taken by the last get_instantaneous_encoder_values() call from start to return.""" + return self._last_get_instantaneous_encoder_values_time_s + + # ── Connect / Disconnect / Reconnect ───────────────────────────────── + + async def connect(self, timeout: float = SCAN_TIMEOUT_S) -> bool: + """ + Scan for the gateway and connect. + Returns True on success, False if the device was not found. + """ + if self.is_connected: + logger.info("Already connected.") + return True + + t0 = time.perf_counter() + logger.info("Scanning for %s …", DEVICE_NAME) + self._device = await BleakScanner.find_device_by_filter( + lambda d, _ad: d.name == DEVICE_NAME, timeout=timeout, + ) + if self._device is None: + logger.error("Gateway not found (scanned for %.0fs).", timeout) + return False + + logger.info("Found %s (%s). Connecting …", DEVICE_NAME, self._device.address) + self._client = BleakClient(self._device, disconnected_callback=self._on_disconnect) + await self._client.connect() + await self._client.start_notify(CHARACTERISTIC_UUID, self._on_notify) + self._connected = True + self._last_connect_time_s = time.perf_counter() - t0 + logger.info("Connected to %s (%.2f s).", DEVICE_NAME, self._last_connect_time_s) + return True + + async def disconnect(self): + """Cleanly close the BLE connection.""" + if self._client is not None: + try: + await self._client.disconnect() + except Exception: + pass + self._connected = False + logger.info("Disconnected.") + + async def reconnect(self, timeout: float = SCAN_TIMEOUT_S) -> bool: + """ + Drop existing connection (if any) and reconnect from scratch. + Returns True on success. + """ + logger.info("Reconnecting …") + await self.disconnect() + await asyncio.sleep(1.0) # give BLE stack time to clean up + return await self.connect(timeout=timeout) + + def _on_disconnect(self, _client): + self._connected = False + logger.warning("BLE disconnected unexpectedly.") + + # ── Live encoder values ────────────────────────────────────────────── + + async def get_instantaneous_encoder_values(self) -> tuple[Optional[float], Optional[float]]: + """ + Request a fresh encoder reading from the gateway and return + (M0, S0) angles in degrees (zero-adjusted). + + Sends a "READ" command and waits for the notification response. + If not connected to BLE, reconnects first. Returns (None, None) if reconnect fails. + """ + t0 = time.perf_counter() + if not self.is_connected: + ok = await self.reconnect() + if not ok: + self._last_get_instantaneous_encoder_values_time_s = time.perf_counter() - t0 + return (None, None) + + self._notify_received.clear() + await self.send_command("READ") + try: + await asyncio.wait_for(self._notify_received.wait(), timeout=5.0) + except asyncio.TimeoutError: + logger.warning("READ response timed out.") + self._last_get_instantaneous_encoder_values_time_s = time.perf_counter() - t0 + return (None, None) + + m0_raw = self._latest_raw["M0"] + s0_raw = self._latest_raw["S0"] + m0 = (m0_raw - self._zero_offsets["M0"]) if m0_raw is not None else None + s0 = (s0_raw - self._zero_offsets["S0"]) if s0_raw is not None else None + self._last_get_instantaneous_encoder_values_time_s = time.perf_counter() - t0 + return (m0, s0) + + # ── Precision measurement ──────────────────────────────────────────── + + async def get_encoder_values( + self, + num_samples: int = SAMPLES_PER_MEASUREMENT, + timeout: float = MEASURE_TIMEOUT_S, + ) -> tuple[Optional[float], Optional[float]]: + """ + Collect *num_samples* on-demand readings from every encoder, + compute medians, and return (M0_median_deg, S0_median_deg). + + Sends a "READ" command for each sample and waits for the response. + If not connected to BLE, reconnects first. Returns (None, None) if reconnect fails. + Raises TimeoutError if samples are not received in time. + """ + t0 = time.perf_counter() + if not self.is_connected: + ok = await self.reconnect() + if not ok: + self._last_get_encoder_values_time_s = time.perf_counter() - t0 + return (None, None) + + self._measurement_id += 1 + mid = self._measurement_id + + # Prepare collection buffers + self._collect_samples = {e: [] for e in ENCODERS} + self._collect_meta = {e: [] for e in ENCODERS} + self._collecting = True + per_sample_timeout = timeout / num_samples + + logger.info("Measurement #%d: collecting %d samples …", mid, num_samples) + + try: + for i in range(num_samples): + self._notify_received.clear() + await self.send_command("READ") + try: + await asyncio.wait_for(self._notify_received.wait(), timeout=per_sample_timeout) + except asyncio.TimeoutError: + self._collecting = False + self._last_get_encoder_values_time_s = time.perf_counter() - t0 + raise TimeoutError( + f"Measurement #{mid} timed out at sample {i+1}/{num_samples} " + f"after {time.perf_counter() - t0:.1f}s." + ) + finally: + self._collecting = False + + # Build result + result = MeasurementResult(measurement_id=mid, timestamp=time.time()) + for enc in ENCODERS: + samples = self._collect_samples[enc] + enc_data: dict = {"samples": samples, "count": len(samples)} + + if len(samples) >= 1: + median = float(np.median(samples)) + jitter = (max(samples) - min(samples)) if len(samples) >= 2 else 0.0 + precision_um = REACH_MM * (jitter * np.pi / 180.0) * 1000.0 + enc_data.update(median=median, jitter=jitter, precision_um=precision_um) + else: + enc_data.update(median=None, jitter=None, precision_um=None) + + # Attach last known status + metas = self._collect_meta[enc] + enc_data["status"] = metas[-1].get("status", "OK") if metas else "NO_DATA" + result.encoders[enc] = enc_data + + self._last_get_encoder_values_time_s = time.perf_counter() - t0 + logger.info("Measurement #%d complete (%.2f s).", mid, self._last_get_encoder_values_time_s) + m0_median = result.encoders["M0"].get("median") + s0_median = result.encoders["S0"].get("median") + return (m0_median, s0_median) + + # ── Zero reference ─────────────────────────────────────────────────── + + def set_zero(self) -> dict[str, float]: + """ + Capture the current raw angles as the zero reference. + All subsequent get_encoder_values() and measure() calls will be + relative to this position. + + Returns a dict of the offsets that were applied, e.g.: + {"M0": 166.990, "S0": 125.395} + + Raises RuntimeError if no data has been received yet. + """ + applied: dict[str, float] = {} + for enc in ENCODERS: + raw = self._latest_raw[enc] + if raw is not None: + self._zero_offsets[enc] = raw + applied[enc] = raw + + if not applied: + raise RuntimeError("No encoder data received yet — cannot set zero.") + + logger.info("Zero set: %s", {k: f"{v:.5f}°" for k, v in applied.items()}) + return applied + + def clear_zero(self): + """Remove any zero offset (revert to absolute angles).""" + self._zero_offsets = {e: 0.0 for e in ENCODERS} + logger.info("Zero offsets cleared.") + + # ── Send BLE command to gateway ────────────────────────────────────── + + async def send_command(self, cmd: str) -> bool: + """ + Send an arbitrary string command to the gateway's command characteristic. + Returns True on success. + """ + if not self.is_connected: + raise RuntimeError("Not connected.") + try: + await self._client.write_gatt_char(COMMAND_UUID, cmd.encode()) + logger.info("Sent command: %s", cmd) + return True + except Exception as exc: + logger.error("Command failed: %s", exc) + return False + + # ── Internal BLE notification handler ──────────────────────────────── + + def _on_notify(self, _sender, data: bytearray): + """Called by bleak on every BLE notification.""" + try: + payload = data.decode() + parts = payload.split("|") + g_idx = int(parts[0]) + + # Track missed packets + if self._last_gateway_idx != -1 and g_idx - self._last_gateway_idx > 1: + self._missed_packets += g_idx - self._last_gateway_idx - 1 + self._last_gateway_idx = g_idx + self._last_notify_time = time.time() + + # ── Parse Master (M0) ── + _, m_result = _parse_encoder_field(parts[1], g_idx) + if m_result is not None: + raw_ang, meta = m_result + self._latest_raw["M0"] = raw_ang + self._latest_meta["M0"] = meta + + # ── Parse Slave (S0) ── + s_result = None + if len(parts) > 2: + _, s_result = _parse_encoder_field(parts[2], g_idx) + if s_result is not None: + raw_ang, meta = s_result + self._latest_raw["S0"] = raw_ang + self._latest_meta["S0"] = meta + + # ── Collecting samples for a measurement? ── + if self._collecting: + for enc, result in [("M0", m_result), ("S0", s_result)]: + if result is None: + continue + raw_ang, meta = result + adj = raw_ang - self._zero_offsets[enc] + if enc == "M0" and meta["status"] == "OK": + meta["status"] = "MASTER" + self._collect_samples[enc].append(adj) + self._collect_meta[enc].append(meta) + + # Signal that a notification was received + self._notify_received.set() + + except Exception as exc: + logger.debug("Parse error: %s | payload=%s", exc, data) + + +# ── Convenience: run interactively if executed directly ────────────────────── +async def _interactive_demo(): + """Quick interactive session for testing.""" + logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(name)s] %(message)s") + + probe = OliverAPI() + ok = await probe.connect() + if not ok: + if VERBOSE: + print("✗ Could not find gateway.") + return + + if VERBOSE: + print("\n" + "=" * 70) + print(" Oliver API — Interactive Demo") + print(" Commands: Enter = measure | z = set zero | v = instantaneous") + print(" c = connect | d = disconnect | r = reconnect | q = quit") + print("=" * 70 + "\n") + + import sys, select + + try: + if sys.platform != "win32": + import tty, termios + old_settings = termios.tcgetattr(sys.stdin) + tty.setcbreak(sys.stdin.fileno()) + else: + old_settings = None + + while True: + # Non-blocking key check + if sys.platform != "win32" and select.select([sys.stdin], [], [], 0)[0]: + key = sys.stdin.read(1) + else: + key = None + + if key: + if key.lower() == "q": + break + + elif key.lower() == "z": + try: + offsets = probe.set_zero() + if VERBOSE: + print(f"✓ Zero set: {offsets}") + except RuntimeError as e: + if VERBOSE: + print(f"✗ {e}") + + elif key.lower() == "c": + ok = await probe.connect() + if VERBOSE: + print("✓ Connected." if ok else "✗ Connect failed.") + + elif key.lower() == "d": + await probe.disconnect() + if VERBOSE: + print("✓ Disconnected.") + + elif key.lower() == "r": + ok = await probe.reconnect() + if VERBOSE: + print("✓ Reconnected." if ok else "✗ Reconnect failed.") + + elif key == "\n": + try: + m0_med, s0_med = await probe.get_encoder_values() + if VERBOSE: + print(f"\n{'=' * 80}") + print(" Measurement (M0_median, S0_median)") + print(f"{'=' * 80}") + if m0_med is not None: + print(f" M0 {m0_med:12.5f}°") + else: + print(" M0 no data") + if s0_med is not None: + print(f" S0 {s0_med:12.5f}°") + else: + print(" S0 no data") + print(f"{'=' * 80}\n") + except TimeoutError as e: + if VERBOSE: + print(f"✗ {e}") + + elif key == "v": + m0, s0 = await probe.get_instantaneous_encoder_values() + if VERBOSE: + print(f" M0: {m0:.5f}°" if m0 is not None else " M0: no data") + print(f" S0: {s0:.5f}°" if s0 is not None else " S0: no data") + print(f" (%.3f s)" % probe.last_get_instantaneous_encoder_values_time_s) + + await asyncio.sleep(0.1) + + except KeyboardInterrupt: + pass + finally: + if sys.platform != "win32" and old_settings: + import termios + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) + await probe.disconnect() + if VERBOSE: + print("\nDone.") + + +if __name__ == "__main__": + asyncio.run(_interactive_demo()) diff --git a/Oliver/oliver_raw.py b/Oliver/oliver_raw.py index 574b574..aeeaa5e 100644 --- a/Oliver/oliver_raw.py +++ b/Oliver/oliver_raw.py @@ -1,310 +1,310 @@ -""" -Author: Swaraj Dangare -""" -import asyncio -import time -import sys -import select -import numpy as np -from collections import deque -from bleak import BleakClient, BleakScanner - -# BLE configuration -DEVICE_NAME = "Oliver_3" -CHARACTERISTIC_UUID = "1d4cd358-172d-4c33-b0b2-ddce9a071aab" -COMMAND_UUID = "308a0c43-80f0-4b01-81e5-bb2798eb92f9" - - -class GatewayInterface: - def __init__(self): - self.start_time = time.time() - self.last_g_idx = -1 - self.missed_packets = 0 - self.client = None - - # Manual sampling mode - self.manual_mode = True - self.samples_per_measurement = 5 - self.current_samples = { - 'M0': [], 'S0': [] - } - self.sample_metadata = { - 'M0': [], 'S0': [] - } - self.collecting = False - self.measurement_id = 0 - - # Zero offset: subtract this from raw angles to get relative values - self.zero_offsets = {'M0': 0.0, 'S0': 0.0} - # Track the latest raw angle from each encoder (updated every packet) - self.latest_raw = {'M0': None, 'S0': None} - - # Legacy variance tracking (kept for reference) - self.sample_size = 100 - self.encoder_samples = { - 'M0': deque(maxlen=self.sample_size), - 'S0': deque(maxlen=self.sample_size), - } - - def set_zero(self): - """Set current encoder positions as the zero reference""" - zeroed = [] - for enc in ['M0', 'S0']: - if self.latest_raw[enc] is not None: - self.zero_offsets[enc] = self.latest_raw[enc] - zeroed.append(f"{enc}={self.latest_raw[enc]:.5f}°") - if zeroed: - print(f"\n✓ ZERO set: {', '.join(zeroed)}") - print(" All future measurements will be relative to this position.") - else: - print("\n✗ No encoder data received yet — cannot set zero.") - print() - - def variance(self, key): - vals = list(self.encoder_samples[key]) - return np.var(vals) if len(vals) > 1 else 0.0 - - def start_measurement(self): - """Initialize a new measurement session""" - self.measurement_id += 1 - self.collecting = True - self.samples_collected = False - for key in self.current_samples: - self.current_samples[key] = [] - self.sample_metadata[key] = [] - print("\nCollecting 5 samples", end="", flush=True) - - def calculate_precision_metrics(self, samples): - """Calculate median, jitter, and precision from samples""" - if len(samples) < 3: - return None, None, None - - median_angle = np.median(samples) - jitter = max(samples) - min(samples) - # Assuming 183mm reach (from reference script) - precision_um = 183 * (jitter * np.pi / 180.0) * 1000 - return median_angle, jitter, precision_um - - def display_results(self): - """Display measurement results with precision metrics""" - print("\n\n" + "="*130) - print(f"=== OLIVER PRECISION MEASUREMENT #{self.measurement_id} | Uptime {int(time.time() - self.start_time)}s ===") - print("="*130) - print( - f"{'ENC':<4} | {'MEDIAN (deg)':>12} | {'JITTER (deg)':>13} | {'PRECISION (μm)':>15} | " - f"{'AGC':<4} | {'MAG':<5} | {'STATUS':<12}" - ) - print("-" * 130) - - for enc_name in ['M0', 'S0']: - samples = self.current_samples[enc_name] - - if len(samples) == 0: - print(f"{enc_name:<4} | {'---':>12} | {'---':>13} | {'---':>15} | {'---':<4} | {'---':<5} | OFFLINE") - continue - - median, jitter, precision = self.calculate_precision_metrics(samples) - - if median is None: - print(f"{enc_name:<4} | {'INSUFFICIENT':>12} | {'DATA':>13} | {'---':>15} | {'---':<4} | {'---':<5} | ERROR") - continue - - # Get metadata from last sample - meta = self.sample_metadata[enc_name][-1] if self.sample_metadata[enc_name] else {} - agc = meta.get('agc', '---') - mag = meta.get('mag', '---') - status = meta.get('status', 'OK') - - print( - f"{enc_name:<4} | {median:12.5f} | {jitter:13.5f} | {precision:15.1f} | " - f"{agc:<4} | {mag:<5} | {status:<12}" - ) - - print("="*130) - print("\nPress Enter to measure again (Z to set zero, Q to quit)") - - def handle_data(self, payload): - # Handle incoming BLE data - collect samples in manual mode - parts = payload.split('|') - try: - g_idx = int(parts[0]) - - if self.last_g_idx != -1 and g_idx - self.last_g_idx > 1: - self.missed_packets += g_idx - self.last_g_idx - 1 - self.last_g_idx = g_idx - - # -------- helper to parse an encoder field -------- - def parse_encoder(part): - """Parse 'XX:angle,pkt[,agc,mag,magl,magh,cof]' -> (name, dict) - Supports both full 7-field and short 2-field formats.""" - name_str, d = part.split(":") - fields = d.split(",") - - if fields[0] == "OFFLINE": - return name_str, None - - ang = float(fields[0]) / 10000.0 - pkt = int(fields[1]) if len(fields) > 1 else 0 - - # Full format has 7 fields; short format has 2 - if len(fields) >= 7: - agc = int(fields[2]) - mag = int(fields[3]) - magl = fields[4] - magh = fields[5] - cof = fields[6] - else: - agc = 0 - mag = 0 - magl = "0" - magh = "0" - cof = "0" - - status = "OK" - if cof == "1": - status = "CORDIC_ERR" - elif magl == "1": - status = "FIELD_LOW" - elif magh == "1": - status = "FIELD_HIGH" - - meta = { - 'g_idx': g_idx, 'pkt': pkt, 'agc': agc, 'mag': mag, - 'magl': magl, 'magh': magh, 'cof': cof, 'status': status - } - return name_str, (ang, meta) - - # -------- MASTER -------- - m_result = None - name_str, result = parse_encoder(parts[1]) - if result is not None: - raw_ang, meta = result - self.latest_raw['M0'] = raw_ang - m_result = (raw_ang, meta) - - # -------- SLAVE (S0 only) -------- - s_result = None - idx = 2 # S0 is at index 2 in the parts array - if idx < len(parts): - name_str, result = parse_encoder(parts[idx]) - if result is not None: - raw_ang, meta = result - self.latest_raw['S0'] = raw_ang - s_result = (raw_ang, meta) - - if not self.collecting: - return # Ignore data when not actively collecting - - # Progress indicator - print(".", end="", flush=True) - - # Store zero-adjusted samples for MASTER - if m_result is not None: - raw_ang, meta = m_result - adj_ang = raw_ang - self.zero_offsets['M0'] - meta['status'] = "MASTER" if meta['status'] == "OK" else meta['status'] - self.current_samples['M0'].append(adj_ang) - self.sample_metadata['M0'].append(meta) - - # Store zero-adjusted samples for SLAVE - if s_result is not None: - raw_ang, meta = s_result - adj_ang = raw_ang - self.zero_offsets['S0'] - self.current_samples['S0'].append(adj_ang) - self.sample_metadata['S0'].append(meta) - - # Check if we've collected enough samples - # Use M0 as reference (master is always present) - if len(self.current_samples['M0']) >= self.samples_per_measurement: - self.collecting = False - self.samples_collected = True - self.display_results() - - except Exception as e: - print(f"\nParse error: {e}") - print(payload) - - async def send_read(self): - """Send a READ command to trigger one on-demand reading.""" - if self.client and self.client.is_connected: - try: - await self.client.write_gatt_char(COMMAND_UUID, b"READ") - except Exception: - pass - - async def send_cmd(self, cmd): - if self.client and self.client.is_connected: - try: - await self.client.write_gatt_char(COMMAND_UUID, cmd.encode()) - print(f"\n✓ Sent command: {cmd}\n") - except Exception: - print("\n✗ Command characteristic not available on gateway\n") - - -iface = GatewayInterface() - - -def notify(_, data): - iface.handle_data(data.decode()) - - -async def get_key(): - if sys.platform == "win32": - return None - if select.select([sys.stdin], [], [], 0)[0]: - return sys.stdin.read(1) - return None - - -async def main(): - print("Scanning for gateway...") - dev = await BleakScanner.find_device_by_filter(lambda d, a: d.name == DEVICE_NAME) - if not dev: - print("✗ Gateway not found") - return - - async with BleakClient(dev) as c: - iface.client = c - await c.start_notify(CHARACTERISTIC_UUID, notify) - print("✓ Connected to Oliver_3\n") - - print("=" * 80) - print(" OLIVER PRECISION MEASUREMENT SYSTEM") - print(" Mode: On-demand sampling (5 samples per encoder)") - print(" Encoders: M0 (Master) + S0 (Slave)") - print("=" * 80) - print("\nPress Enter to start a measurement (Z to set zero, Q to quit)\n") - - try: - while c.is_connected: - k = await get_key() - if k: - if k.lower() == 'q': - print("\nDisconnecting...") - break - elif k.lower() == 'z': - await iface.send_read() - await asyncio.sleep(0.3) - iface.set_zero() - elif k == '\n': - iface.start_measurement() - while iface.collecting: - await iface.send_read() - await asyncio.sleep(0.15) - await asyncio.sleep(0.05) - except KeyboardInterrupt: - print("\n\nDisconnecting...") - - -if __name__ == "__main__": - if sys.platform != "win32": - import tty, termios - old = termios.tcgetattr(sys.stdin) - try: - tty.setcbreak(sys.stdin.fileno()) - asyncio.run(main()) - finally: - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old) - else: - asyncio.run(main()) - +""" +Author: Swaraj Dangare +""" +import asyncio +import time +import sys +import select +import numpy as np +from collections import deque +from bleak import BleakClient, BleakScanner + +# BLE configuration +DEVICE_NAME = "Oliver_3" +CHARACTERISTIC_UUID = "1d4cd358-172d-4c33-b0b2-ddce9a071aab" +COMMAND_UUID = "308a0c43-80f0-4b01-81e5-bb2798eb92f9" + + +class GatewayInterface: + def __init__(self): + self.start_time = time.time() + self.last_g_idx = -1 + self.missed_packets = 0 + self.client = None + + # Manual sampling mode + self.manual_mode = True + self.samples_per_measurement = 5 + self.current_samples = { + 'M0': [], 'S0': [] + } + self.sample_metadata = { + 'M0': [], 'S0': [] + } + self.collecting = False + self.measurement_id = 0 + + # Zero offset: subtract this from raw angles to get relative values + self.zero_offsets = {'M0': 0.0, 'S0': 0.0} + # Track the latest raw angle from each encoder (updated every packet) + self.latest_raw = {'M0': None, 'S0': None} + + # Legacy variance tracking (kept for reference) + self.sample_size = 100 + self.encoder_samples = { + 'M0': deque(maxlen=self.sample_size), + 'S0': deque(maxlen=self.sample_size), + } + + def set_zero(self): + """Set current encoder positions as the zero reference""" + zeroed = [] + for enc in ['M0', 'S0']: + if self.latest_raw[enc] is not None: + self.zero_offsets[enc] = self.latest_raw[enc] + zeroed.append(f"{enc}={self.latest_raw[enc]:.5f}°") + if zeroed: + print(f"\n✓ ZERO set: {', '.join(zeroed)}") + print(" All future measurements will be relative to this position.") + else: + print("\n✗ No encoder data received yet — cannot set zero.") + print() + + def variance(self, key): + vals = list(self.encoder_samples[key]) + return np.var(vals) if len(vals) > 1 else 0.0 + + def start_measurement(self): + """Initialize a new measurement session""" + self.measurement_id += 1 + self.collecting = True + self.samples_collected = False + for key in self.current_samples: + self.current_samples[key] = [] + self.sample_metadata[key] = [] + print("\nCollecting 5 samples", end="", flush=True) + + def calculate_precision_metrics(self, samples): + """Calculate median, jitter, and precision from samples""" + if len(samples) < 3: + return None, None, None + + median_angle = np.median(samples) + jitter = max(samples) - min(samples) + # Assuming 183mm reach (from reference script) + precision_um = 183 * (jitter * np.pi / 180.0) * 1000 + return median_angle, jitter, precision_um + + def display_results(self): + """Display measurement results with precision metrics""" + print("\n\n" + "="*130) + print(f"=== OLIVER PRECISION MEASUREMENT #{self.measurement_id} | Uptime {int(time.time() - self.start_time)}s ===") + print("="*130) + print( + f"{'ENC':<4} | {'MEDIAN (deg)':>12} | {'JITTER (deg)':>13} | {'PRECISION (μm)':>15} | " + f"{'AGC':<4} | {'MAG':<5} | {'STATUS':<12}" + ) + print("-" * 130) + + for enc_name in ['M0', 'S0']: + samples = self.current_samples[enc_name] + + if len(samples) == 0: + print(f"{enc_name:<4} | {'---':>12} | {'---':>13} | {'---':>15} | {'---':<4} | {'---':<5} | OFFLINE") + continue + + median, jitter, precision = self.calculate_precision_metrics(samples) + + if median is None: + print(f"{enc_name:<4} | {'INSUFFICIENT':>12} | {'DATA':>13} | {'---':>15} | {'---':<4} | {'---':<5} | ERROR") + continue + + # Get metadata from last sample + meta = self.sample_metadata[enc_name][-1] if self.sample_metadata[enc_name] else {} + agc = meta.get('agc', '---') + mag = meta.get('mag', '---') + status = meta.get('status', 'OK') + + print( + f"{enc_name:<4} | {median:12.5f} | {jitter:13.5f} | {precision:15.1f} | " + f"{agc:<4} | {mag:<5} | {status:<12}" + ) + + print("="*130) + print("\nPress Enter to measure again (Z to set zero, Q to quit)") + + def handle_data(self, payload): + # Handle incoming BLE data - collect samples in manual mode + parts = payload.split('|') + try: + g_idx = int(parts[0]) + + if self.last_g_idx != -1 and g_idx - self.last_g_idx > 1: + self.missed_packets += g_idx - self.last_g_idx - 1 + self.last_g_idx = g_idx + + # -------- helper to parse an encoder field -------- + def parse_encoder(part): + """Parse 'XX:angle,pkt[,agc,mag,magl,magh,cof]' -> (name, dict) + Supports both full 7-field and short 2-field formats.""" + name_str, d = part.split(":") + fields = d.split(",") + + if fields[0] == "OFFLINE": + return name_str, None + + ang = float(fields[0]) / 10000.0 + pkt = int(fields[1]) if len(fields) > 1 else 0 + + # Full format has 7 fields; short format has 2 + if len(fields) >= 7: + agc = int(fields[2]) + mag = int(fields[3]) + magl = fields[4] + magh = fields[5] + cof = fields[6] + else: + agc = 0 + mag = 0 + magl = "0" + magh = "0" + cof = "0" + + status = "OK" + if cof == "1": + status = "CORDIC_ERR" + elif magl == "1": + status = "FIELD_LOW" + elif magh == "1": + status = "FIELD_HIGH" + + meta = { + 'g_idx': g_idx, 'pkt': pkt, 'agc': agc, 'mag': mag, + 'magl': magl, 'magh': magh, 'cof': cof, 'status': status + } + return name_str, (ang, meta) + + # -------- MASTER -------- + m_result = None + name_str, result = parse_encoder(parts[1]) + if result is not None: + raw_ang, meta = result + self.latest_raw['M0'] = raw_ang + m_result = (raw_ang, meta) + + # -------- SLAVE (S0 only) -------- + s_result = None + idx = 2 # S0 is at index 2 in the parts array + if idx < len(parts): + name_str, result = parse_encoder(parts[idx]) + if result is not None: + raw_ang, meta = result + self.latest_raw['S0'] = raw_ang + s_result = (raw_ang, meta) + + if not self.collecting: + return # Ignore data when not actively collecting + + # Progress indicator + print(".", end="", flush=True) + + # Store zero-adjusted samples for MASTER + if m_result is not None: + raw_ang, meta = m_result + adj_ang = raw_ang - self.zero_offsets['M0'] + meta['status'] = "MASTER" if meta['status'] == "OK" else meta['status'] + self.current_samples['M0'].append(adj_ang) + self.sample_metadata['M0'].append(meta) + + # Store zero-adjusted samples for SLAVE + if s_result is not None: + raw_ang, meta = s_result + adj_ang = raw_ang - self.zero_offsets['S0'] + self.current_samples['S0'].append(adj_ang) + self.sample_metadata['S0'].append(meta) + + # Check if we've collected enough samples + # Use M0 as reference (master is always present) + if len(self.current_samples['M0']) >= self.samples_per_measurement: + self.collecting = False + self.samples_collected = True + self.display_results() + + except Exception as e: + print(f"\nParse error: {e}") + print(payload) + + async def send_read(self): + """Send a READ command to trigger one on-demand reading.""" + if self.client and self.client.is_connected: + try: + await self.client.write_gatt_char(COMMAND_UUID, b"READ") + except Exception: + pass + + async def send_cmd(self, cmd): + if self.client and self.client.is_connected: + try: + await self.client.write_gatt_char(COMMAND_UUID, cmd.encode()) + print(f"\n✓ Sent command: {cmd}\n") + except Exception: + print("\n✗ Command characteristic not available on gateway\n") + + +iface = GatewayInterface() + + +def notify(_, data): + iface.handle_data(data.decode()) + + +async def get_key(): + if sys.platform == "win32": + return None + if select.select([sys.stdin], [], [], 0)[0]: + return sys.stdin.read(1) + return None + + +async def main(): + print("Scanning for gateway...") + dev = await BleakScanner.find_device_by_filter(lambda d, a: d.name == DEVICE_NAME) + if not dev: + print("✗ Gateway not found") + return + + async with BleakClient(dev) as c: + iface.client = c + await c.start_notify(CHARACTERISTIC_UUID, notify) + print("✓ Connected to Oliver_3\n") + + print("=" * 80) + print(" OLIVER PRECISION MEASUREMENT SYSTEM") + print(" Mode: On-demand sampling (5 samples per encoder)") + print(" Encoders: M0 (Master) + S0 (Slave)") + print("=" * 80) + print("\nPress Enter to start a measurement (Z to set zero, Q to quit)\n") + + try: + while c.is_connected: + k = await get_key() + if k: + if k.lower() == 'q': + print("\nDisconnecting...") + break + elif k.lower() == 'z': + await iface.send_read() + await asyncio.sleep(0.3) + iface.set_zero() + elif k == '\n': + iface.start_measurement() + while iface.collecting: + await iface.send_read() + await asyncio.sleep(0.15) + await asyncio.sleep(0.05) + except KeyboardInterrupt: + print("\n\nDisconnecting...") + + +if __name__ == "__main__": + if sys.platform != "win32": + import tty, termios + old = termios.tcgetattr(sys.stdin) + try: + tty.setcbreak(sys.stdin.fileno()) + asyncio.run(main()) + finally: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old) + else: + asyncio.run(main()) + diff --git a/Oliver/slave/slave.ino b/Oliver/slave/slave.ino index ca5f5c0..894f8ea 100644 --- a/Oliver/slave/slave.ino +++ b/Oliver/slave/slave.ino @@ -1,207 +1,207 @@ -#include -#include -#include -#include -#include - -/** - * Project: Wireless Ultra-Precision Slave (On-Demand) - * Protocol: ESP-NOW (Request -> Response) - * Sensor: AS5047D (SPI) - * Logic: Wait for Request -> Measure (~15ms) -> Reply - * Author: Swaraj Dangare - */ - -#define SLAVE_ID 0 //<--- CHANGE THIS FOR EACH BOARD: 0, 1, 2, 3, 4 -#define WIFI_CHANNEL 11 // Must match master's WIFI_CHANNEL (use 1, 6, or 11) - -// ---------------- COMMUNICATION ---------------- -typedef struct __attribute__((packed)) { - uint8_t id; - int value; // Angle * 10000 - uint32_t packetIdx; - uint8_t agc; // AS5047D AGC value (0-255) - uint16_t mag; // AS5047D CORDIC magnitude (14-bit) - uint8_t magl; // Magnetic field too low (0 or 1) - uint8_t magh; // Magnetic field too high (0 or 1) - uint8_t cof; // CORDIC overflow (0 or 1) -} Payload; - -Payload pkt; -uint32_t packet_counter = 0; - -// ---------------- AS5047D SETTINGS ---------------- -#define ANGLECOM 0x3FFF -#define DIAAGC_REG 0x3FFC -#define MAG_REG 0x3FFD -#define RD 0x40 -#define NUM_BLOCKS 16 -#define SAMPLES_PER_BLOCK 256 -#define TOTAL_SAMPLES (NUM_BLOCKS * SAMPLES_PER_BLOCK) // 4096 - -// XIAO ESP32C3 SPI Pins (Standard) -const int PIN_CS = D7; -const int PIN_SCK = D1; -const int PIN_MISO = D0; -const int PIN_MOSI = D10; - -SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); - -// ---------------- UTILS & ENCODER ---------------- -uint16_t evenParityBit(uint16_t x) { - x &= 0x7FFF; - return __builtin_parity(x); -} - -uint16_t makeReadCmd(uint16_t addr) { - uint16_t cmd = (1 << 14) | (addr & 0x3FFF); - cmd |= (evenParityBit(cmd) << 15); - return cmd; -} - -uint16_t AS5047D_Read() { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(PIN_CS, LOW); - SPI.transfer16(makeReadCmd(ANGLECOM)); - digitalWrite(PIN_CS, HIGH); - delayMicroseconds(1); - digitalWrite(PIN_CS, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(PIN_CS, HIGH); - SPI.endTransaction(); - return result; -} - -uint16_t readRegister(uint16_t addr) { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(PIN_CS, LOW); - SPI.transfer16(makeReadCmd(addr)); - digitalWrite(PIN_CS, HIGH); - delayMicroseconds(1); - digitalWrite(PIN_CS, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(PIN_CS, HIGH); - SPI.endTransaction(); - return result & 0x3FFF; -} - -// Robust Mean: Calculate mean of samples within 1.5 LSB distance from initial -// median -double getRobustMean(uint16_t *samples, int size) { - double sum = 0; - for (int i = 0; i < size; i++) - sum += (samples[i] & 0x3FFF); - double initialMean = sum / size; - - double robustSum = 0; - int count = 0; - for (int i = 0; i < size; i++) { - uint16_t val = samples[i] & 0x3FFF; - if (abs((double)val - initialMean) < 1.5) { - robustSum += val; - count++; - } - } - return (count > 0) ? (robustSum / count) : initialMean; -} - -double getMedian(double *values, int size) { - std::sort(values, values + size); - return values[size / 2]; -} - -double getUltraPrecisionReading() { - double blockMeans[NUM_BLOCKS]; - uint16_t blockSamples[SAMPLES_PER_BLOCK]; - - for (int b = 0; b < NUM_BLOCKS; b++) { - for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { - uint16_t raw = AS5047D_Read(); - if (((raw >> 15) & 1) == evenParityBit(raw)) { - blockSamples[s] = raw; - } else { - s--; - } - } - blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); - delayMicroseconds(50); // Small yield - } - double finalCounts = getMedian(blockMeans, NUM_BLOCKS); - return (finalCounts * 360.0) / 16384.0; -} - -// ---------------- ESP-NOW CALLBACK ---------------- -void onDataRecv(const esp_now_recv_info_t *info, const uint8_t *data, int len) { - // 1. Discovery Response (Master sends 0xFF) - if (len == 1 && data[0] == 0xFF) { - // Add small random delay to prevent collision when multiple slaves respond - // This staggers responses across 10-30ms window - delayMicroseconds(random(10000, 30000)); // 10-30ms - - uint8_t reply = SLAVE_ID; - // Add master as peer dynamically so we can reply - if (!esp_now_is_peer_exist(info->src_addr)) { - esp_now_peer_info_t peer{}; - memcpy(peer.peer_addr, info->src_addr, 6); - peer.channel = WIFI_CHANNEL; - peer.encrypt = false; - esp_now_add_peer(&peer); - } - esp_now_send(info->src_addr, &reply, 1); - } - - // 2. Data Request (Master sends Slave ID) - if (len == 1 && data[0] == SLAVE_ID) { - // Perform Measurement (Block-Blocking but fast enough ~15-20ms) - double angle = getUltraPrecisionReading(); - - // Read diagnostic registers - uint16_t diaagc = readRegister(DIAAGC_REG); - uint16_t mag = readRegister(MAG_REG); - - pkt.id = SLAVE_ID; - pkt.value = (int)(angle * 10000.0); - pkt.packetIdx = packet_counter++; - pkt.agc = diaagc & 0xFF; - pkt.mag = mag & 0x3FFF; - pkt.magl = (diaagc >> 8) & 0x01; - pkt.magh = (diaagc >> 10) & 0x01; - pkt.cof = (diaagc >> 9) & 0x01; - - esp_now_send(info->src_addr, (uint8_t *)&pkt, sizeof(pkt)); - } -} - -void setup() { - Serial.begin(115200); - Serial.println("init"); - // SPI Setup - pinMode(PIN_CS, OUTPUT); - digitalWrite(PIN_CS, HIGH); - SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); - - // WiFi / ESP-NOW Setup - WiFi.mode(WIFI_STA); - WiFi.disconnect(); - - // Force WiFi channel (must match master) - esp_wifi_set_promiscuous(true); - esp_wifi_set_channel(WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE); - esp_wifi_set_promiscuous(false); - - if (esp_now_init() != ESP_OK) { - Serial.println("ESP-NOW Init Failed"); - return; - } - esp_now_register_recv_cb(onDataRecv); - - Serial.printf("Slave %d Ready | Channel %d | MAC: %s\n", SLAVE_ID, - WIFI_CHANNEL, WiFi.macAddress().c_str()); -} - -void loop() { - // Nothing here - totally interrupt driven - delay(100); +#include +#include +#include +#include +#include + +/** + * Project: Wireless Ultra-Precision Slave (On-Demand) + * Protocol: ESP-NOW (Request -> Response) + * Sensor: AS5047D (SPI) + * Logic: Wait for Request -> Measure (~15ms) -> Reply + * Author: Swaraj Dangare + */ + +#define SLAVE_ID 0 //<--- CHANGE THIS FOR EACH BOARD: 0, 1, 2, 3, 4 +#define WIFI_CHANNEL 11 // Must match master's WIFI_CHANNEL (use 1, 6, or 11) + +// ---------------- COMMUNICATION ---------------- +typedef struct __attribute__((packed)) { + uint8_t id; + int value; // Angle * 10000 + uint32_t packetIdx; + uint8_t agc; // AS5047D AGC value (0-255) + uint16_t mag; // AS5047D CORDIC magnitude (14-bit) + uint8_t magl; // Magnetic field too low (0 or 1) + uint8_t magh; // Magnetic field too high (0 or 1) + uint8_t cof; // CORDIC overflow (0 or 1) +} Payload; + +Payload pkt; +uint32_t packet_counter = 0; + +// ---------------- AS5047D SETTINGS ---------------- +#define ANGLECOM 0x3FFF +#define DIAAGC_REG 0x3FFC +#define MAG_REG 0x3FFD +#define RD 0x40 +#define NUM_BLOCKS 16 +#define SAMPLES_PER_BLOCK 256 +#define TOTAL_SAMPLES (NUM_BLOCKS * SAMPLES_PER_BLOCK) // 4096 + +// XIAO ESP32C3 SPI Pins (Standard) +const int PIN_CS = D7; +const int PIN_SCK = D1; +const int PIN_MISO = D0; +const int PIN_MOSI = D10; + +SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); + +// ---------------- UTILS & ENCODER ---------------- +uint16_t evenParityBit(uint16_t x) { + x &= 0x7FFF; + return __builtin_parity(x); +} + +uint16_t makeReadCmd(uint16_t addr) { + uint16_t cmd = (1 << 14) | (addr & 0x3FFF); + cmd |= (evenParityBit(cmd) << 15); + return cmd; +} + +uint16_t AS5047D_Read() { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(PIN_CS, LOW); + SPI.transfer16(makeReadCmd(ANGLECOM)); + digitalWrite(PIN_CS, HIGH); + delayMicroseconds(1); + digitalWrite(PIN_CS, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(PIN_CS, HIGH); + SPI.endTransaction(); + return result; +} + +uint16_t readRegister(uint16_t addr) { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(PIN_CS, LOW); + SPI.transfer16(makeReadCmd(addr)); + digitalWrite(PIN_CS, HIGH); + delayMicroseconds(1); + digitalWrite(PIN_CS, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(PIN_CS, HIGH); + SPI.endTransaction(); + return result & 0x3FFF; +} + +// Robust Mean: Calculate mean of samples within 1.5 LSB distance from initial +// median +double getRobustMean(uint16_t *samples, int size) { + double sum = 0; + for (int i = 0; i < size; i++) + sum += (samples[i] & 0x3FFF); + double initialMean = sum / size; + + double robustSum = 0; + int count = 0; + for (int i = 0; i < size; i++) { + uint16_t val = samples[i] & 0x3FFF; + if (abs((double)val - initialMean) < 1.5) { + robustSum += val; + count++; + } + } + return (count > 0) ? (robustSum / count) : initialMean; +} + +double getMedian(double *values, int size) { + std::sort(values, values + size); + return values[size / 2]; +} + +double getUltraPrecisionReading() { + double blockMeans[NUM_BLOCKS]; + uint16_t blockSamples[SAMPLES_PER_BLOCK]; + + for (int b = 0; b < NUM_BLOCKS; b++) { + for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { + uint16_t raw = AS5047D_Read(); + if (((raw >> 15) & 1) == evenParityBit(raw)) { + blockSamples[s] = raw; + } else { + s--; + } + } + blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); + delayMicroseconds(50); // Small yield + } + double finalCounts = getMedian(blockMeans, NUM_BLOCKS); + return (finalCounts * 360.0) / 16384.0; +} + +// ---------------- ESP-NOW CALLBACK ---------------- +void onDataRecv(const esp_now_recv_info_t *info, const uint8_t *data, int len) { + // 1. Discovery Response (Master sends 0xFF) + if (len == 1 && data[0] == 0xFF) { + // Add small random delay to prevent collision when multiple slaves respond + // This staggers responses across 10-30ms window + delayMicroseconds(random(10000, 30000)); // 10-30ms + + uint8_t reply = SLAVE_ID; + // Add master as peer dynamically so we can reply + if (!esp_now_is_peer_exist(info->src_addr)) { + esp_now_peer_info_t peer{}; + memcpy(peer.peer_addr, info->src_addr, 6); + peer.channel = WIFI_CHANNEL; + peer.encrypt = false; + esp_now_add_peer(&peer); + } + esp_now_send(info->src_addr, &reply, 1); + } + + // 2. Data Request (Master sends Slave ID) + if (len == 1 && data[0] == SLAVE_ID) { + // Perform Measurement (Block-Blocking but fast enough ~15-20ms) + double angle = getUltraPrecisionReading(); + + // Read diagnostic registers + uint16_t diaagc = readRegister(DIAAGC_REG); + uint16_t mag = readRegister(MAG_REG); + + pkt.id = SLAVE_ID; + pkt.value = (int)(angle * 10000.0); + pkt.packetIdx = packet_counter++; + pkt.agc = diaagc & 0xFF; + pkt.mag = mag & 0x3FFF; + pkt.magl = (diaagc >> 8) & 0x01; + pkt.magh = (diaagc >> 10) & 0x01; + pkt.cof = (diaagc >> 9) & 0x01; + + esp_now_send(info->src_addr, (uint8_t *)&pkt, sizeof(pkt)); + } +} + +void setup() { + Serial.begin(115200); + Serial.println("init"); + // SPI Setup + pinMode(PIN_CS, OUTPUT); + digitalWrite(PIN_CS, HIGH); + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); + + // WiFi / ESP-NOW Setup + WiFi.mode(WIFI_STA); + WiFi.disconnect(); + + // Force WiFi channel (must match master) + esp_wifi_set_promiscuous(true); + esp_wifi_set_channel(WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE); + esp_wifi_set_promiscuous(false); + + if (esp_now_init() != ESP_OK) { + Serial.println("ESP-NOW Init Failed"); + return; + } + esp_now_register_recv_cb(onDataRecv); + + Serial.printf("Slave %d Ready | Channel %d | MAC: %s\n", SLAVE_ID, + WIFI_CHANNEL, WiFi.macAddress().c_str()); +} + +void loop() { + // Nothing here - totally interrupt driven + delay(100); } \ No newline at end of file diff --git a/README.md b/README.md index ab87c5c..b0377e6 100644 --- a/README.md +++ b/README.md @@ -1,137 +1,137 @@ -# ESP Encoder - -A collection of ESP32-based encoder measurement projects using the AS5047D 14-bit magnetic rotary encoder. - -Each project lives in `projects/` with its own Python scripts. All projects share firmware templates from `firmware/`. - ---- - -## Firmware Templates - -| Template | Location | Use When | -|---|---|---| -| **Single Encoder** | [`firmware/single_encoder/`](firmware/single_encoder/) | One ESP reads one encoder, sends directly to PC via BLE | -| **Dual Encoder** | [`firmware/dual_encoder/`](firmware/dual_encoder/) | One ESP reads two encoders on same SPI bus, sends to PC via BLE | -| **Master** | [`firmware/master/`](firmware/master/) | ESP32C6 — aggregates encoder data from slaves, sends to PC via BLE | -| **Slave** | [`firmware/slave/`](firmware/slave/) | ESP32C3 — reads one encoder, responds to master over ESP-NOW | - -### Flashing a Single-Encoder Project - -1. Open `firmware/single_encoder/single_encoder.ino` in Arduino IDE -2. Edit the config block at the top: - ```cpp - #define ESP_NAME "YOUR_DEVICE_NAME" - #define SERVICE_UUID "4fafc201-..." - #define CHARACTERISTIC_UUID "beb5483e-..." - ``` -3. Select board (XIAO ESP32C3 or ESP32C6) and upload - -### Flashing a Multi-Encoder Project (Master + Slave) - -See [`Oliver/README.md`](Oliver/README.md) for full setup instructions. - ---- - -## Projects - -| Project | Firmware | Description | -|---|---|---| -| [block_height](projects/block_height/) | Single Encoder | Block height from encoder rotation via trigonometry or polynomial fit | -| [Oliver](Oliver/) | Master + Slave | Wireless multi-encoder system (ESP32C6 master + ESP32C3 slaves) | - ---- - -## Repository Structure - -``` -Esp_Encoder/ -├── firmware/ -│ ├── single_encoder/ -│ │ └── single_encoder.ino # Single ESP → BLE to PC -│ ├── dual_encoder/ -│ │ └── dual_encoder.ino # Single ESP reading 2 Encoders → BLE to PC -│ ├── master/ -│ │ └── master.ino # ESP32C6 master — aggregates slaves, sends BLE -│ └── slave/ -│ └── slave.ino # ESP32C3 slave — reads encoder, replies via ESP-NOW -│ -├── projects/ -│ └── block_height/ # Block height measurement Python scripts -│ -├── Oliver/ # Multi-encoder project (firmware + Python) -│ ├── master/master.ino # (copy of firmware/master) -│ ├── slave/slave.ino # (copy of firmware/slave) -│ ├── oliver.py # Python API for BLE communication -│ ├── oliver_raw.py # Raw BLE data script -│ └── README.md -│ -├── Datasheets/ -│ └── AS5047D.pdf -├── requirements.txt -├── setup.sh -└── README.md -``` - ---- - -## How It Works - -### Ultra-Precision Sampling Algorithm - -All projects use the same AS5047D reading algorithm: - -1. **4,096 raw SPI reads**, split into 16 blocks of 256 samples -2. **Robust mean per block** — discard samples > 1.5 LSB from mean, re-average the rest -3. **Median of 16 block means** — rejects any block corrupted by radio bursts -4. **Convert to degrees**: `angle = (counts × 360) / 16384` - -### Single-Encoder BLE Data Format - -The single encoder uses the **same 7-field format as Oliver master** for protocol compatibility: - -``` -|M0:,,,,,, -``` - -Example: `5|M0:1669900,5,38,1823,0,0,0` - -| Field | Description | -|---|---| -| `packetIdx` | Global packet counter (increments per READ) | -| `angle` | Encoder angle × 10000 as integer (e.g. `1669900` = 166.9900°) | -| `pkt` | Same as packetIdx (per-encoder index) | -| `agc` | Automatic gain control (0–255) | -| `mag` | CORDIC magnitude (field strength) | -| `magl / magh / cof` | Diagnostic flags (0 or 1) | - -**BLE Commands:** - -| Command | Description | -|---|---| -| `READ` | Request one on-demand encoder reading | -| `ZERO` | Set current position as zero reference | - -### Multi-Encoder (Oliver) BLE Data Format - -``` -|M0:,,,,,,|S0:...|S1:... -``` - -See [`Oliver/README.md`](Oliver/README.md) for full protocol details. - ---- - -## Getting Started - -```bash -./setup.sh -source .venv/bin/activate -``` - -## License - -MIT - ---- - -**Author:** Swaraj Dangare +# ESP Encoder + +A collection of ESP32-based encoder measurement projects using the AS5047D 14-bit magnetic rotary encoder. + +Each project lives in `projects/` with its own Python scripts. All projects share firmware templates from `firmware/`. + +--- + +## Firmware Templates + +| Template | Location | Use When | +|---|---|---| +| **Single Encoder** | [`firmware/single_encoder/`](firmware/single_encoder/) | One ESP reads one encoder, sends directly to PC via BLE | +| **Dual Encoder** | [`firmware/dual_encoder/`](firmware/dual_encoder/) | One ESP reads two encoders on same SPI bus, sends to PC via BLE | +| **Master** | [`firmware/master/`](firmware/master/) | ESP32C6 — aggregates encoder data from slaves, sends to PC via BLE | +| **Slave** | [`firmware/slave/`](firmware/slave/) | ESP32C3 — reads one encoder, responds to master over ESP-NOW | + +### Flashing a Single-Encoder Project + +1. Open `firmware/single_encoder/single_encoder.ino` in Arduino IDE +2. Edit the config block at the top: + ```cpp + #define ESP_NAME "YOUR_DEVICE_NAME" + #define SERVICE_UUID "4fafc201-..." + #define CHARACTERISTIC_UUID "beb5483e-..." + ``` +3. Select board (XIAO ESP32C3 or ESP32C6) and upload + +### Flashing a Multi-Encoder Project (Master + Slave) + +See [`Oliver/README.md`](Oliver/README.md) for full setup instructions. + +--- + +## Projects + +| Project | Firmware | Description | +|---|---|---| +| [block_height](projects/block_height/) | Single Encoder | Block height from encoder rotation via trigonometry or polynomial fit | +| [Oliver](Oliver/) | Master + Slave | Wireless multi-encoder system (ESP32C6 master + ESP32C3 slaves) | + +--- + +## Repository Structure + +``` +Esp_Encoder/ +├── firmware/ +│ ├── single_encoder/ +│ │ └── single_encoder.ino # Single ESP → BLE to PC +│ ├── dual_encoder/ +│ │ └── dual_encoder.ino # Single ESP reading 2 Encoders → BLE to PC +│ ├── master/ +│ │ └── master.ino # ESP32C6 master — aggregates slaves, sends BLE +│ └── slave/ +│ └── slave.ino # ESP32C3 slave — reads encoder, replies via ESP-NOW +│ +├── projects/ +│ └── block_height/ # Block height measurement Python scripts +│ +├── Oliver/ # Multi-encoder project (firmware + Python) +│ ├── master/master.ino # (copy of firmware/master) +│ ├── slave/slave.ino # (copy of firmware/slave) +│ ├── oliver.py # Python API for BLE communication +│ ├── oliver_raw.py # Raw BLE data script +│ └── README.md +│ +├── Datasheets/ +│ └── AS5047D.pdf +├── requirements.txt +├── setup.sh +└── README.md +``` + +--- + +## How It Works + +### Ultra-Precision Sampling Algorithm + +All projects use the same AS5047D reading algorithm: + +1. **4,096 raw SPI reads**, split into 16 blocks of 256 samples +2. **Robust mean per block** — discard samples > 1.5 LSB from mean, re-average the rest +3. **Median of 16 block means** — rejects any block corrupted by radio bursts +4. **Convert to degrees**: `angle = (counts × 360) / 16384` + +### Single-Encoder BLE Data Format + +The single encoder uses the **same 7-field format as Oliver master** for protocol compatibility: + +``` +|M0:,,,,,, +``` + +Example: `5|M0:1669900,5,38,1823,0,0,0` + +| Field | Description | +|---|---| +| `packetIdx` | Global packet counter (increments per READ) | +| `angle` | Encoder angle × 10000 as integer (e.g. `1669900` = 166.9900°) | +| `pkt` | Same as packetIdx (per-encoder index) | +| `agc` | Automatic gain control (0–255) | +| `mag` | CORDIC magnitude (field strength) | +| `magl / magh / cof` | Diagnostic flags (0 or 1) | + +**BLE Commands:** + +| Command | Description | +|---|---| +| `READ` | Request one on-demand encoder reading | +| `ZERO` | Set current position as zero reference | + +### Multi-Encoder (Oliver) BLE Data Format + +``` +|M0:,,,,,,|S0:...|S1:... +``` + +See [`Oliver/README.md`](Oliver/README.md) for full protocol details. + +--- + +## Getting Started + +```bash +./setup.sh +source .venv/bin/activate +``` + +## License + +MIT + +--- + +**Author:** Swaraj Dangare diff --git a/firmware/16_bit_encoder/16_bit_encoder.ino b/firmware/16_bit_encoder/16_bit_encoder.ino new file mode 100644 index 0000000..af613ec --- /dev/null +++ b/firmware/16_bit_encoder/16_bit_encoder.ino @@ -0,0 +1,476 @@ +/** + * ============================================================ + * AEAT-8800-Q24 — 16-bit Absolute Magnetic Rotary Encoder + * Board : Seeed Studio XIAO ESP32-C6 hi + * + * ── DUAL PROTOCOL ──────────────────────────────────────────── + * • SSI (SSI_SPI_SEL = HIGH) — reads real-time absolute position + * • SPI (SSI_SPI_SEL = LOW) — reads / writes configuration registers + * + * ── PIN MAPPING ────────────────────────────────────────────── + * Encoder Pin │ Pin Name │ GPIO │ Role + * ────────────┼───────────────┼───────┼────────────────────────────────── + * Pin 21 │ SSI_SPI_SEL │ GPIO21│ HIGH = SSI mode / LOW = SPI mode + * Pin 10 │ SCL / CLK │ GPIO19│ Shared clock (SCK) + * Pin 11 │ NSL / DIN │ GPIO18│ SSI enable (NSL) / SPI data-in (MOSI) + * Pin 12 │ DO / DOUT │ GPIO20│ Data output from encoder (MISO) + * ──────────────────────────────────────────────────────────── + * + * ── SSI READ SEQUENCE ──────────────────────────────────────── + * CLK idles HIGH, NSL idles HIGH. + * 1. Pull NSL LOW → encoder latches current angle into shift-reg + * 2. Toggle CLK (HIGH→LOW→HIGH) × 18: + * Bits [17:2] = 16-bit absolute position (MSB first) + * Bit [1] = MHi (magnet too close) + * Bit [0] = MLo (magnet too far) + * Data is valid on the rising edge of each CLK cycle. + * 3. Pull NSL HIGH → end of frame + * 4. Wait ≥ monoflop time before next read (~20 µs typical) + * + * ── SPI PROTOCOL ───────────────────────────────────────────── + * SPI_MODE3 (CPOL=1, CPHA=1), ≤ 1 MHz + * Read cmd : 0b10_AAAAAA (8-bit) → dummy byte → 8-bit reply + * Write cmd : 0b01_AAAAAA (8-bit) → data byte + * + * ── REGISTER MAP ───────────────────────────────────────────── + * 0x00 CustReserve0 0x04 CustConfig0 + * 0x01 CustReserve1 0x05 CPR_Set1 + * 0x02 ZeroPos_L 0x06 CPR_Set2 + * 0x03 ZeroPos_H 0x07 Resolution (bits[1:0]: 00=10b…11=16b) + * 0x10 Lock 0x11 ProgCust + * + * ── SERIAL COMMANDS ────────────────────────────────────────── + * p / P — Print current absolute position (SSI, one-shot) + * c / C — Continuous SSI position stream (toggle on/off) + * s / S — Dump all SPI config registers + * r / R — Read single register: r e.g. "r 07" + * w / W — Write register: w e.g. "w 10 AB" + * u / U — Unlock config registers (write 0xAB → REG_LOCK) + * z / Z — Set hardware zero (writes 0x0000 → ZeroPos shadow regs) + * b / B — Burn shadow regs to OTP ⚠ ONE-TIME, IRREVERSIBLE! + * h / H — Print this help + * + * Author : Swaraj Dangare + * ============================================================ + */ + +#include + +// ─── Pin Definitions +// ────────────────────────────────────────────────────────── GPIO numbers match +// the user's pin table exactly. +#define PIN_SEL D7 // SSI_SPI_SEL : HIGH = SSI, LOW = SPI +#define PIN_CLK D8 // SCL / CLK : shared clock +#define PIN_NSL D10 // NSL / DIN : SSI enable / SPI MOSI +#define PIN_DO D9 // DO / DOUT : data from encoder / SPI MISO + +// ─── SSI Settings +// ───────────────────────────────────────────────────────────── Bit-banged; +// half-period determines SSI clock speed. 5 µs half-period → ~100 kHz SSI clock +// (datasheet max ~1 MHz) +#define SSI_HALF_PERIOD_US 5 +#define SSI_TOTAL_BITS 18 // 16 position + MHi + MLo + +// ─── SPI Settings +// ───────────────────────────────────────────────────────────── SPI_MODE3: +// CPOL=1 (idle high), CPHA=1 (sample on 2nd / rising edge) +SPISettings spiCfg(500000UL, MSBFIRST, SPI_MODE3); + +// ─── Register Addresses +// ─────────────────────────────────────────────────────── +#define REG_CUST_RESERVE_0 0x00 +#define REG_CUST_RESERVE_1 0x01 +#define REG_ZERO_POS_L 0x02 +#define REG_ZERO_POS_H 0x03 +#define REG_CUST_CONFIG_0 0x04 +#define REG_CPR_SET1 0x05 +#define REG_CPR_SET2 0x06 +#define REG_RESOLUTION 0x07 +#define REG_LOCK 0x10 +#define REG_PROG_CUST 0x11 + +// ─── Command / Key Bytes +// ────────────────────────────────────────────────────── +#define CMD_READ 0x80 // 0b10xxxxxx +#define CMD_WRITE 0x40 // 0b01xxxxxx +#define UNLOCK_KEY 0xAB // Unlock config writes +#define PROG_KEY 0xA1 // Burn OTP + +// Resolution field (REG_RESOLUTION bits[1:0]) +#define RES_10BIT 0x00 +#define RES_12BIT 0x01 +#define RES_14BIT 0x02 +#define RES_16BIT 0x03 + +// ─── SSI Frame ─────────────────────────────────────────────────────────────── +// Declared here (global scope) so Arduino's auto-prototype generator +// can see the type before it emits prototypes for ssiReadPosition() etc. +struct SSIFrame { + uint16_t position; // absolute angle, 0–65535 (16-bit) + bool mhi; // magnet too close + bool mlo; // magnet too far +}; + +// ─── State +// ──────────────────────────────────────────────────────────────────── +static bool g_streaming = false; // continuous SSI stream active +static uint32_t g_lastStream = 0; // millis of last stream per print + +// ─── Mode Switching +// ─────────────────────────────────────────────────────────── + +/** Enter SSI mode: SEL HIGH, let CLK and NSL settle HIGH. */ +static void enterSSI() { + SPI.endTransaction(); // release hardware SPI bus + // Re-assert GPIO control of the shared pins + pinMode(PIN_CLK, OUTPUT); + pinMode(PIN_NSL, OUTPUT); + pinMode(PIN_DO, INPUT); + digitalWrite(PIN_CLK, HIGH); + digitalWrite(PIN_NSL, HIGH); + digitalWrite(PIN_SEL, HIGH); + delayMicroseconds(2); +} + +/** Enter SPI mode: SEL LOW, hardware SPI takes over CLK and MOSI. */ +static void enterSPI() { + digitalWrite(PIN_SEL, LOW); + delayMicroseconds(2); + SPI.beginTransaction(spiCfg); +} + +static void exitSPI() { + SPI.endTransaction(); + digitalWrite(PIN_SEL, HIGH); // back to SSI idle + delayMicroseconds(2); +} + +// ─── SSI Read +// ───────────────────────────────────────────────────────────────── + +/** Read 18 bits via bit-banged SSI (16-bit position + MHi + MLo). */ + +SSIFrame ssiReadPosition() { + enterSSI(); + + uint32_t raw = 0; + + // Pull NSL LOW → encoder latches position into shift register + digitalWrite(PIN_NSL, LOW); + delayMicroseconds(SSI_HALF_PERIOD_US); + + for (int i = 0; i < SSI_TOTAL_BITS; i++) { + // Falling edge — encoder shifts next bit onto DO + digitalWrite(PIN_CLK, LOW); + delayMicroseconds(SSI_HALF_PERIOD_US); + + // Rising edge — read the bit + digitalWrite(PIN_CLK, HIGH); + raw = (raw << 1) | (digitalRead(PIN_DO) ? 1u : 0u); + delayMicroseconds(SSI_HALF_PERIOD_US); + } + + // End of frame: NSL HIGH + digitalWrite(PIN_NSL, HIGH); + delayMicroseconds(20); // monoflop recovery time + + SSIFrame f; + f.mlo = (raw >> 0) & 0x01; + f.mhi = (raw >> 1) & 0x01; + f.position = (uint16_t)((raw >> 2) & 0xFFFF); + return f; +} + +// ─── SPI Register Helpers +// ───────────────────────────────────────────────────── + +uint8_t spiReadReg(uint8_t addr) { + enterSPI(); + SPI.transfer(CMD_READ | (addr & 0x3F)); + uint8_t val = SPI.transfer(0xFF); + exitSPI(); + delayMicroseconds(5); + return val; +} + +void spiWriteReg(uint8_t addr, uint8_t data) { + enterSPI(); + SPI.transfer(CMD_WRITE | (addr & 0x3F)); + SPI.transfer(data); + exitSPI(); + delayMicroseconds(5); +} + +void unlockRegisters() { + spiWriteReg(REG_LOCK, UNLOCK_KEY); + delayMicroseconds(10); +} + +uint16_t readHardwareZero() { + uint8_t lo = spiReadReg(REG_ZERO_POS_L); + uint8_t hi = spiReadReg(REG_ZERO_POS_H); + return ((uint16_t)hi << 8) | lo; +} + +void writeHardwareZero(uint16_t zp) { + unlockRegisters(); + spiWriteReg(REG_ZERO_POS_L, zp & 0xFF); + spiWriteReg(REG_ZERO_POS_H, (zp >> 8) & 0xFF); +} + +// ─── Helpers ───────────────────────────────────────────────────────────────── + +const char *resolutionStr(uint8_t regVal) { + switch (regVal & 0x03) { + case RES_10BIT: + return "10-bit (1024 cpr)"; + case RES_12BIT: + return "12-bit (4096 cpr)"; + case RES_14BIT: + return "14-bit (16384 cpr)"; + case RES_16BIT: + return "16-bit (65536 cpr)"; + default: + return "unknown"; + } +} + +const char *magnetStatus(bool mhi, bool mlo) { + if (mhi) + return "TOO CLOSE"; + if (mlo) + return "TOO FAR"; + return "OK"; +} + +// ─── Serial Output Helpers +// ──────────────────────────────────────────────────── + +void printPosition(const SSIFrame &f) { + double deg = (f.position / 65536.0) * 360.0; + Serial.printf("[SSI] Pos=%5u Angle=%8.4f deg Magnet=%s%s%s\n", f.position, + deg, magnetStatus(f.mhi, f.mlo), f.mhi ? " (MHi)" : "", + f.mlo ? " (MLo)" : ""); +} + +void printAllRegisters() { + Serial.println(F("\n╔══════════════════════════════════════════════╗")); + Serial.println(F("║ AEAT-8800-Q24 — SPI Register Dump ║")); + Serial.println(F("╠══════════════════════════════════════════════╣")); + + struct { + uint8_t addr; + const char *name; + } regs[] = { + {REG_CUST_RESERVE_0, "CustReserve0 (0x00)"}, + {REG_CUST_RESERVE_1, "CustReserve1 (0x01)"}, + {REG_ZERO_POS_L, "ZeroPos_L (0x02)"}, + {REG_ZERO_POS_H, "ZeroPos_H (0x03)"}, + {REG_CUST_CONFIG_0, "CustConfig0 (0x04)"}, + {REG_CPR_SET1, "CPR_Set1 (0x05)"}, + {REG_CPR_SET2, "CPR_Set2 (0x06)"}, + {REG_RESOLUTION, "Resolution (0x07)"}, + {REG_LOCK, "Lock (0x10)"}, + }; + + for (auto &r : regs) { + uint8_t val = spiReadReg(r.addr); + Serial.printf("║ %-22s 0x%02X (%3u)\n", r.name, val, val); + } + + uint16_t zp = readHardwareZero(); + uint8_t resReg = spiReadReg(REG_RESOLUTION); + Serial.println(F("╠══════════════════════════════════════════════╣")); + Serial.printf("║ HW Zero Pos : %5u (%.4f deg)\n", zp, + (zp / 65536.0) * 360.0); + Serial.printf("║ Resolution : %s\n", resolutionStr(resReg)); + Serial.println(F("╚══════════════════════════════════════════════╝\n")); +} + +void printHelp() { + Serial.println( + F("\n╔══════════════════════════════════════════════════════╗")); + Serial.println(F("║ AEAT-8800-Q24 Serial Command Reference ║")); + Serial.println(F("╠══════════════════════════════════════════════════════╣")); + Serial.println(F("║ p — Read position once (SSI) ║")); + Serial.println( + F("║ c — Toggle continuous position stream ║")); + Serial.println( + F("║ s — Dump all SPI config registers ║")); + Serial.println(F("║ r — Read register at hex addr (e.g. r 07) ║")); + Serial.println(F("║ w — Write val to reg (e.g. w 10 AB) ║")); + Serial.println(F("║ u — Unlock config registers (0xAB→Lock) ║")); + Serial.println(F("║ z — Set zero position = 0x0000 (shadow only) ║")); + Serial.println(F("║ b — Burn shadow regs to OTP ⚠ IRREVERSIBLE ║")); + Serial.println( + F("║ h — Show this help ║")); + Serial.println( + F("╚══════════════════════════════════════════════════════╝\n")); +} + +// ─── Command Parser +// ─────────────────────────────────────────────────────────── + +void handleSerial() { + if (!Serial.available()) + return; + + // Read full line + String line = Serial.readStringUntil('\n'); + line.trim(); + if (line.length() == 0) + return; + + char cmd = (char)tolower((unsigned char)line[0]); + + switch (cmd) { + + // ── Position read (SSI) ──────────────────────────────────────────────── + case 'p': { + SSIFrame f = ssiReadPosition(); + printPosition(f); + break; + } + + // ── Continuous stream toggle ─────────────────────────────────────────── + case 'c': + g_streaming = !g_streaming; + Serial.printf("[INFO] Continuous stream %s\n", g_streaming ? "ON" : "OFF"); + break; + + // ── Register dump (SPI) ─────────────────────────────────────────────── + case 's': + printAllRegisters(); + break; + + // ── Read single register "r " ────────────────────────────── + case 'r': { + if (line.length() < 3) { + Serial.println(F("[ERR] Usage: r e.g. 'r 07'")); + break; + } + uint8_t addr = (uint8_t)strtoul(line.c_str() + 2, nullptr, 16); + uint8_t val = spiReadReg(addr); + Serial.printf("[SPI-RD] Reg 0x%02X = 0x%02X (%u)\n", addr, val, val); + break; + } + + // ── Write single register "w " ────────────────── + case 'w': { + if (line.length() < 5) { + Serial.println(F("[ERR] Usage: w e.g. 'w 10 AB'")); + break; + } + char *ptr = nullptr; + uint8_t addr = (uint8_t)strtoul(line.c_str() + 2, &ptr, 16); + uint8_t val = (uint8_t)strtoul(ptr, nullptr, 16); + spiWriteReg(addr, val); + uint8_t rb = spiReadReg(addr); + Serial.printf("[SPI-WR] Reg 0x%02X ← 0x%02X | Readback: 0x%02X %s\n", + addr, val, rb, (rb == val) ? "OK" : "MISMATCH!"); + break; + } + + // ── Unlock registers ────────────────────────────────────────────────── + case 'u': + unlockRegisters(); + Serial.println( + F("[SPI] Config registers unlocked (0xAB written to Lock reg)")); + break; + + // ── Set zero position ───────────────────────────────────────────────── + case 'z': { + Serial.println(F("[ZERO] Writing 0x0000 to ZeroPos shadow registers...")); + writeHardwareZero(0x0000); + uint16_t rb = readHardwareZero(); + Serial.printf("[ZERO] Readback: 0x%04X — %s\n", rb, + rb == 0x0000 ? "OK" : "MISMATCH!"); + break; + } + + // ── Burn OTP ⚠ IRREVERSIBLE ───────────────────────────────────────── + case 'b': { + // Require explicit confirmation by sending "burn" as the line + if (!line.equalsIgnoreCase("burn")) { + Serial.println(F("[WARN] ⚠ OTP BURN is IRREVERSIBLE!")); + Serial.println( + F("[WARN] Type exactly 'burn' and press Enter to confirm.")); + break; + } + Serial.println(F("[OTP] Unlocking and burning shadow regs to OTP...")); + unlockRegisters(); + spiWriteReg(REG_PROG_CUST, PROG_KEY); + Serial.println(F("[OTP] Done. Power-cycle the encoder to verify.")); + break; + } + + // ── Help ────────────────────────────────────────────────────────────── + case 'h': + default: + printHelp(); + break; + } +} + +// ─── Setup +// ──────────────────────────────────────────────────────────────────── + +void setup() { + Serial.begin(115200); + delay(300); + + // GPIO init — SSI pins, all idle HIGH + pinMode(PIN_SEL, OUTPUT); + // pinMode(PIN_SEL1, OUTPUT); + pinMode(PIN_CLK, OUTPUT); + pinMode(PIN_NSL, OUTPUT); + pinMode(PIN_DO, INPUT); + digitalWrite(PIN_SEL, HIGH); // SSI mode at boot + // digitalWrite(PIN_SEL1, LOW); + digitalWrite(PIN_CLK, HIGH); + digitalWrite(PIN_NSL, HIGH); + + // Hardware SPI init (pins will be re-claimed by SPI.beginTransaction when + // needed) + SPI.begin(PIN_CLK, PIN_DO, PIN_NSL, -1); // SCK, MISO, MOSI, no CS + + delay(50); // t_POR encoder stabilisation + + // ── Boot banner ────────────────────────────────────────────────────────── + Serial.println(F("\n╔════════════════════════════════════════════════════╗")); + Serial.println(F("║ AEAT-8800-Q24 | SSI + SPI | XIAO ESP32-C6 ║")); + Serial.println(F("╠════════════════════════════════════════════════════╣")); + Serial.println(F("║ SSI_SPI_SEL=21 CLK=19 NSL/DIN=18 DO=20 ║")); + Serial.println(F("╠════════════════════════════════════════════════════╣")); + + // Quick SSI position read + SSIFrame f = ssiReadPosition(); + double deg = (f.position / 65536.0) * 360.0; + Serial.printf("║ SSI Position : %5u counts (%.4f deg)\n", f.position, deg); + Serial.printf("║ Magnet Status : %s\n", magnetStatus(f.mhi, f.mlo)); + + // Quick SPI config read + uint8_t resReg = spiReadReg(REG_RESOLUTION); + uint16_t zp = readHardwareZero(); + Serial.printf("║ Resolution : %s\n", resolutionStr(resReg)); + Serial.printf("║ HW Zero Pos : %u counts (%.4f deg)\n", zp, + (zp / 65536.0) * 360.0); + Serial.println(F("╠════════════════════════════════════════════════════╣")); + Serial.println(F("║ Type 'h' for command help ║")); + Serial.println(F("╚════════════════════════════════════════════════════╝\n")); +} + +// ─── Main Loop +// ──────────────────────────────────────────────────────────────── + +void loop() { + // Handle serial commands (full-line parser) + handleSerial(); + + // Continuous SSI position stream (100 ms interval) + if (g_streaming && (millis() - g_lastStream >= 100)) { + g_lastStream = millis(); + SSIFrame f = ssiReadPosition(); + printPosition(f); + } +} diff --git a/firmware/16_bit_encoder/test.h b/firmware/16_bit_encoder/test.h new file mode 100644 index 0000000..1bc1f3f --- /dev/null +++ b/firmware/16_bit_encoder/test.h @@ -0,0 +1,584 @@ +/** + * ============================================================ + * AEAT-8800-Q24 — 16-bit Absolute Magnetic Rotary Encoder + * Board : Seeed Studio XIAO ESP32-C6 + * + * ── DUAL PROTOCOL ──────────────────────────────────────────── + * • SSI (SSI_SPI_SEL = HIGH) — reads real-time absolute position + * • SPI (SSI_SPI_SEL = LOW) — reads / writes configuration registers + * NOTE: Position data is ONLY available via SSI. The SPI interface + * is exclusively for OTP / configuration registers (no angle register). + * + * ── PIN MAPPING ────────────────────────────────────────────── + * Encoder Pin │ Pin Name │ GPIO │ Role + * ────────────┼───────────────┼───────┼────────────────────────────────── + * Pin 21 │ SSI_SPI_SEL │ GPIO21│ HIGH = SSI mode / LOW = SPI mode + * Pin 10 │ SCL / CLK │ GPIO19│ Shared clock (SCK) + * Pin 11 │ NSL / DIN │ GPIO18│ SSI enable (NSL) / SPI data-in (MOSI) + * Pin 12 │ DO / DOUT │ GPIO20│ Data output from encoder (MISO) + * ──────────────────────────────────────────────────────────── + * + * ── SSI READ SEQUENCE (16-bit mode — 20 clocks total) ──────── + * CLK idles HIGH, NSL idles HIGH, SSI_SPI_SEL HIGH. + * 1. Pull NSL LOW → encoder latches current angle into shift-reg + * 2. Wait ≥ 300 ns (tREQ) + * 3. Toggle CLK (HIGH→LOW→HIGH) × 20 — read DO on falling edge: + * Bits [19:4] = 16-bit absolute position D[15]..D[0] (MSB first) + * Bit [3] = Ready (1 = encoder output valid) + * Bit [2] = MHi (magnet too close) + * Bit [1] = MLo (magnet too far) + * Bit [0] = Parity (even parity over all 20 bits) + * 4. Pull NSL HIGH → end of frame + * 5. Wait ≥ 200 ns (tNSLH) before next read + * + * ── SPI PROTOCOL ───────────────────────────────────────────── + * SPI_MODE3 (CPOL=1, CPHA=1), ≤ 1 MHz + * Read cmd : 0b10_AAAAAA (8-bit) → dummy byte → 8-bit reply + * Write cmd : 0b01_AAAAAA (8-bit) → data byte + * + * ── REGISTER MAP (corrected from datasheet) ────────────────── + * Addr Register Notes + * 0x00 CustReserve0 User programmable + * 0x01 CustReserve1 User programmable + * 0x02 ZeroPos_L Zero Reset Position [7:0] + * 0x03 ZeroPos_H Zero Reset Position [15:8] + * 0x04 CustConfig0 UVW/PWM/I-width/UVW-pole config + * 0x05 CPR_Set1 CPR setting [7:4] / Hysteresis [3:0] + * 0x06 CPR_Set2 bit[7]=Dir, bit[6]=ZeroLatency, + * bits[5:4]=AbsResolution (00=16-bit, DEFAULT), + * bits[3:0]=CPR_Set2 + * 0x10 Lock Write 0xAB to unlock shadow regs + * 0x11 ProgCustRsv Write 0xA1 → OTP regs 0x00-0x01 + * 0x12 ProgZero Write 0xA2 → OTP regs 0x02-0x03 + * 0x13 ProgConfig Write 0xA3 → OTP regs 0x04-0x06 + * + * ── SERIAL COMMANDS ────────────────────────────────────────── + * p / P — Read position once (single fast SSI read) + * m / M — Precision measurement (4096 SSI samples, filtered) + * c / C — Toggle continuous precision stream (~3-4 readings/sec) + * s / S — Dump all SPI config registers + * r / R — Read register: r e.g. "r 06" + * w / W — Write register: w e.g. "w 10 AB" + * u / U — Unlock config registers (write 0xAB → Lock reg) + * z / Z — Set hardware zero (writes 0x0000 → ZeroPos shadow) + * b / B — Burn shadow regs to OTP ⚠ ONE-TIME, IRREVERSIBLE! + * h / H — Print this help + * + * Author : Swaraj Dangare + * ============================================================ + */ + + #include + #include + #include "soc/gpio_reg.h" // GPIO_OUT_W1TS_REG, GPIO_OUT_W1TC_REG, GPIO_IN_REG + + // ─── Pin Definitions ────────────────────────────────────────────────────────── + #define PIN_SEL D7 // SSI_SPI_SEL : HIGH = SSI, LOW = SPI (GPIO21) + #define PIN_CLK D10 // SCL / CLK : shared clock (GPIO19) + #define PIN_NSL D8 // NSL / DIN : SSI enable / SPI MOSI (GPIO18) + #define PIN_DO D9 // DO / DOUT : data from encoder (GPIO20) + + // ─── Fast GPIO Bit Masks ────────────────────────────────────────────────────── + // These GPIO numbers must match the GPIO column in the pin table above. + // Using direct register writes avoids digitalWrite() function call overhead + // in the inner SSI bit-bang loop. + #define GPIO_CLK_BIT (1UL << 19) // D10 = GPIO19 + #define GPIO_NSL_BIT (1UL << 18) // D8 = GPIO18 + #define GPIO_DO_BIT (1UL << 20) // D9 = GPIO20 + + #define FAST_CLK_HIGH() REG_WRITE(GPIO_OUT_W1TS_REG, GPIO_CLK_BIT) + #define FAST_CLK_LOW() REG_WRITE(GPIO_OUT_W1TC_REG, GPIO_CLK_BIT) + #define FAST_NSL_HIGH() REG_WRITE(GPIO_OUT_W1TS_REG, GPIO_NSL_BIT) + #define FAST_NSL_LOW() REG_WRITE(GPIO_OUT_W1TC_REG, GPIO_NSL_BIT) + #define FAST_READ_DO() ((REG_READ(GPIO_IN_REG) >> 20) & 1u) + + // ─── SSI Settings ───────────────────────────────────────────────────────────── + // Half-period of 1 us → ~500 kHz SSI clock. Datasheet max is 10 MHz. + // 20 bits for 16-bit mode: 16 position + Ready + MHi + MLo + Parity. + #define SSI_HALF_PERIOD_US 5 + #define SSI_TOTAL_BITS 20 + + // ─── SPI Settings ───────────────────────────────────────────────────────────── + // SPI_MODE3: CPOL=1 (idle HIGH), CPHA=1 (sample on rising edge) + SPISettings spiCfg(500000UL, MSBFIRST, SPI_MODE3); + + // ─── Register Addresses ─────────────────────────────────────────────────────── + #define REG_CUST_RESERVE_0 0x00 + #define REG_CUST_RESERVE_1 0x01 + #define REG_ZERO_POS_L 0x02 + #define REG_ZERO_POS_H 0x03 + #define REG_CUST_CONFIG_0 0x04 + #define REG_CPR_SET1 0x05 + #define REG_CPR_SET2 0x06 // bits[5:4] = absolute resolution + #define REG_LOCK 0x10 + #define REG_PROG_CUST_RSV 0x11 + #define REG_PROG_ZERO 0x12 + #define REG_PROG_CONFIG 0x13 + + // ─── Command / Key Bytes ────────────────────────────────────────────────────── + #define CMD_READ 0x80 // 0b10xxxxxx + #define CMD_WRITE 0x40 // 0b01xxxxxx + #define UNLOCK_KEY 0xAB + + // ─── Absolute Resolution (REG_CPR_SET2 bits [5:4]) ─────────────────────────── + // 00 = 16-bit (65536 cpr) — factory default; what we want + // 01 = 14-bit (16384 cpr) + // 10 = 12-bit (4096 cpr) + // 11 = 10-bit (1024 cpr) + #define RES_SHIFT 4 + #define RES_MASK 0x03 + + // ─── Filtering Constants ────────────────────────────────────────────────────── + #define NUM_BLOCKS 16 + #define SAMPLES_PER_BLOCK 256 + // TOTAL_SAMPLES = 4096 + + // ─── SSI Frame ─────────────────────────────────────────────────────────────── + struct SSIFrame { + uint16_t position; // absolute angle 0–65535 (16-bit) + bool ready; // 1 = encoder output is valid + bool mhi; // magnet too close + bool mlo; // magnet too far + bool parityOk; // even parity check passed + }; + + // ─── State ──────────────────────────────────────────────────────────────────── + static bool g_streaming = false; + + // ─── Mode Switching ─────────────────────────────────────────────────────────── + + static void enterSSI() { + SPI.endTransaction(); + pinMode(PIN_CLK, OUTPUT); + pinMode(PIN_NSL, OUTPUT); + pinMode(PIN_DO, INPUT); + digitalWrite(PIN_CLK, HIGH); + digitalWrite(PIN_NSL, HIGH); + digitalWrite(PIN_SEL, HIGH); + delayMicroseconds(2); + } + + static void enterSPI() { + digitalWrite(PIN_SEL, LOW); + delayMicroseconds(2); + SPI.beginTransaction(spiCfg); + } + + static void exitSPI() { + SPI.endTransaction(); + digitalWrite(PIN_SEL, HIGH); + delayMicroseconds(2); + } + + // ─── SSI Raw Read ───────────────────────────────────────────────────────────── + // Reads 20 bits via fast bit-banged SSI using direct GPIO register access. + // Assumes SSI mode is already active (enterSSI() called before this). + // Data is read on the falling edge of CLK per datasheet recommendation. + // Returns raw 20-bit frame: + // bits [19:4] = position D[15:0] + // bit [3] = Ready + // bit [2] = MHi + // bit [1] = MLo + // bit [0] = Parity + + static uint32_t ssiReadRaw() { + uint32_t raw = 0; + + FAST_NSL_LOW(); + delayMicroseconds(SSI_HALF_PERIOD_US); // tREQ >= 300 ns + + for (int i = 0; i < SSI_TOTAL_BITS; i++) { + FAST_CLK_LOW(); + delayMicroseconds(SSI_HALF_PERIOD_US); // data valid on falling edge + raw = (raw << 1) | FAST_READ_DO(); + FAST_CLK_HIGH(); + delayMicroseconds(SSI_HALF_PERIOD_US); + } + + FAST_NSL_HIGH(); + delayMicroseconds(SSI_HALF_PERIOD_US); // tNSLH >= 200 ns + return raw; + } + + // ─── Parity & Frame Parsing ─────────────────────────────────────────────────── + + // Even parity: XOR of all 20 bits must be 0. + static inline bool checkParity20(uint32_t raw) { + return __builtin_parity(raw & 0xFFFFF) == 0; + } + + static SSIFrame parseSSIFrame(uint32_t raw) { + SSIFrame f; + f.position = (uint16_t)((raw >> 4) & 0xFFFF); + f.ready = (raw >> 3) & 1u; + f.mhi = (raw >> 2) & 1u; + f.mlo = (raw >> 1) & 1u; + f.parityOk = checkParity20(raw); + return f; + } + + // ─── Single-Shot SSI Read ───────────────────────────────────────────────────── + + SSIFrame ssiReadPosition() { + enterSSI(); + return parseSSIFrame(ssiReadRaw()); + } + + // ─── Filtering ──────────────────────────────────────────────────────────────── + + // Robust mean: average of samples within 1.5 LSB of the initial mean. + // Outliers (noise spikes > 1.5 counts away) are discarded before re-averaging. + static double getRobustMean(const uint16_t* samples, int size) { + double sum = 0; + for (int i = 0; i < size; i++) sum += samples[i]; + double initMean = sum / size; + + double rSum = 0; + int count = 0; + for (int i = 0; i < size; i++) { + if (abs((double)samples[i] - initMean) < 1.5) { + rSum += samples[i]; + count++; + } + } + return (count > 0) ? rSum / count : initMean; + } + + static double getMedian(double* values, int size) { + std::sort(values, values + size); + return values[size / 2]; + } + + // 4096-sample precision read: + // 16 blocks x 256 samples -> robust mean per block -> median of block means. + // Any sample that fails parity or has Ready=0 is silently discarded and + // retried so every block always accumulates exactly 256 valid counts. + // Returns angle in degrees (0.0 to 360.0). + double getUltraPrecisionReading() { + double blockMeans[NUM_BLOCKS]; + uint16_t blockSamples[SAMPLES_PER_BLOCK]; + + enterSSI(); + + for (int b = 0; b < NUM_BLOCKS; b++) { + for (int s = 0; s < SAMPLES_PER_BLOCK; ) { + uint32_t raw = ssiReadRaw(); + if (!checkParity20(raw) || !((raw >> 3) & 1u)) { + continue; // bad sample — retry this slot + } + blockSamples[s] = (uint16_t)((raw >> 4) & 0xFFFF); + s++; + } + blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); + } + + return (getMedian(blockMeans, NUM_BLOCKS) * 360.0) / 65536.0; + } + + // ─── SPI Register Helpers ───────────────────────────────────────────────────── + + uint8_t spiReadReg(uint8_t addr) { + enterSPI(); + SPI.transfer(CMD_READ | (addr & 0x3F)); + uint8_t val = SPI.transfer(0xFF); + exitSPI(); + delayMicroseconds(5); + return val; + } + + void spiWriteReg(uint8_t addr, uint8_t data) { + enterSPI(); + SPI.transfer(CMD_WRITE | (addr & 0x3F)); + SPI.transfer(data); + exitSPI(); + delayMicroseconds(5); + } + + void unlockRegisters() { + spiWriteReg(REG_LOCK, UNLOCK_KEY); + delayMicroseconds(10); + } + + uint16_t readHardwareZero() { + uint8_t lo = spiReadReg(REG_ZERO_POS_L); + uint8_t hi = spiReadReg(REG_ZERO_POS_H); + return ((uint16_t)hi << 8) | lo; + } + + void writeHardwareZero(uint16_t zp) { + unlockRegisters(); + spiWriteReg(REG_ZERO_POS_L, zp & 0xFF); + spiWriteReg(REG_ZERO_POS_H, (zp >> 8) & 0xFF); + } + + // ─── Resolution Helpers (REG_CPR_SET2 0x06 bits [5:4]) ─────────────────────── + + static uint8_t getResolutionField() { + return (spiReadReg(REG_CPR_SET2) >> RES_SHIFT) & RES_MASK; + } + + static const char* resolutionStr(uint8_t field) { + switch (field & RES_MASK) { + case 0x00: return "16-bit (65536 cpr)"; + case 0x01: return "14-bit (16384 cpr)"; + case 0x02: return "12-bit (4096 cpr)"; + case 0x03: return "10-bit (1024 cpr)"; + default: return "unknown"; + } + } + + // ─── Helpers ────────────────────────────────────────────────────────────────── + + static const char* magnetStatus(bool mhi, bool mlo) { + if (mhi) return "TOO CLOSE"; + if (mlo) return "TOO FAR"; + return "OK"; + } + + // ─── Serial Output Helpers ──────────────────────────────────────────────────── + + void printPosition(const SSIFrame& f) { + double deg = (f.position / 65536.0) * 360.0; + Serial.printf("[SSI] Pos=%5u Angle=%8.4f deg Magnet=%-9s Ready=%u Parity=%s\n", + f.position, deg, + magnetStatus(f.mhi, f.mlo), + (uint8_t)f.ready, + f.parityOk ? "OK" : "FAIL"); + } + + void printAllRegisters() { + Serial.println(F("\n╔══════════════════════════════════════════════╗")); + Serial.println(F("║ AEAT-8800-Q24 — SPI Register Dump ║")); + Serial.println(F("╠══════════════════════════════════════════════╣")); + + struct { uint8_t addr; const char* name; } regs[] = { + { REG_CUST_RESERVE_0, "CustReserve0 (0x00)" }, + { REG_CUST_RESERVE_1, "CustReserve1 (0x01)" }, + { REG_ZERO_POS_L, "ZeroPos_L (0x02)" }, + { REG_ZERO_POS_H, "ZeroPos_H (0x03)" }, + { REG_CUST_CONFIG_0, "CustConfig0 (0x04)" }, + { REG_CPR_SET1, "CPR_Set1 (0x05)" }, + { REG_CPR_SET2, "CPR_Set2 (0x06)" }, + { REG_LOCK, "Lock (0x10)" }, + }; + + for (auto& r : regs) { + uint8_t val = spiReadReg(r.addr); + Serial.printf("║ %-22s 0x%02X (%3u)\n", r.name, val, val); + } + + uint16_t zp = readHardwareZero(); + uint8_t res = getResolutionField(); + Serial.println(F("╠══════════════════════════════════════════════╣")); + Serial.printf("║ HW Zero Pos : %5u (%.4f deg)\n", zp, (zp / 65536.0) * 360.0); + Serial.printf("║ Resolution : %s\n", resolutionStr(res)); + Serial.printf("║ [0x06][5:4] : 0b%u%u (raw field)\n", + (res >> 1) & 1u, res & 1u); + Serial.println(F("╚══════════════════════════════════════════════╝\n")); + } + + void printHelp() { + Serial.println(F("\n╔══════════════════════════════════════════════════════╗")); + Serial.println(F("║ AEAT-8800-Q24 Serial Command Reference ║")); + Serial.println(F("╠══════════════════════════════════════════════════════╣")); + Serial.println(F("║ p — Read position once (single SSI read) ║")); + Serial.println(F("║ m — Precision reading (4096 SSI samples) ║")); + Serial.println(F("║ c — Toggle continuous precision stream ║")); + Serial.println(F("║ s — Dump all SPI config registers ║")); + Serial.println(F("║ r — Read register at hex addr (e.g. r 06) ║")); + Serial.println(F("║ w — Write val to reg (e.g. w 10 AB) ║")); + Serial.println(F("║ u — Unlock config registers (0xAB -> Lock) ║")); + Serial.println(F("║ z — Set zero position = 0x0000 (shadow only) ║")); + Serial.println(F("║ burn — Burn shadow regs to OTP WARNING: ║")); + Serial.println(F("║ Type exactly 'burn' + Enter ║")); + Serial.println(F("║ h — Show this help ║")); + Serial.println(F("╚══════════════════════════════════════════════════════╝\n")); + } + + // ─── Command Parser ─────────────────────────────────────────────────────────── + + void handleSerial() { + if (!Serial.available()) return; + + String line = Serial.readStringUntil('\n'); + line.trim(); + if (line.length() == 0) return; + + char cmd = (char)tolower((unsigned char)line[0]); + + switch (cmd) { + + // ── Single fast SSI read ─────────────────────────────────────────────── + case 'p': { + SSIFrame f = ssiReadPosition(); + printPosition(f); + break; + } + + // ── Precision measurement (4096 SSI samples, filtered) ──────────────── + case 'm': { + Serial.println(F("[PREC] Measuring (4096 SSI samples)...")); + uint32_t t0 = millis(); + double deg = getUltraPrecisionReading(); + Serial.printf("[PREC] Angle=%10.5f deg (%u ms)\n", deg, millis() - t0); + break; + } + + // ── Continuous precision stream toggle ──────────────────────────────── + case 'c': + g_streaming = !g_streaming; + Serial.printf("[INFO] Continuous precision stream %s\n", + g_streaming ? "ON (each sample = 4096 reads)" : "OFF"); + break; + + // ── Register dump ───────────────────────────────────────────────────── + case 's': + printAllRegisters(); + break; + + // ── Read register "r " ────────────────────────────────────────── + case 'r': { + if (line.length() < 3) { + Serial.println(F("[ERR] Usage: r e.g. 'r 06'")); + break; + } + uint8_t addr = (uint8_t)strtoul(line.c_str() + 2, nullptr, 16); + uint8_t val = spiReadReg(addr); + Serial.printf("[SPI-RD] Reg 0x%02X = 0x%02X (%u)\n", addr, val, val); + break; + } + + // ── Write register "w " ─────────────────────────────────── + case 'w': { + if (line.length() < 5) { + Serial.println(F("[ERR] Usage: w e.g. 'w 10 AB'")); + break; + } + char* ptr = nullptr; + uint8_t addr = (uint8_t)strtoul(line.c_str() + 2, &ptr, 16); + uint8_t val = (uint8_t)strtoul(ptr, nullptr, 16); + spiWriteReg(addr, val); + uint8_t rb = spiReadReg(addr); + Serial.printf("[SPI-WR] Reg 0x%02X <- 0x%02X | Readback: 0x%02X %s\n", + addr, val, rb, (rb == val) ? "OK" : "MISMATCH!"); + break; + } + + // ── Unlock registers ────────────────────────────────────────────────── + case 'u': + unlockRegisters(); + Serial.println(F("[SPI] Registers unlocked (0xAB written to Lock reg 0x10)")); + break; + + // ── Set zero position ───────────────────────────────────────────────── + case 'z': { + Serial.println(F("[ZERO] Writing 0x0000 to ZeroPos shadow registers...")); + writeHardwareZero(0x0000); + uint16_t rb = readHardwareZero(); + Serial.printf("[ZERO] Readback: 0x%04X — %s\n", rb, + rb == 0x0000 ? "OK" : "MISMATCH!"); + break; + } + + // ── OTP burn — requires typing "burn" in full ───────────────────────── + case 'b': { + if (!line.equalsIgnoreCase("burn")) { + Serial.println(F("[WARN] OTP BURN is IRREVERSIBLE!")); + Serial.println(F("[WARN] Type exactly 'burn' + Enter to confirm.")); + break; + } + Serial.println(F("[OTP] Unlocking registers...")); + unlockRegisters(); + Serial.println(F("[OTP] Burning Customer Config OTP (regs 0x04-0x06)...")); + spiWriteReg(REG_PROG_CONFIG, 0xA3); + delayMicroseconds(200); + Serial.println(F("[OTP] Burning Zero Position OTP (regs 0x02-0x03)...")); + spiWriteReg(REG_PROG_ZERO, 0xA2); + delayMicroseconds(200); + Serial.println(F("[OTP] Done. Power-cycle the encoder to verify.")); + break; + } + + // ── Help ────────────────────────────────────────────────────────────── + case 'h': + default: + printHelp(); + break; + } + } + + // ─── Setup ──────────────────────────────────────────────────────────────────── + + void setup() { + Serial.begin(115200); + delay(300); + + // GPIO init — idle state: CLK HIGH, NSL HIGH, SEL HIGH (SSI mode) + pinMode(PIN_SEL, OUTPUT); + pinMode(PIN_CLK, OUTPUT); + pinMode(PIN_NSL, OUTPUT); + pinMode(PIN_DO, INPUT); + digitalWrite(PIN_SEL, HIGH); + digitalWrite(PIN_CLK, HIGH); + digitalWrite(PIN_NSL, HIGH); + + // Hardware SPI init (SCK=PIN_CLK, MISO=PIN_DO, MOSI=PIN_NSL, no CS) + SPI.begin(PIN_CLK, PIN_DO, PIN_NSL, -1); + + delay(50); // t_POR encoder power-on stabilisation + + // ── Boot banner ────────────────────────────────────────────────────────── + Serial.println(F("\n╔════════════════════════════════════════════════════╗")); + Serial.println(F("║ AEAT-8800-Q24 | SSI + SPI | XIAO ESP32-C6 ║")); + Serial.println(F("╠════════════════════════════════════════════════════╣")); + Serial.println(F("║ SEL=GPIO21 CLK=GPIO19 NSL=GPIO18 DO=GPIO20 ║")); + Serial.println(F("╠════════════════════════════════════════════════════╣")); + + // Boot SSI read — single fast sample for startup status + SSIFrame f = ssiReadPosition(); + double bootDeg = (f.position / 65536.0) * 360.0; + Serial.printf("║ SSI Position : %5u counts (%.4f deg)\n", + f.position, bootDeg); + Serial.printf("║ Magnet Status : %s\n", magnetStatus(f.mhi, f.mlo)); + Serial.printf("║ Encoder Ready : %s\n", f.ready ? "YES" : "NO"); + Serial.printf("║ Frame Parity : %s\n", f.parityOk ? "OK" : "FAIL"); + Serial.println(F("╠════════════════════════════════════════════════════╣")); + + // ── Auto-verify 16-bit resolution (REG_CPR_SET2 bits [5:4]) ───────────── + uint8_t reg06 = spiReadReg(REG_CPR_SET2); + uint8_t resField = (reg06 >> RES_SHIFT) & RES_MASK; + Serial.printf("║ Resolution : %s\n", resolutionStr(resField)); + + if (resField != 0x00) { + Serial.println(F("╠════════════════════════════════════════════════════╣")); + Serial.println(F("║ [WARN] Not 16-bit! Correcting shadow register... ║")); + unlockRegisters(); + spiWriteReg(REG_CPR_SET2, reg06 & (uint8_t)~(RES_MASK << RES_SHIFT)); + uint8_t verify = (spiReadReg(REG_CPR_SET2) >> RES_SHIFT) & RES_MASK; + Serial.printf("║ Resolution now: %s\n", resolutionStr(verify)); + Serial.println(F("║ Shadow updated. Type 'burn' to make permanent. ║")); + } else { + Serial.println(F("║ 16-bit confirmed (factory default). ║")); + } + + uint16_t zp = readHardwareZero(); + Serial.printf("║ HW Zero Pos : %u counts (%.4f deg)\n", + zp, (zp / 65536.0) * 360.0); + Serial.println(F("╠════════════════════════════════════════════════════╣")); + Serial.println(F("║ Type 'h' for command help ║")); + Serial.println(F("╚════════════════════════════════════════════════════╝\n")); + } + + // ─── Main Loop ──────────────────────────────────────────────────────────────── + + void loop() { + handleSerial(); + + // Precision stream: the ~250 ms measurement time is the natural rate limiter. + if (g_streaming) { + uint32_t t0 = millis(); + double deg = getUltraPrecisionReading(); + Serial.printf("[STREAM] Angle=%10.5f deg (%u ms)\n", deg, millis() - t0); + } + } + \ No newline at end of file diff --git a/firmware/dual_aeat8800_independent/dual_aeat8800_independent.ino b/firmware/dual_aeat8800_independent/dual_aeat8800_independent.ino new file mode 100644 index 0000000..e6a7a06 --- /dev/null +++ b/firmware/dual_aeat8800_independent/dual_aeat8800_independent.ino @@ -0,0 +1,1184 @@ +/** jgkrsjgklrslgrs + * ============================================================ + * AEAT-8800-Q24 — Fully Independent Dual Encoder + * Board : Seeed Studio XIAO ESP32-C6 + * + * ── FULLY INDEPENDENT WIRING ───────────────────────────────── + * Every encoder has its own dedicated CLK, DIN/NSL, DO, and SEL. + * No pins are shared between encoders — eliminates all bus + * conflicts and DOUT tri-state contention. + * + * ── DEFAULT PIN ASSIGNMENT ─────────────────────────────────── + * Signal │ Encoder 1 │ Encoder 2 + * ────────────┼─────────────────┼───────────────── + * CLK │ D8 / GPIO19 │ D5 / GPIO23 + * DIN / NSL │ D10 / GPIO18 │ D2 / GPIO2 + * DO / DOUT │ D9 / GPIO20 │ D1 / GPIO1 + * SEL │ D3 / GPIO21 │ D4 / GPIO22 + * + * ── ANTENNA PINS ───────────────────────────────────────────── + * WIFI_ENABLE GPIO3 — LOW = activate RF switch control + * WIFI_ANT_CONFIG GPIO14 — HIGH = select external antenna + * + * ── KEY TIMING (datasheet Fig. 8 & timing table) ───────────── + * Symbol │ Min │ Unit │ Datasheet description + * ──────────┼──────┼──────┼────────────────────────────────────────────── + * tsw(SEL) │ 1 │ µs │ SSI_SPI_SEL switch time + * tREQ │ 300 │ ns │ SCL high time between NSL falling edge and + * │ │ │ first SCL falling edge + * tREQ2 │ 200 │ ns │ NSL low time after rising edge of last clock + * │ │ │ period for an SSI read + * tNSLH │ 200 │ ns │ NSL high time between 2 successive SSI reads + * ──────────┼──────┼──────┼────────────────────────────────────────────── + * Notes (datasheet p.15): + * • CLK = 1 when inactive; DIN = 1 when inactive. + * • CLK must be HIGH when switching between SSI and SPI modes. + * • NSL must be held HIGH for at least 3 ms after power-up before + * the first SSI read. + * • The user is advised to read from the SSI falling edge. + * • 16-bit resolution → 20 total bits (16 pos + Ready + MHi + MLo + Parity) + * All margins use SSI_HALF_US = 5 µs (>> all minimum requirements). + * + * ── BOOT WIZARD ────────────────────────────────────────────── + * Set RUN_SETUP_WIZARD = true to prompt over Serial (115200 baud): + * 1. How many encoders are connected [1/2] + * 2. Which slot (only if 1 encoder) + * 3. Use default pins, or enter custom GPIO numbers + * Set RUN_SETUP_WIZARD = false to skip the wizard: both encoders on + * default pins, continuous stream (c) starts automatically. + * + * ── SERIAL COMMANDS ────────────────────────────────────────── + * p — Read position (SSI, one-shot, enabled encoders) + * c — Toggle continuous stream (both enabled, 100 ms) + * o — Software offset: current pos reports as (both encoders) + * o clear — Remove software offset (revert to raw hardware angles) + * 1 / 2 — Select active encoder for SPI commands + * s — Dump SPI config registers (active encoder) + * r — Read register at hex address e.g. "r 07" + * w — Write register e.g. "w 10 AB" + * u — Unlock config registers + * z — HW zero position = 0x0000 (SPI shadow only) + * burn — Burn OTP ⚠ IRREVERSIBLE (type "burn" exactly) + * h — Show this help + * + * Author : Swaraj Dangare + * ============================================================ + */ + +#include // For std::sort +#include // For sin, cos, atan2, PI + +// ─── Hybrid Robust Filtering Settings ──────────────────────────────────────── +#define NUM_BLOCKS 16 +#define SAMPLES_PER_BLOCK 16 +#define OUTLIER_THRESHOLD \ + 20.0 // Allow some natural jitter, reject massive spikes + +// true = interactive boot wizard (encoder count, slot, custom pins) +// false = skip wizard; both encoders on default pins; start stream (c) immediately +static constexpr bool RUN_SETUP_WIZARD = false; + +// 16-bit encoder: 65536 counts/rev → ~0.0055° per count +#define ENCODER_CPR 65536 +static constexpr double ENCODER_CPR_D = 65536.0; +static constexpr double DEG_PER_COUNT = 360.0 / ENCODER_CPR_D; + +static uint16_t snapToCounts(double counts); +static double countsToDeg(uint16_t counts); + +// ─── Antenna Pins +// ───────────────────────────────────────────────────────────── +#define WIFI_ENABLE 3 // GPIO3 — RF switch control (LOW = active) +#define WIFI_ANT_CONFIG 14 // GPIO14 — Antenna select (HIGH = external) + +// ─── Default Encoder Pins +// ───────────────────────────────────────────────────── +#define DEFAULT_ENC1_CLK D8 // GPIO19 +#define DEFAULT_ENC1_DIN D10 // GPIO18 +#define DEFAULT_ENC1_DOUT D9 // GPIO20 +#define DEFAULT_ENC1_SEL D7 // GPIO21 + +#define DEFAULT_ENC2_CLK D5 // GPIO23 +#define DEFAULT_ENC2_DIN D3 // GPIO2 +#define DEFAULT_ENC2_DOUT D4 // GPIO1 +#define DEFAULT_ENC2_SEL D6 // GPIO22 + +// ─── Register Addresses +// ─────────────────────────────────────────────────────── +#define REG_CUST_RESERVE_0 0x00 +#define REG_CUST_RESERVE_1 0x01 +#define REG_ZERO_POS_L 0x02 +#define REG_ZERO_POS_H 0x03 +#define REG_CUST_CONFIG_0 0x04 +#define REG_CPR_SET1 0x05 +#define REG_CPR_SET2 0x06 +#define REG_RESOLUTION 0x07 +#define REG_VCC 0x0A +#define REG_LOCK 0x10 +#define REG_PROG_CUST 0x11 + +// ─── Register Values +// ────────────────────────────────────────────────────────── +#define UNLOCK_KEY 0xAB +#define PROG_KEY 0xA1 +#define VAL_VCC 0x00 // 0x0A [1]=0 → 3.3 V +#define VAL_CFG0 0x00 // 0x04 PWM mode, 1 pole-pair +// 0x05 [7:4]=CPR1, [3:0]=Hysteresis (datasheet Table 3) +// CPR1 0b0100 → 512 CPR (ABI; absolute-only uses CPR_Set2 in 0x06) +// Hyst 0b0010 → 0.01 mechanical degree (mdeg) +#define VAL_CPR1 0x42 +#define VAL_CPR2 0x04 // 0x06 16-bit abs, CW, zero-latency OFF + +// ─── SSI Timing +// ─────────────────────────────────────────────────────────────── Timing +// constants from AEAT-8800-Q24 datasheet timing table (Fig. 8): +// tREQ >= 300 ns SCL high time between NSL falling edge and first SCL +// falling edge tREQ2 >= 200 ns NSL low time after rising edge of last +// clock period for an SSI read tNSLH >= 200 ns NSL high time between 2 +// successive SSI reads tsw(SEL) >= 1 µs SSI_SPI_SEL switch time +// SSI_HALF_US = 5 µs gives 7–25× margin over minimums. +// SSI_MONOFLOP_US: encoder internal load-cycle recovery after NSL goes HIGH. +#define SSI_TOTAL_BITS 20 // 16-bit position + Ready + MHi + MLo + Parity +#define SSI_HALF_US 1 // 1 us half-period -> ~500 kHz SSI clock (Fast for 30Hz) +#define SSI_MONOFLOP_US 20 // monoflop recovery after NSL HIGH + +// ─── Encoder Config +// ─────────────────────────────────────────────────────────── + +struct EncoderConfig { + uint8_t pinCLK; + uint8_t pinDIN; + uint8_t pinDOUT; + uint8_t pinSEL; +}; + +// ─── AEAT8800 Class +// ─────────────────────────────────────────────────────────── Each instance is +// completely self-contained: CLK, DIN, DOUT, and SEL are all per-instance. No +// pins are shared with any other instance. + +class AEAT8800 { +public: + AEAT8800(uint8_t clk, uint8_t din, uint8_t dout, uint8_t sel) + : _clk(clk), _din(din), _dout(dout), _sel(sel) {} + + /** Assert SEL HIGH → SSI mode. */ + void select(); + + /** Assert SEL LOW → SPI/idle mode. */ + void deselect(); + + /** Bit-bang one SPI register read using this encoder's CLK and DIN. */ + uint8_t spiRead(uint8_t addr); + + /** Bit-bang one SPI register write using this encoder's CLK and DIN. */ + void spiWrite(uint8_t addr, uint8_t data); + + /** Unlock and write all required OTP shadow registers with readback verify. + * Returns true on success. Failure is non-fatal; SSI reads still work. */ + bool configure(); + + /** Perform one 20-bit SSI read. Stores position in pos (0–65535). + * Precondition: select() must have been called. + * Returns false if Ready=0 or parity fails. */ + bool readAngleRaw(uint16_t &pos, bool silent = false); + + /** Read angle in degrees [0, 360). Returns -1.0f on error. */ + float readAngleDegrees(); + +private: + uint8_t _clk, _din, _dout, _sel; +}; + +// ─── select / deselect +// ──────────────────────────────────────────────────────── + +void AEAT8800::select() { + // CLK must be HIGH before SEL transition (datasheet p.15, p.21) + digitalWrite(_clk, HIGH); + digitalWrite(_sel, HIGH); + delayMicroseconds(1); // tsw(SEL) >= 1 µs +} + +void AEAT8800::deselect() { + digitalWrite(_sel, LOW); + delayMicroseconds(1); +} + +// ─── SPI Write +// ──────────────────────────────────────────────────────────────── + +void AEAT8800::spiWrite(uint8_t addr, uint8_t data) { + int prevSel = digitalRead(_sel); + + digitalWrite(_clk, HIGH); + digitalWrite(_sel, LOW); + delayMicroseconds(1); // tsw(SEL) >= 1 µs + + // 16-bit frame: opcode 0b01 (2b) + addr (6b) + data (8b), MSB first + uint16_t word = (0b01u << 14) | ((addr & 0x3F) << 8) | (data & 0xFF); + for (int i = 15; i >= 0; i--) { + digitalWrite(_din, (word >> i) & 1u); // DIN valid before CLK fall + digitalWrite(_clk, LOW); // falling edge + delayMicroseconds(SSI_HALF_US); + digitalWrite(_clk, HIGH); // rising edge — encoder captures DIN + delayMicroseconds(SSI_HALF_US); + } + + digitalWrite(_din, HIGH); // DIN back to idle HIGH + digitalWrite(_clk, HIGH); // CLK must be HIGH before SEL change + digitalWrite(_sel, prevSel); +} + +// ─── SPI Read +// ───────────────────────────────────────────────────────────────── + +uint8_t AEAT8800::spiRead(uint8_t addr) { + int prevSel = digitalRead(_sel); + + digitalWrite(_clk, HIGH); + digitalWrite(_sel, LOW); + delayMicroseconds(1); // tsw(SEL) >= 1 µs + + // 8-bit command: opcode 0b10 (2b) + addr (6b) + uint8_t cmd = (0b10u << 6) | (addr & 0x3F); + for (int i = 7; i >= 0; i--) { + digitalWrite(_din, (cmd >> i) & 1u); + digitalWrite(_clk, LOW); + delayMicroseconds(SSI_HALF_US); + digitalWrite(_clk, HIGH); // rising edge — encoder captures DIN + delayMicroseconds(SSI_HALF_US); + } + digitalWrite(_din, HIGH); // release DIN; encoder drives DOUT for reply + + uint8_t result = 0; + for (int i = 0; i < 8; i++) { + digitalWrite(_clk, LOW); // falling edge + delayMicroseconds(SSI_HALF_US); // DOUT valid <= 200 ns after CLK fall + digitalWrite(_clk, HIGH); // rising edge — master captures DOUT + result = + (result << 1) | + (uint8_t)digitalRead(_dout); // sample on rising edge (datasheet p.20) + delayMicroseconds(SSI_HALF_US); + } + + digitalWrite(_clk, HIGH); + digitalWrite(_din, HIGH); + digitalWrite(_sel, prevSel); + return result; +} + +// ─── Configure +// ──────────────────────────────────────────────────────────────── + +bool AEAT8800::configure() { + // Unlock must be the very first SPI write (datasheet p.10 note 3) + spiWrite(REG_LOCK, UNLOCK_KEY); + delayMicroseconds(10); + + struct RegTarget { + uint8_t addr, val; + const char *name; + }; + static const RegTarget targets[] = { + {REG_VCC, VAL_VCC, "0x0A (VCC)"}, + {REG_CUST_CONFIG_0, VAL_CFG0, "0x04 (CFG0)"}, + {REG_CPR_SET1, VAL_CPR1, "0x05 (CPR1)"}, + {REG_CPR_SET2, VAL_CPR2, "0x06 (CPR2)"}, + }; + + for (const auto &r : targets) { + spiWrite(r.addr, r.val); + delayMicroseconds(5); + uint8_t rb = spiRead(r.addr); + if (rb != r.val) { + Serial.printf("[ERR] Reg %s verify: wrote 0x%02X readback 0x%02X\n", + r.name, r.val, rb); + return false; + } + } + + deselect(); + return true; +} + +// ─── SSI Read +// ───────────────────────────────────────────────────────────────── + +bool AEAT8800::readAngleRaw(uint16_t &pos, bool silent) { + // Precondition: select() already called — SEL is HIGH, encoder in SSI mode + + // CLK = 1 when inactive; DIN = 1 when inactive (datasheet note) + digitalWrite(_clk, HIGH); + digitalWrite(_din, HIGH); + delayMicroseconds(SSI_HALF_US); // tNSLH >= 200 ns: NSL high time between 2 + // successive SSI reads + + // NSL LOW → shift mode: encoder freezes position into shift register (Fig. 8) + digitalWrite(_din, LOW); + delayMicroseconds(SSI_HALF_US); // tREQ >= 300 ns: SCL high time between NSL + // falling edge and first SCL falling edge + + uint32_t raw = 0; + for (int i = 0; i < SSI_TOTAL_BITS; i++) { + digitalWrite(_clk, + LOW); // SCL falling edge — encoder shifts next bit onto DO + delayMicroseconds(SSI_HALF_US); // read from SSI falling edge per datasheet + // (data stable after CLK fall) + raw = (raw << 1) | + (uint32_t)digitalRead( + _dout); // sample DO after falling edge (datasheet Fig. 8, p.15) + digitalWrite(_clk, HIGH); // SCL rising edge + delayMicroseconds(SSI_HALF_US); // CLK high hold (CLK = 1 when inactive) + } + + delayMicroseconds(SSI_HALF_US); // tREQ2 >= 200 ns: NSL low time after rising + // edge of last clock period for an SSI read + + // End of frame: NSL HIGH → load mode (encoder resumes tracking position) + digitalWrite(_din, HIGH); + delayMicroseconds(SSI_MONOFLOP_US); // monoflop recovery: encoder completes + // internal load cycle + + // Unpack 20-bit frame (datasheet Fig. 8–9) + uint8_t parity = (raw >> 0) & 0x1u; + uint8_t mlo = (raw >> 1) & 0x1u; + uint8_t mhi = (raw >> 2) & 0x1u; + uint8_t ready = (raw >> 3) & 0x1u; + uint16_t position = (uint16_t)((raw >> 4) & 0xFFFFu); + + (void)parity; // checked implicitly via __builtin_popcount below + + if (!ready) { + if (!silent) + Serial.printf("[ERR] Ready=0 (raw=0x%05X) — data not valid\n", + (unsigned)raw); + return false; + } + if (!silent) { + if (mhi) + Serial.println(F("[WARN] MHi=1 — magnet too strong / too close")); + if (mlo) + Serial.println(F("[WARN] MLo=1 — magnet too weak / too far")); + } + + // Even parity: total 1-bit count across all 20 raw bits must be even + if (__builtin_popcount((unsigned)raw) % 2 != 0) { + if (!silent) + Serial.printf("[ERR] Parity error (raw=0x%05X)\n", (unsigned)raw); + return false; + } + + pos = position; + return true; +} + +float AEAT8800::readAngleDegrees() { + uint16_t raw = 0; + if (!readAngleRaw(raw)) + return -1.0f; + return (float)countsToDeg(raw); +} + +// ─── Global State +// ───────────────────────────────────────────────────────────── + +static AEAT8800 *g_enc1 = nullptr; +static AEAT8800 *g_enc2 = nullptr; +static bool g_enc1Enabled = false; +static bool g_enc2Enabled = false; +static bool g_streaming = false; +static uint32_t g_lastStream = 0; +static uint8_t g_activeEnc = 1; // target encoder for SPI serial commands + +// Software angle offsets (degrees). Applied at display time only. +// Offset = rawDeg - targetDeg at the moment `o ` is sent. +static double g_offsetDeg1 = 0.0; +static double g_offsetDeg2 = 0.0; + +// ─── Setup Wizard Helpers +// ───────────────────────────────────────────────────── + +/** Block until a complete line is received on Serial. Returns trimmed string. + */ +static String readLine() { + while (!Serial.available()) { /* busy-wait */ + } + String s = Serial.readStringUntil('\n'); + s.trim(); + return s; +} + +/** Print a pin prompt, read a GPIO integer. Returns the chosen GPIO number. */ +static uint8_t promptPin(const char *label, uint8_t def) { + Serial.printf(" %-8s (default GPIO%-2u): ", label, def); + String s = readLine(); + if (s.length() == 0) { + Serial.printf("GPIO%u (default)\n", def); + return def; + } + uint8_t v = (uint8_t)s.toInt(); + Serial.printf("GPIO%u\n", v); + return v; +} + +/** Prompt for custom pin override for one encoder. Modifies cfg in-place. */ +static void promptCustomPins(const char *label, EncoderConfig &cfg, + const EncoderConfig &def) { + Serial.printf("\n Encoder %s — defaults:" + " CLK=GPIO%u DIN=GPIO%u DO=GPIO%u SEL=GPIO%u\n", + label, def.pinCLK, def.pinDIN, def.pinDOUT, def.pinSEL); + Serial.print(F(" Use default pins? [Y/n]: ")); + String ans = readLine(); + ans.toLowerCase(); + + if (ans == "n") { + cfg.pinCLK = promptPin("CLK", def.pinCLK); + cfg.pinDIN = promptPin("DIN", def.pinDIN); + cfg.pinDOUT = promptPin("DO", def.pinDOUT); + cfg.pinSEL = promptPin("SEL", def.pinSEL); + } else { + cfg = def; + Serial.println(F(" Using defaults.")); + } +} + +// ─── Setup Wizard +// ───────────────────────────────────────────────────────────── + +static void runSetupWizard(EncoderConfig &cfg1, EncoderConfig &cfg2) { + const EncoderConfig def1 = {DEFAULT_ENC1_CLK, DEFAULT_ENC1_DIN, + DEFAULT_ENC1_DOUT, DEFAULT_ENC1_SEL}; + const EncoderConfig def2 = {DEFAULT_ENC2_CLK, DEFAULT_ENC2_DIN, + DEFAULT_ENC2_DOUT, DEFAULT_ENC2_SEL}; + cfg1 = def1; + cfg2 = def2; + + Serial.println( + F("\n╔══════════════════════════════════════════════════════════╗")); + Serial.println( + F("║ AEAT-8800-Q24 — Encoder Setup Wizard ║")); + Serial.println( + F("╠══════════════════════════════════════════════════════════╣")); + Serial.println( + F("║ GPIO reference: ║")); + Serial.println( + F("║ D0=0 D1=1 D2=2 D3=21 D4=22 D5=23 ║")); + Serial.println( + F("║ D6=16 D7=17 D8=19 D9=20 D10=18 ║")); + Serial.println( + F("╠══════════════════════════════════════════════════════════╣")); + Serial.println( + F("║ Default pin layout (fully independent, no sharing): ║")); + Serial.println( + F("║ ENC1: CLK=19(D8) DIN=18(D10) DO=20(D9) SEL=21(D3) ║")); + Serial.println( + F("║ ENC2: CLK=23(D5) DIN=2(D2) DO=1(D1) SEL=22(D4) ║")); + Serial.println( + F("╚══════════════════════════════════════════════════════════╝\n")); + + // ── Step 1: number of encoders + // ──────────────────────────────────────────────── + uint8_t numEnc = 0; + while (numEnc != 1 && numEnc != 2) { + Serial.print(F("How many encoders are connected? [1/2]: ")); + String s = readLine(); + numEnc = (uint8_t)s.toInt(); + if (numEnc != 1 && numEnc != 2) + Serial.println(F(" Please enter 1 or 2.")); + } + + if (numEnc == 1) { + // ── Step 2 (single encoder): which slot + // ──────────────────────────────────── + uint8_t slot = 0; + while (slot != 1 && slot != 2) { + Serial.print(F("Which encoder slot is connected? [1/2]: ")); + String s = readLine(); + slot = (uint8_t)s.toInt(); + if (slot != 1 && slot != 2) + Serial.println(F(" Please enter 1 or 2.")); + } + g_enc1Enabled = (slot == 1); + g_enc2Enabled = (slot == 2); + g_activeEnc = slot; + } else { + g_enc1Enabled = true; + g_enc2Enabled = true; + g_activeEnc = 1; + } + + // ── Step 3: optional custom pins per enabled encoder + // ────────────────────────── + if (g_enc1Enabled) + promptCustomPins("1", cfg1, def1); + if (g_enc2Enabled) + promptCustomPins("2", cfg2, def2); + + // ── Configuration summary + // ───────────────────────────────────────────────────── + Serial.println( + F("\n╔══════════════════════════════════════════════════════════╗")); + Serial.println( + F("║ Configuration confirmed: ║")); + if (g_enc1Enabled) + Serial.printf( + "║ ENC1 CLK=GPIO%-2u DIN=GPIO%-2u DO=GPIO%-2u SEL=GPIO%-2u ║\n", + cfg1.pinCLK, cfg1.pinDIN, cfg1.pinDOUT, cfg1.pinSEL); + else + Serial.println( + F("║ ENC1 disabled ║")); + if (g_enc2Enabled) + Serial.printf( + "║ ENC2 CLK=GPIO%-2u DIN=GPIO%-2u DO=GPIO%-2u SEL=GPIO%-2u ║\n", + cfg2.pinCLK, cfg2.pinDIN, cfg2.pinDOUT, cfg2.pinSEL); + else + Serial.println( + F("║ ENC2 disabled ║")); + Serial.println( + F("╚══════════════════════════════════════════════════════════╝\n")); +} + +/** Apply built-in dual-encoder pin config and enable continuous stream. */ +static void applyDefaultDualConfig(EncoderConfig &cfg1, EncoderConfig &cfg2) { + cfg1 = {DEFAULT_ENC1_CLK, DEFAULT_ENC1_DIN, DEFAULT_ENC1_DOUT, + DEFAULT_ENC1_SEL}; + cfg2 = {DEFAULT_ENC2_CLK, DEFAULT_ENC2_DIN, DEFAULT_ENC2_DOUT, + DEFAULT_ENC2_SEL}; + g_enc1Enabled = true; + g_enc2Enabled = true; + g_activeEnc = 1; + g_streaming = true; +} + +// ─── Pin Init +// ───────────────────────────────────────────────────────────────── + +static void initEncoderPins(const EncoderConfig &cfg) { + pinMode(cfg.pinCLK, OUTPUT); + digitalWrite(cfg.pinCLK, HIGH); // CLK idles HIGH + pinMode(cfg.pinDIN, OUTPUT); + digitalWrite(cfg.pinDIN, HIGH); // DIN/NSL idles HIGH + pinMode(cfg.pinDOUT, INPUT); + pinMode(cfg.pinSEL, OUTPUT); + digitalWrite(cfg.pinSEL, LOW); // SPI/idle mode +} + +// ─── Runtime Helpers ───────────────────────────────────────────────────────── + +/** Snap a fractional count to the nearest integer encoder step [0, CPR). */ +static uint16_t snapToCounts(double counts) { + long c = (long)(counts + 0.5); + c %= ENCODER_CPR; + if (c < 0) + c += ENCODER_CPR; + return (uint16_t)c; +} + +/** Convert encoder counts to degrees, quantized to 3 decimal places. */ +static double countsToDeg(uint16_t counts) { + return round(counts * DEG_PER_COUNT * 1000.0) / 1000.0; +} + +/** Wrap deg into [0, 360) and round to 3 decimal places. */ +static double normalizeDeg3(double deg) { + deg = fmod(deg, 360.0); + if (deg < 0.0) + deg += 360.0; + return round(deg * 1000.0) / 1000.0; +} + +/** Snap filtered counts → integer counts → degrees (no offset). */ +static double rawDisplayDeg(double filteredCounts) { + return countsToDeg(snapToCounts(filteredCounts)); +} + +/** Return the active encoder pointer, or nullptr if that encoder is disabled. + */ +static AEAT8800 *activeEncoder() { + if (g_activeEnc == 1 && g_enc1Enabled) + return g_enc1; + if (g_activeEnc == 2 && g_enc2Enabled) + return g_enc2; + return nullptr; +} + +static const char *resolutionStr(uint8_t regVal) { + switch (regVal & 0x03) { + case 0x00: + return "10-bit (1024 cpr)"; + case 0x01: + return "12-bit (4096 cpr)"; + case 0x02: + return "14-bit (16384 cpr)"; + case 0x03: + return "16-bit (65536 cpr)"; + default: + return "unknown"; + } +} + +static void printSinglePosition(uint8_t id, double pos, bool ok) { + if (ok) { + uint16_t snapped = snapToCounts(pos); + double deg = displayDeg(id, pos); + Serial.printf("[ENC%u] Pos=%5u Angle=%8.3f deg\n", id, snapped, deg); + } else { + Serial.printf("[ENC%u] READ ERROR\n", id); + } +} + +// ─── Hybrid Circular Filtering Algorithms ─────────────────────────────────── + +static double getCircularRobustMean(uint16_t *samples, int size) { + if (size == 0) + return 0; + + double sum_x = 0; + double sum_y = 0; + for (int i = 0; i < size; i++) { + double theta = (samples[i] / ENCODER_CPR_D) * 2.0 * PI; + sum_x += cos(theta); + sum_y += sin(theta); + } + double initialMeanTheta = atan2(sum_y, sum_x); + if (initialMeanTheta < 0) + initialMeanTheta += 2.0 * PI; + uint16_t initialMeanCnt = + (uint16_t)((initialMeanTheta / (2.0 * PI)) * ENCODER_CPR_D); + + double robustSum_x = 0; + double robustSum_y = 0; + int count = 0; + for (int i = 0; i < size; i++) { + // Calculate circular distance + uint16_t diff = samples[i] - initialMeanCnt; + uint16_t dist = (diff > 32768) ? (ENCODER_CPR - diff) : diff; + + if (dist < OUTLIER_THRESHOLD) { + double theta = (samples[i] / ENCODER_CPR_D) * 2.0 * PI; + robustSum_x += cos(theta); + robustSum_y += sin(theta); + count++; + } + } + + // If everything was an outlier (e.g. extremely noisy), fallback to standard + // circular mean + if (count == 0) { + return (double)initialMeanCnt; + } + + double robustMeanTheta = atan2(robustSum_y, robustSum_x); + if (robustMeanTheta < 0) + robustMeanTheta += 2.0 * PI; + return (robustMeanTheta / (2.0 * PI)) * ENCODER_CPR_D; +} + +static double getCircularMedian(double *values, int size) { + if (size == 0) + return 0; + + // Unwrap all block means relative to the first block mean to handle + // wrap-around + double unwrapped[NUM_BLOCKS]; + unwrapped[0] = values[0]; + + for (int i = 1; i < size; i++) { + double diff = values[i] - values[0]; + if (diff > 32768.0) + diff -= ENCODER_CPR_D; + else if (diff < -32768.0) + diff += ENCODER_CPR_D; + unwrapped[i] = values[0] + diff; + } + + std::sort(unwrapped, unwrapped + size); + double med = unwrapped[size / 2]; + + // Wrap back to [0, CPR) + if (med < 0) + med += ENCODER_CPR_D; + else if (med >= ENCODER_CPR_D) + med -= ENCODER_CPR_D; + + return med; +} + +static bool readAngleFiltered(AEAT8800 *enc, double &finalPos) { + if (!enc) + return false; + + double blockMeans[NUM_BLOCKS]; + uint16_t blockSamples[SAMPLES_PER_BLOCK]; + + enc->select(); + int validBlocks = 0; + + for (int b = 0; b < NUM_BLOCKS; b++) { + int validSamples = 0; + for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { + uint16_t raw; + if (enc->readAngleRaw(raw, true)) { // silent = true + blockSamples[validSamples++] = raw; + } + } + if (validSamples > 0) { + blockMeans[validBlocks++] = + getCircularRobustMean(blockSamples, validSamples); + } + } + enc->deselect(); + + if (validBlocks == 0) + return false; + + finalPos = getCircularMedian(blockMeans, validBlocks); + return true; +} + +// ─── Read Both Enabled Encoders +// ─────────────────────────────────────────────── + +static void readBothEnabled(double &pos1, double &pos2, bool &ok1, bool &ok2) { + pos1 = 0; + pos2 = 0; + ok1 = false; + ok2 = false; + + if (g_enc1Enabled) { + ok1 = readAngleFiltered(g_enc1, pos1); + if (g_enc2Enabled) + delay(1); // brief gap before reading enc2 + } + if (g_enc2Enabled) { + ok2 = readAngleFiltered(g_enc2, pos2); + } +} + +/** Snap filtered counts, apply software offset, normalize to [0,360), 3 dp. */ +static double displayDeg(uint8_t encId, double filteredCounts) { + double raw = rawDisplayDeg(filteredCounts); + double off = (encId == 1) ? g_offsetDeg1 : g_offsetDeg2; + + + return normalizeDeg3(raw - off); +} + +// ─── Register Dump +// ──────────────────────────────────────────────────────────── + +static void printAllRegisters() { + AEAT8800 *enc = activeEncoder(); + if (!enc) { + Serial.printf("[ERR] ENC%u is not enabled.\n", g_activeEnc); + return; + } + + Serial.printf("\n╔══════════════════════════════════════════════╗\n"); + Serial.printf("║ AEAT-8800-Q24 — ENC%u SPI Register Dump ║\n", + g_activeEnc); + Serial.println(F("╠══════════════════════════════════════════════╣")); + + struct { + uint8_t addr; + const char *name; + } regs[] = { + {REG_CUST_RESERVE_0, "CustReserve0 (0x00)"}, + {REG_CUST_RESERVE_1, "CustReserve1 (0x01)"}, + {REG_ZERO_POS_L, "ZeroPos_L (0x02)"}, + {REG_ZERO_POS_H, "ZeroPos_H (0x03)"}, + {REG_CUST_CONFIG_0, "CustConfig0 (0x04)"}, + {REG_CPR_SET1, "CPR_Set1 (0x05)"}, + {REG_CPR_SET2, "CPR_Set2 (0x06)"}, + {REG_RESOLUTION, "Resolution (0x07)"}, + {REG_LOCK, "Lock (0x10)"}, + }; + + for (auto &r : regs) { + uint8_t val = enc->spiRead(r.addr); + Serial.printf("║ %-22s 0x%02X (%3u) ║\n", r.name, val, val); + } + + uint8_t lo = enc->spiRead(REG_ZERO_POS_L); + uint8_t hi = enc->spiRead(REG_ZERO_POS_H); + uint16_t zp = ((uint16_t)hi << 8) | lo; + uint8_t resReg = enc->spiRead(REG_RESOLUTION); + + Serial.println(F("╠══════════════════════════════════════════════╣")); + Serial.printf("║ HW Zero Pos : %5u (%.3f deg) ║\n", zp, + countsToDeg(zp)); + Serial.printf("║ Resolution : %-30s ║\n", resolutionStr(resReg)); + Serial.println(F("╚══════════════════════════════════════════════╝\n")); +} + +// ─── Help +// ───────────────────────────────────────────────────────────────────── + +static void printHelp() { + Serial.println( + F("\n╔══════════════════════════════════════════════════════╗")); + Serial.println(F("║ AEAT-8800-Q24 Independent Dual — Commands ║")); + Serial.println(F("╠══════════════════════════════════════════════════════╣")); + Serial.println(F("║ p — Read enabled encoders (SSI, one-shot) ║")); + Serial.println(F("║ c — Toggle continuous stream (100 ms) ║")); + Serial.println(F("║ o — Offset: current pos reports as ║")); + Serial.println(F("║ o clear — Remove software offset (back to raw) ║")); + Serial.println(F("║ 1 / 2 — Select active encoder for SPI cmds ║")); + Serial.println(F("║ s — Dump SPI registers (active encoder) ║")); + Serial.println(F("║ r — Read register e.g. 'r 07' ║")); + Serial.println(F("║ w — Write register e.g. 'w 10 AB' ║")); + Serial.println(F("║ u — Unlock config registers ║")); + Serial.println(F("║ z — HW zero position = 0x0000 (SPI) ║")); + Serial.println(F("║ burn — Burn OTP ⚠ IRREVERSIBLE ║")); + Serial.println( + F("║ h — Show this help ║")); + Serial.println( + F("╚══════════════════════════════════════════════════════╝\n")); +} + +// ─── Serial Command Parser +// ──────────────────────────────────────────────────── + +static void handleSerial() { + if (!Serial.available()) + return; + + String line = Serial.readStringUntil('\n'); + line.trim(); + if (line.length() == 0) + return; + + // ── Software angle offset "o [deg]" or "o clear" ───────────────────── + if (line.length() >= 1 && + tolower((unsigned char)line[0]) == 'o' && + (line.length() == 1 || line[1] == ' ')) { + String arg = (line.length() > 2) ? line.substring(2) : String(""); + arg.trim(); + arg.toLowerCase(); + + if (arg == "clear") { + g_offsetDeg1 = 0.0; + g_offsetDeg2 = 0.0; + Serial.println(F("[OFFSET] Cleared — both encoders report raw hardware angles")); + } else { + double targetDeg = (arg.length() > 0) ? arg.toDouble() : 0.0; + double pos1 = 0, pos2 = 0; + bool ok1 = false, ok2 = false; + readBothEnabled(pos1, pos2, ok1, ok2); + + if (g_enc1Enabled) { + if (ok1) { + double raw1 = rawDisplayDeg(pos1); + g_offsetDeg1 = raw1 - targetDeg; + Serial.printf("[OFFSET] ENC1 raw=%.3f → reports as %.3f (offset=%.3f)\n", + raw1, targetDeg, g_offsetDeg1); + } else { + Serial.println(F("[OFFSET] ENC1 read failed — offset unchanged")); + } + } + if (g_enc2Enabled) { + if (ok2) { + double raw2 = rawDisplayDeg(pos2); + g_offsetDeg2 = raw2 - targetDeg; + Serial.printf("[OFFSET] ENC2 raw=%.3f → reports as %.3f (offset=%.3f)\n", + raw2, targetDeg, g_offsetDeg2); + } else { + Serial.println(F("[OFFSET] ENC2 read failed — offset unchanged")); + } + } + } + return; + } + + // OTP burn — requires the exact string "burn" to prevent accidents + if (line.equalsIgnoreCase("burn")) { + AEAT8800 *enc = activeEncoder(); + if (!enc) { + Serial.printf("[ERR] ENC%u is not enabled.\n", g_activeEnc); + return; + } + Serial.printf("[OTP] Unlocking ENC%u and burning shadow regs to OTP...\n", + g_activeEnc); + enc->spiWrite(REG_LOCK, UNLOCK_KEY); + delayMicroseconds(10); + enc->spiWrite(REG_PROG_CUST, PROG_KEY); + Serial.println(F("[OTP] Done. Power-cycle the encoder to verify.")); + return; + } + + char cmd = (char)tolower((unsigned char)line[0]); + + switch (cmd) { + + // ── Read both enabled encoders (SSI) ───────────────────────────────────── + case 'p': { + double pos1 = 0, pos2 = 0; + bool ok1 = false, ok2 = false; + readBothEnabled(pos1, pos2, ok1, ok2); + if (g_enc1Enabled) + printSinglePosition(1, pos1, ok1); + if (g_enc2Enabled) + printSinglePosition(2, pos2, ok2); + break; + } + + // ── Continuous stream toggle ────────────────────────────────────────────── + case 'c': + g_streaming = !g_streaming; + Serial.printf("[INFO] Continuous stream %s\n", g_streaming ? "ON" : "OFF"); + break; + + // ── Select active encoder for SPI commands ──────────────────────────────── + case '1': + if (!g_enc1Enabled) { + Serial.println(F("[ERR] ENC1 is not enabled.")); + break; + } + g_activeEnc = 1; + Serial.println(F("[INFO] Active encoder: ENC1")); + break; + case '2': + if (!g_enc2Enabled) { + Serial.println(F("[ERR] ENC2 is not enabled.")); + break; + } + g_activeEnc = 2; + Serial.println(F("[INFO] Active encoder: ENC2")); + break; + + // ── Register dump ───────────────────────────────────────────────────────── + case 's': + printAllRegisters(); + break; + + // ── Read single register "r " ───────────────────────────────── + case 'r': { + if (line.length() < 3) { + Serial.println(F("[ERR] Usage: r e.g. 'r 07'")); + break; + } + AEAT8800 *enc = activeEncoder(); + if (!enc) { + Serial.printf("[ERR] ENC%u is not enabled.\n", g_activeEnc); + break; + } + uint8_t addr = (uint8_t)strtoul(line.c_str() + 2, nullptr, 16); + uint8_t val = enc->spiRead(addr); + Serial.printf("[ENC%u SPI-RD] Reg 0x%02X = 0x%02X (%u)\n", g_activeEnc, + addr, val, val); + break; + } + + // ── Write single register "w " ────────────────────── + case 'w': { + if (line.length() < 5) { + Serial.println(F("[ERR] Usage: w e.g. 'w 10 AB'")); + break; + } + AEAT8800 *enc = activeEncoder(); + if (!enc) { + Serial.printf("[ERR] ENC%u is not enabled.\n", g_activeEnc); + break; + } + char *ptr = nullptr; + uint8_t addr = (uint8_t)strtoul(line.c_str() + 2, &ptr, 16); + uint8_t val = (uint8_t)strtoul(ptr, nullptr, 16); + enc->spiWrite(addr, val); + uint8_t rb = enc->spiRead(addr); + Serial.printf( + "[ENC%u SPI-WR] Reg 0x%02X <- 0x%02X | Readback: 0x%02X %s\n", + g_activeEnc, addr, val, rb, (rb == val) ? "OK" : "MISMATCH!"); + break; + } + + // ── Unlock registers ────────────────────────────────────────────────────── + case 'u': { + AEAT8800 *enc = activeEncoder(); + if (!enc) { + Serial.printf("[ERR] ENC%u is not enabled.\n", g_activeEnc); + break; + } + enc->spiWrite(REG_LOCK, UNLOCK_KEY); + Serial.printf("[ENC%u SPI] Registers unlocked (0xAB → Lock reg)\n", + g_activeEnc); + break; + } + + // ── Set zero position ───────────────────────────────────────────────────── + case 'z': { + AEAT8800 *enc = activeEncoder(); + if (!enc) { + Serial.printf("[ERR] ENC%u is not enabled.\n", g_activeEnc); + break; + } + Serial.printf("[ENC%u ZERO] Writing 0x0000 to ZeroPos shadow regs...\n", + g_activeEnc); + enc->spiWrite(REG_LOCK, UNLOCK_KEY); + delayMicroseconds(10); + enc->spiWrite(REG_ZERO_POS_L, 0x00); + enc->spiWrite(REG_ZERO_POS_H, 0x00); + uint8_t lo = enc->spiRead(REG_ZERO_POS_L); + uint8_t hi = enc->spiRead(REG_ZERO_POS_H); + uint16_t zp = ((uint16_t)hi << 8) | lo; + Serial.printf("[ENC%u ZERO] Readback: 0x%04X — %s\n", g_activeEnc, zp, + zp == 0x0000 ? "OK" : "MISMATCH!"); + break; + } + + // ── Burn guard (must type "burn" exactly) ───────────────────────────────── + case 'b': + Serial.println(F("[WARN] OTP BURN is IRREVERSIBLE!")); + Serial.println(F("[WARN] Type exactly 'burn' and press Enter to confirm.")); + break; + + // ── Help ────────────────────────────────────────────────────────────────── + case 'h': + default: + printHelp(); + break; + } +} + +// ─── Setup +// ──────────────────────────────────────────────────────────────────── + +void setup() { + Serial.begin(115200); + delay(300); + + // ── External antenna + // ───────────────────────────────────────────────────────── GPIO3 LOW + // activates the RF switch; GPIO14 HIGH selects the external antenna. + pinMode(WIFI_ENABLE, OUTPUT); + digitalWrite(WIFI_ENABLE, LOW); + delay(100); + pinMode(WIFI_ANT_CONFIG, OUTPUT); + digitalWrite(WIFI_ANT_CONFIG, HIGH); + + // ── Encoder configuration (wizard or defaults) + // ───────────────────────────────────────────── + EncoderConfig cfg1, cfg2; + if (RUN_SETUP_WIZARD) { + runSetupWizard(cfg1, cfg2); + } else { + applyDefaultDualConfig(cfg1, cfg2); + Serial.println(F( + "[INFO] Setup wizard skipped — ENC1+ENC2 on default pins, stream ON")); + } + + // ── GPIO init for enabled encoders + // ─────────────────────────────────────────── + if (g_enc1Enabled) + initEncoderPins(cfg1); + if (g_enc2Enabled) + initEncoderPins(cfg2); + + // tPwrUp ~4 ms (datasheet p.6) + NSL must be HIGH >= 3 ms before first SSI + // read + delay(5); + + // ── Instantiate encoder objects + // ─────────────────────────────────────────────── + if (g_enc1Enabled) + g_enc1 = new AEAT8800(cfg1.pinCLK, cfg1.pinDIN, cfg1.pinDOUT, cfg1.pinSEL); + if (g_enc2Enabled) + g_enc2 = new AEAT8800(cfg2.pinCLK, cfg2.pinDIN, cfg2.pinDOUT, cfg2.pinSEL); + + // ── SPI configuration + // ──────────────────────────────────────────────────────── + Serial.println( + F("╔══════════════════════════════════════════════════════════╗")); + Serial.println( + F("║ AEAT-8800-Q24 Independent | XIAO ESP32-C6 ║")); + Serial.println( + F("╠══════════════════════════════════════════════════════════╣")); + + if (g_enc1Enabled) { + Serial.print(F("║ Configuring ENC1 ... ")); + bool ok = g_enc1->configure(); + Serial.println(ok ? F("OK ║") + : F("FAILED — SSI reads will still be tried ║")); + } + if (g_enc2Enabled) { + Serial.print(F("║ Configuring ENC2 ... ")); + bool ok = g_enc2->configure(); + Serial.println(ok ? F("OK ║") + : F("FAILED — SSI reads will still be tried ║")); + } + + Serial.println( + F("╠══════════════════════════════════════════════════════════╣")); + + // ── Initial SSI position read + // ───────────────────────────────────────────────── + double filt1 = 0, filt2 = 0; + bool r1 = false, r2 = false; + readBothEnabled(filt1, filt2, r1, r2); + + if (g_enc1Enabled) { + if (r1) + Serial.printf("║ ENC1 Position : %5u counts (%.3f deg) ║\n", + (unsigned)(filt1 + 0.5), countsToDeg((uint16_t)(filt1 + 0.5))); + else + Serial.println( + F("║ ENC1 Position : READ ERROR ║")); + } + if (g_enc2Enabled) { + if (r2) + Serial.printf("║ ENC2 Position : %5u counts (%.3f deg) ║\n", + (unsigned)(filt2 + 0.5), countsToDeg((uint16_t)(filt2 + 0.5))); + else + Serial.println( + F("║ ENC2 Position : READ ERROR ║")); + } + + Serial.println( + F("╠══════════════════════════════════════════════════════════╣")); + if (!RUN_SETUP_WIZARD) { + Serial.println( + F("║ Continuous stream ON (send 'c' to toggle) ║")); + } + Serial.println( + F("║ Type 'h' for command help ║")); + Serial.println( + F("╚══════════════════════════════════════════════════════════╝\n")); +} + +// ─── Main Loop +// ──────────────────────────────────────────────────────────────── + +void loop() { + handleSerial(); + + if (g_streaming && (millis() - g_lastStream >= 33)) { + g_lastStream = millis(); + + double pos1 = 0, pos2 = 0; + bool ok1 = false, ok2 = false; + readBothEnabled(pos1, pos2, ok1, ok2); + + if (g_enc1Enabled && g_enc2Enabled) { + float d1 = ok1 ? (float)displayDeg(1, pos1) : -1.0f; + float d2 = ok2 ? (float)displayDeg(2, pos2) : -1.0f; + if (ok1 && ok2) + Serial.printf("ENC1: %.3f deg | ENC2: %.3f deg\n", d1, d2); + else if (!ok1 && !ok2) + Serial.println(F("ENC1: ERR | ENC2: ERR")); + else if (!ok1) + Serial.printf("ENC1: ERR | ENC2: %.3f deg\n", d2); + else + Serial.printf("ENC1: %.3f deg | ENC2: ERR\n", d1); + } else if (g_enc1Enabled) { + if (ok1) + Serial.printf("ENC1: %.3f deg\n", displayDeg(1, pos1)); + else + Serial.println(F("ENC1: ERR")); + } else if (g_enc2Enabled) { + if (ok2) + Serial.printf("ENC2: %.3f deg\n", displayDeg(2, pos2)); + else + Serial.println(F("ENC2: ERR")); + } + } +} diff --git a/firmware/dual_aeat8800_star/dual_aeat8800_star.ino b/firmware/dual_aeat8800_star/dual_aeat8800_star.ino new file mode 100644 index 0000000..5f556e2 --- /dev/null +++ b/firmware/dual_aeat8800_star/dual_aeat8800_star.ino @@ -0,0 +1,694 @@ +/** + * ============================================================ + * AEAT-8800-Q24 — Dual 16-bit Absolute Magnetic Encoder + * Star-Topology Clock Wiring (each encoder has dedicated CLK) + * Board : Seeed Studio XIAO ESP32-C6 + * + * ── WHY STAR TOPOLOGY ──────────────────────────────────────── + * SSI protocol does not support multiple slaves on one shared + * clock line. An unclocked slave holds DOUT in high-impedance + * only when its own CLK is idle. Giving each encoder a dedicated + * CLK line means the inactive slave is never clocked, so its + * DOUT stays tri-state and bus contention is eliminated. + * + * ── PIN MAPPING ────────────────────────────────────────────── + * Signal │ Arduino │ GPIO │ Notes + * ────────────────┼─────────┼───────┼──────────────────────── + * CLK1 │ D8 │ 19 │ Encoder 1 dedicated clock + * CLK2 │ D5 │ 23 │ Encoder 2 dedicated clock + * DIN / NSL │ D10 │ 18 │ Shared: SSI enable / SPI MOSI + * DOUT │ D9 │ 20 │ Shared: data from encoders + * SEL1 │ D3 │ 21 │ Encoder 1 SSI_SPI_SEL + * SEL2 │ D4 │ 22 │ Encoder 2 SSI_SPI_SEL + * WIFI_ENABLE │ — │ 3 │ RF switch control (active LOW) + * WIFI_ANT_CONFIG │ — │ 14 │ Antenna select (HIGH = external) + * + * ── DUAL-ENCODER BUS ARBITRATION ───────────────────────────── + * SSI read ENC1 : SEL1=HIGH, SEL2=LOW, clock CLK1 × 20 bits. + * → ENC2 unclocked + SPI/idle → DOUT high-impedance. + * SSI read ENC2 : SEL2=HIGH, SEL1=LOW, clock CLK2 × 20 bits. + * → ENC1 unclocked + SPI/idle → DOUT high-impedance. + * SPI access ENC1: SEL1=LOW (SPI mode), SEL2=HIGH (SSI idle), + * bit-bang on CLK1. CLK2 stays idle — ENC2 DOUT stays quiet. + * SPI access ENC2: SEL2=LOW (SPI mode), SEL1=HIGH (SSI idle), + * bit-bang on CLK2. CLK1 stays idle — ENC1 DOUT stays quiet. + * + * ── SSI FRAME (20 bits, 16-bit resolution) ─────────────────── + * Bits [19:4] = 16-bit absolute position, MSB first + * Bit [3] = Ready (must be 1; retry if 0) + * Bit [2] = MHi (magnet too strong / too close) + * Bit [1] = MLo (magnet too weak / too far) + * Bit [0] = Even parity over all 20 bits + * Data sampled on the FALLING edge of CLK (datasheet p.15). + * + * ── SPI PROTOCOL ───────────────────────────────────────────── + * Write : 0b01_aaaaaa_dddddddd (16 bits, MSB first) + * Read : 0b10_aaaaaa (8-bit cmd) then 8-bit reply + * CLK idles HIGH (CPOL=1); encoder captures DIN on rising CLK (CPHA=1). + * + * ── KEY TIMING (datasheet Fig. 8 & timing table) ──────────────── + * Symbol │ Min │ Unit │ Datasheet description + * ──────────┼──────┼──────┼────────────────────────────────────────────── + * tsw(SEL) │ 1 │ µs │ SSI_SPI_SEL switch time + * tREQ │ 300 │ ns │ SCL high time between NSL falling edge and + * │ │ │ first SCL falling edge + * tREQ2 │ 200 │ ns │ NSL low time after rising edge of last clock + * │ │ │ period for an SSI read + * tNSLH │ 200 │ ns │ NSL high time between 2 successive SSI reads + * ──────────┼──────┼──────┼────────────────────────────────────────────── + * Notes (datasheet p.15): + * • CLK = 1 when inactive; DIN = 1 when inactive. + * • CLK must be HIGH when switching between SSI and SPI modes. + * • NSL must be held HIGH for at least 3 ms after power-up before + * the first SSI read. + * • The user is advised to read from the SSI falling edge. + * • SSI data length depends on resolution setting: + * 16-bit → 20 total bits (16 pos + Ready + MHi + MLo + Parity) + * All timing margins use SSI_HALF_US = 5 µs (>> all minimum requirements). + * + * ── SERIAL COMMANDS ────────────────────────────────────────── + * p — Read position from both encoders (SSI, one-shot) + * c — Toggle continuous stream (both encoders, 100 ms) + * 1 / 2 — Select active encoder for SPI commands + * s — Dump all SPI config registers (active encoder) + * r — Read register at hex address e.g. "r 07" + * w — Write register e.g. "w 10 AB" + * u — Unlock config registers (0xAB → Lock reg) + * z — Set zero position = 0x0000 (shadow only) + * burn — Burn shadow regs to OTP ⚠ IRREVERSIBLE + * h — Show this help + * + * Author : Swaraj Dangare + * ============================================================ + */ + +// ─── Pin Definitions ────────────────────────────────────────────────────────── +#define PIN_CLK1 D8 // GPIO19 — Encoder 1 dedicated clock +#define PIN_CLK2 D8 // GPIO23 — Encoder 2 dedicated clock +#define PIN_DIN D10 // GPIO18 — Shared DIN / NSL (SSI enable / SPI MOSI) +#define PIN_DOUT D9 // GPIO20 — Shared DOUT (data output from encoders) +#define PIN_SEL1 D3 // GPIO21 — Encoder 1 SSI_SPI_SEL +#define PIN_SEL2 D4 // GPIO22 — Encoder 2 SSI_SPI_SEL +#define WIFI_ENABLE 3 // GPIO3 — RF switch control (LOW = active) +#define WIFI_ANT_CONFIG 14 // GPIO14 — Antenna select (HIGH = external) + +// ─── Register Addresses ─────────────────────────────────────────────────────── +#define REG_CUST_RESERVE_0 0x00 +#define REG_CUST_RESERVE_1 0x01 +#define REG_ZERO_POS_L 0x02 +#define REG_ZERO_POS_H 0x03 +#define REG_CUST_CONFIG_0 0x04 +#define REG_CPR_SET1 0x05 +#define REG_CPR_SET2 0x06 +#define REG_RESOLUTION 0x07 +#define REG_VCC 0x0A +#define REG_LOCK 0x10 +#define REG_PROG_CUST 0x11 + +// ─── Register Values ────────────────────────────────────────────────────────── +#define UNLOCK_KEY 0xAB +#define PROG_KEY 0xA1 +#define VAL_VCC 0x00 // 0x0A [1]=0 → 3.3 V +#define VAL_CFG0 0x00 // 0x04 PWM mode, 1 pole-pair +#define VAL_CPR1 0x40 // 0x05 CPR1=0b0100, no hysteresis +#define VAL_CPR2 0x04 // 0x06 16-bit abs, CW, zero-latency OFF + +// ─── SSI Timing ─────────────────────────────────────────────────────────────── +// Timing constants derived from AEAT-8800-Q24 datasheet timing table (Fig. 8): +// tREQ >= 300 ns SCL high time between NSL falling edge and first SCL falling edge +// tREQ2 >= 200 ns NSL low time after rising edge of last clock period for an SSI read +// tNSLH >= 200 ns NSL high time between 2 successive SSI reads +// tsw(SEL) >= 1 µs SSI_SPI_SEL switch time +// SSI_HALF_US = 5 µs is used for every CLK half-period and for all +// tREQ / tREQ2 / tNSLH margins, giving 7–25× margin over the minimums +// and matching the clock rate of the proven single-encoder firmware. +// SSI_MONOFLOP_US: encoder internal load-cycle recovery after NSL goes HIGH. +#define SSI_TOTAL_BITS 20 // 16-bit position + Ready + MHi + MLo + Parity +#define SSI_HALF_US 5 // 5 µs half-period → ~100 kHz SSI clock +#define SSI_MONOFLOP_US 20 // 20 µs monoflop recovery after NSL HIGH + +// ─── AEAT8800 Class ─────────────────────────────────────────────────────────── +// Each instance owns its CLK pin so SSI and SPI operations on enc1 and enc2 +// use completely independent clock lines — no shared-bus arbitration needed. + +class AEAT8800 { +public: + AEAT8800(uint8_t clk, uint8_t din, uint8_t dout, uint8_t sel) + : _clk(clk), _din(din), _dout(dout), _sel(sel) {} + + /** Assert SEL HIGH → SSI mode. Call only after peer is deselected. */ + void select(); + + /** Assert SEL LOW → SPI/idle mode. Safe to call at any time. */ + void deselect(); + + /** Bit-bang one SPI register read using this encoder's CLK. */ + uint8_t spiRead(uint8_t addr); + + /** Bit-bang one SPI register write using this encoder's CLK. */ + void spiWrite(uint8_t addr, uint8_t data); + + /** Unlock and write all required OTP shadow registers with readback verify. + * Precondition: peer encoder's SEL must be HIGH (SSI idle) before calling + * so its DOUT does not contend the shared bus during SPI readback phases. + * Returns true on success, false if any register readback mismatches. */ + bool configure(); + + /** Perform one 20-bit SSI read. Stores position in `pos` (0–65535). + * Precondition: select() must already have been called. + * Returns false and prints the reason if Ready=0 or parity fails. */ + bool readAngleRaw(uint16_t &pos); + + /** Read angle in degrees [0.0, 360.0). Returns -1.0f on read error. */ + float readAngleDegrees(); + +private: + uint8_t _clk, _din, _dout, _sel; +}; + +// ─── select / deselect ──────────────────────────────────────────────────────── + +void AEAT8800::select() { + // CLK must be HIGH before the SEL transition (datasheet p.15 and p.21: + // "Make sure CLK is high when switching between SSI and SPI modes.") + digitalWrite(_clk, HIGH); + digitalWrite(_sel, HIGH); + delayMicroseconds(1); // tsw(SEL) >= 1 µs +} + +void AEAT8800::deselect() { + digitalWrite(_sel, LOW); + delayMicroseconds(1); // brief settle before next bus activity +} + +// ─── SPI Write ──────────────────────────────────────────────────────────────── + +void AEAT8800::spiWrite(uint8_t addr, uint8_t data) { + int prevSel = digitalRead(_sel); + + // CLK HIGH before SEL edge; drive SEL LOW for SPI mode + digitalWrite(_clk, HIGH); + digitalWrite(_sel, LOW); + delayMicroseconds(1); // tsw(SEL) >= 1 µs + + // 16-bit frame: opcode 0b01 (2b) + addr (6b) + data (8b), MSB first + uint16_t word = (0b01u << 14) | ((addr & 0x3F) << 8) | (data & 0xFF); + + for (int i = 15; i >= 0; i--) { + digitalWrite(_din, (word >> i) & 1u); // DIN valid before CLK falling edge + digitalWrite(_clk, LOW); // falling edge + delayMicroseconds(SSI_HALF_US); + digitalWrite(_clk, HIGH); // rising edge — encoder captures DIN + delayMicroseconds(SSI_HALF_US); // last iteration: thi(CLK) >= 300 ns + } + + digitalWrite(_din, HIGH); // release DIN to idle HIGH + digitalWrite(_clk, HIGH); // ensure CLK HIGH before SEL change + digitalWrite(_sel, prevSel); // restore caller's SEL state +} + +// ─── SPI Read ───────────────────────────────────────────────────────────────── + +uint8_t AEAT8800::spiRead(uint8_t addr) { + int prevSel = digitalRead(_sel); + + digitalWrite(_clk, HIGH); + digitalWrite(_sel, LOW); + delayMicroseconds(1); // tsw(SEL) >= 1 µs + + // 8-bit command: opcode 0b10 (2b) + addr (6b) + uint8_t cmd = (0b10u << 6) | (addr & 0x3F); + + for (int i = 7; i >= 0; i--) { + digitalWrite(_din, (cmd >> i) & 1u); + digitalWrite(_clk, LOW); + delayMicroseconds(SSI_HALF_US); + digitalWrite(_clk, HIGH); // rising edge — encoder captures DIN + delayMicroseconds(SSI_HALF_US); + } + + digitalWrite(_din, HIGH); // release DIN; encoder now drives DOUT for reply + + uint8_t result = 0; + for (int i = 0; i < 8; i++) { + digitalWrite(_clk, LOW); // falling edge — encoder shifts DOUT + delayMicroseconds(SSI_HALF_US); // DOUT valid <= 200 ns after CLK fall + digitalWrite(_clk, HIGH); // rising edge — master captures DOUT + result = (result << 1) | (uint8_t)digitalRead(_dout); // sample on rising edge (datasheet p.20) + delayMicroseconds(SSI_HALF_US); // last iteration: thi(CLK) >= 300 ns + } + + digitalWrite(_clk, HIGH); + digitalWrite(_din, HIGH); + digitalWrite(_sel, prevSel); + return result; +} + +// ─── Configure ──────────────────────────────────────────────────────────────── + +bool AEAT8800::configure() { + // Unlock must be the very first SPI write (datasheet p.10 note 3) + spiWrite(REG_LOCK, UNLOCK_KEY); + delayMicroseconds(10); // allow lock state to propagate internally + + struct RegTarget { uint8_t addr, val; const char* name; }; + static const RegTarget targets[] = { + { REG_VCC, VAL_VCC, "0x0A (VCC)" }, + { REG_CUST_CONFIG_0, VAL_CFG0, "0x04 (CFG0)" }, + { REG_CPR_SET1, VAL_CPR1, "0x05 (CPR1)" }, + { REG_CPR_SET2, VAL_CPR2, "0x06 (CPR2)" }, + }; + + for (const auto& r : targets) { + spiWrite(r.addr, r.val); + delayMicroseconds(5); // propagation margin before readback + uint8_t rb = spiRead(r.addr); + if (rb != r.val) { + Serial.printf("[ERR] Reg %s verify: wrote 0x%02X readback 0x%02X\n", + r.name, r.val, rb); + return false; + } + } + + deselect(); // explicit deselect on exit + return true; +} + +// ─── SSI Read ───────────────────────────────────────────────────────────────── + +bool AEAT8800::readAngleRaw(uint16_t &pos) { + // Precondition: select() already called — SEL is HIGH, encoder in SSI mode + + // CLK = 1 when inactive; DIN = 1 when inactive (datasheet note) + digitalWrite(_clk, HIGH); + digitalWrite(_din, HIGH); + delayMicroseconds(SSI_HALF_US); // tNSLH >= 200 ns: NSL high time between 2 successive SSI reads + + // NSL LOW → shift mode: encoder freezes position into shift register (Fig. 8) + digitalWrite(_din, LOW); + delayMicroseconds(SSI_HALF_US); // tREQ >= 300 ns: SCL high time between NSL falling edge and first SCL falling edge + + uint32_t raw = 0; + for (int i = 0; i < SSI_TOTAL_BITS; i++) { + digitalWrite(_clk, LOW); // SCL falling edge — encoder shifts next bit onto DO + delayMicroseconds(SSI_HALF_US); // read from SSI falling edge per datasheet (data stable after CLK fall) + raw = (raw << 1) | (uint32_t)digitalRead(_dout); // sample DO after falling edge (datasheet Fig. 8, p.15) + digitalWrite(_clk, HIGH); // SCL rising edge + delayMicroseconds(SSI_HALF_US); // CLK high hold (CLK = 1 when inactive) + } + + delayMicroseconds(SSI_HALF_US); // tREQ2 >= 200 ns: NSL low time after rising edge of last clock period for an SSI read + + // End of frame: NSL HIGH → load mode (encoder resumes tracking position) + digitalWrite(_din, HIGH); + delayMicroseconds(SSI_MONOFLOP_US); // monoflop recovery: encoder completes internal load cycle + + // Unpack 20-bit frame (datasheet Fig. 8–9) + uint8_t parity = (raw >> 0) & 0x1u; + uint8_t mlo = (raw >> 1) & 0x1u; + uint8_t mhi = (raw >> 2) & 0x1u; + uint8_t ready = (raw >> 3) & 0x1u; + uint16_t position = (uint16_t)((raw >> 4) & 0xFFFFu); + + (void)parity; // value is implicitly checked via __builtin_popcount below + + if (!ready) { + Serial.printf("[ERR] Ready=0 (raw=0x%05X) — data not valid\n", (unsigned)raw); + return false; + } + + if (mhi) Serial.println(F("[WARN] MHi=1 — magnet too strong / too close")); + if (mlo) Serial.println(F("[WARN] MLo=1 — magnet too weak / too far")); + + // Even parity: total 1-bit count across all 20 raw bits must be even + if (__builtin_popcount((unsigned)raw) % 2 != 0) { + Serial.printf("[ERR] Parity error (raw=0x%05X)\n", (unsigned)raw); + return false; + } + + pos = position; + return true; +} + +float AEAT8800::readAngleDegrees() { + uint16_t raw = 0; + if (!readAngleRaw(raw)) return -1.0f; + return (raw / 65536.0f) * 360.0f; +} + +// ─── Global Encoder Instances ───────────────────────────────────────────────── +AEAT8800 enc1(PIN_CLK1, PIN_DIN, PIN_DOUT, PIN_SEL1); +AEAT8800 enc2(PIN_CLK2, PIN_DIN, PIN_DOUT, PIN_SEL2); + +// ─── Global State ───────────────────────────────────────────────────────────── +static bool g_streaming = false; +static uint32_t g_lastStream = 0; +static uint8_t g_activeEnc = 1; // 1 or 2 — target for SPI serial commands + +// ─── Helpers ───────────────────────────────────────────────────────────────── + +/** Return a pointer to the currently selected encoder. */ +static inline AEAT8800* activeEncoder() { + return (g_activeEnc == 1) ? &enc1 : &enc2; +} + +/** Raise the peer's SEL to SSI/idle, isolating its DOUT from the bus. */ +static inline void isolatePeer() { + if (g_activeEnc == 1) { enc2.select(); } else { enc1.select(); } +} + +/** Lower the peer's SEL back to SPI/idle after the transaction. */ +static inline void releasePeer() { + if (g_activeEnc == 1) { enc2.deselect(); } else { enc1.deselect(); } +} + +const char* resolutionStr(uint8_t regVal) { + switch (regVal & 0x03) { + case 0x00: return "10-bit (1024 cpr)"; + case 0x01: return "12-bit (4096 cpr)"; + case 0x02: return "14-bit (16384 cpr)"; + case 0x03: return "16-bit (65536 cpr)"; + default: return "unknown"; + } +} + +// ─── Serial Output Helpers ──────────────────────────────────────────────────── + +void printSinglePosition(uint8_t id, uint16_t pos, bool ok) { + if (ok) { + double deg = (pos / 65536.0) * 360.0; + Serial.printf("[ENC%u] Pos=%5u Angle=%8.4f deg\n", id, pos, deg); + } else { + Serial.printf("[ENC%u] READ ERROR\n", id); + } +} + +void printAllRegisters() { + AEAT8800* enc = activeEncoder(); + + Serial.printf("\n╔══════════════════════════════════════════════╗\n"); + Serial.printf( "║ AEAT-8800-Q24 — ENC%u SPI Register Dump ║\n", g_activeEnc); + Serial.println(F("╠══════════════════════════════════════════════╣")); + + struct { uint8_t addr; const char* name; } regs[] = { + { REG_CUST_RESERVE_0, "CustReserve0 (0x00)" }, + { REG_CUST_RESERVE_1, "CustReserve1 (0x01)" }, + { REG_ZERO_POS_L, "ZeroPos_L (0x02)" }, + { REG_ZERO_POS_H, "ZeroPos_H (0x03)" }, + { REG_CUST_CONFIG_0, "CustConfig0 (0x04)" }, + { REG_CPR_SET1, "CPR_Set1 (0x05)" }, + { REG_CPR_SET2, "CPR_Set2 (0x06)" }, + { REG_RESOLUTION, "Resolution (0x07)" }, + { REG_LOCK, "Lock (0x10)" }, + }; + + isolatePeer(); // peer SEL HIGH → its DOUT tri-state during SPI reads + + for (auto& r : regs) { + uint8_t val = enc->spiRead(r.addr); + Serial.printf("║ %-22s 0x%02X (%3u) ║\n", r.name, val, val); + } + + uint8_t lo = enc->spiRead(REG_ZERO_POS_L); + uint8_t hi = enc->spiRead(REG_ZERO_POS_H); + uint16_t zp = ((uint16_t)hi << 8) | lo; + uint8_t resReg = enc->spiRead(REG_RESOLUTION); + + releasePeer(); + + Serial.println(F("╠══════════════════════════════════════════════╣")); + Serial.printf( "║ HW Zero Pos : %5u (%.4f deg) ║\n", + zp, (zp / 65536.0) * 360.0); + Serial.printf( "║ Resolution : %-30s ║\n", resolutionStr(resReg)); + Serial.println(F("╚══════════════════════════════════════════════╝\n")); +} + +void printHelp() { + Serial.println(F("\n╔══════════════════════════════════════════════════════╗")); + Serial.println(F("║ AEAT-8800-Q24 DUAL Star-Topology Command Menu ║")); + Serial.println(F("╠══════════════════════════════════════════════════════╣")); + Serial.println(F("║ p — Read both encoders (SSI, one-shot) ║")); + Serial.println(F("║ c — Toggle continuous stream (100 ms) ║")); + Serial.println(F("║ 1 / 2 — Select active encoder for SPI cmds ║")); + Serial.println(F("║ s — Dump SPI registers (active encoder) ║")); + Serial.println(F("║ r — Read register e.g. 'r 07' ║")); + Serial.println(F("║ w — Write register e.g. 'w 10 AB' ║")); + Serial.println(F("║ u — Unlock config registers ║")); + Serial.println(F("║ z — Set zero position = 0x0000 ║")); + Serial.println(F("║ burn — Burn OTP ⚠ IRREVERSIBLE ║")); + Serial.println(F("║ h — Show this help ║")); + Serial.println(F("╚══════════════════════════════════════════════════════╝\n")); +} + +// ─── Dual SSI Read (used by 'p' command and continuous stream) ──────────────── + +static void readBothEncoders(uint16_t &pos1, uint16_t &pos2, + bool &ok1, bool &ok2) { + // Read ENC1: SEL2 LOW (already), raise SEL1, clock CLK1 + enc2.deselect(); + enc1.select(); + ok1 = enc1.readAngleRaw(pos1); + enc1.deselect(); + + delay(1); // inter-encoder gap; NSL HIGH > tNSLH before next read + + // Read ENC2: SEL1 LOW (already), raise SEL2, clock CLK2 + enc1.deselect(); + enc2.select(); + ok2 = enc2.readAngleRaw(pos2); + enc2.deselect(); +} + +// ─── Serial Command Parser ──────────────────────────────────────────────────── + +void handleSerial() { + if (!Serial.available()) return; + + String line = Serial.readStringUntil('\n'); + line.trim(); + if (line.length() == 0) return; + + // OTP burn requires the exact string "burn" to prevent accidental triggers + if (line.equalsIgnoreCase("burn")) { + AEAT8800* enc = activeEncoder(); + Serial.printf("[OTP] Unlocking ENC%u and burning shadow regs to OTP...\n", g_activeEnc); + enc->spiWrite(REG_LOCK, UNLOCK_KEY); + delayMicroseconds(10); + enc->spiWrite(REG_PROG_CUST, PROG_KEY); + Serial.println(F("[OTP] Done. Power-cycle the encoder to verify.")); + return; + } + + char cmd = (char)tolower((unsigned char)line[0]); + + switch (cmd) { + + // ── Read both positions (SSI) ───────────────────────────────────────── + case 'p': { + uint16_t pos1 = 0, pos2 = 0; + bool ok1 = false, ok2 = false; + readBothEncoders(pos1, pos2, ok1, ok2); + printSinglePosition(1, pos1, ok1); + printSinglePosition(2, pos2, ok2); + break; + } + + // ── Continuous stream toggle ────────────────────────────────────────── + case 'c': + g_streaming = !g_streaming; + Serial.printf("[INFO] Continuous stream %s\n", g_streaming ? "ON" : "OFF"); + break; + + // ── Select active encoder for SPI operations ────────────────────────── + case '1': + g_activeEnc = 1; + Serial.println(F("[INFO] Active encoder: ENC1")); + break; + case '2': + g_activeEnc = 2; + Serial.println(F("[INFO] Active encoder: ENC2")); + break; + + // ── Register dump (SPI) ─────────────────────────────────────────────── + case 's': + printAllRegisters(); + break; + + // ── Read single register "r " ───────────────────────────── + case 'r': { + if (line.length() < 3) { + Serial.println(F("[ERR] Usage: r e.g. 'r 07'")); + break; + } + uint8_t addr = (uint8_t)strtoul(line.c_str() + 2, nullptr, 16); + isolatePeer(); + uint8_t val = activeEncoder()->spiRead(addr); + releasePeer(); + Serial.printf("[ENC%u SPI-RD] Reg 0x%02X = 0x%02X (%u)\n", + g_activeEnc, addr, val, val); + break; + } + + // ── Write single register "w " ────────────────── + case 'w': { + if (line.length() < 5) { + Serial.println(F("[ERR] Usage: w e.g. 'w 10 AB'")); + break; + } + char* ptr = nullptr; + uint8_t addr = (uint8_t)strtoul(line.c_str() + 2, &ptr, 16); + uint8_t val = (uint8_t)strtoul(ptr, nullptr, 16); + activeEncoder()->spiWrite(addr, val); + isolatePeer(); + uint8_t rb = activeEncoder()->spiRead(addr); + releasePeer(); + Serial.printf("[ENC%u SPI-WR] Reg 0x%02X <- 0x%02X | Readback: 0x%02X %s\n", + g_activeEnc, addr, val, rb, (rb == val) ? "OK" : "MISMATCH!"); + break; + } + + // ── Unlock registers ────────────────────────────────────────────────── + case 'u': + activeEncoder()->spiWrite(REG_LOCK, UNLOCK_KEY); + Serial.printf("[ENC%u SPI] Registers unlocked (0xAB written to Lock reg)\n", + g_activeEnc); + break; + + // ── Set zero position ───────────────────────────────────────────────── + case 'z': { + AEAT8800* enc = activeEncoder(); + Serial.printf("[ENC%u ZERO] Writing 0x0000 to ZeroPos shadow registers...\n", + g_activeEnc); + enc->spiWrite(REG_LOCK, UNLOCK_KEY); + delayMicroseconds(10); + enc->spiWrite(REG_ZERO_POS_L, 0x00); + enc->spiWrite(REG_ZERO_POS_H, 0x00); + isolatePeer(); + uint8_t lo = enc->spiRead(REG_ZERO_POS_L); + uint8_t hi = enc->spiRead(REG_ZERO_POS_H); + releasePeer(); + uint16_t zp = ((uint16_t)hi << 8) | lo; + Serial.printf("[ENC%u ZERO] Readback: 0x%04X — %s\n", g_activeEnc, zp, + zp == 0x0000 ? "OK" : "MISMATCH!"); + break; + } + + // ── Burn guard (user must type "burn" exactly) ──────────────────────── + case 'b': + Serial.println(F("[WARN] OTP BURN is IRREVERSIBLE!")); + Serial.println(F("[WARN] Type exactly 'burn' and press Enter to confirm.")); + break; + + // ── Help ────────────────────────────────────────────────────────────── + case 'h': + default: + printHelp(); + break; + } +} + +// ─── Setup ──────────────────────────────────────────────────────────────────── + +void setup() { + Serial.begin(115200); + delay(300); + + // ── External antenna ───────────────────────────────────────────────────── + // GPIO3 LOW activates the RF switch control circuit; GPIO14 HIGH selects + // the external antenna over the built-in ceramic one. + pinMode(WIFI_ENABLE, OUTPUT); + digitalWrite(WIFI_ENABLE, LOW); // activate RF switch control + delay(100); + pinMode(WIFI_ANT_CONFIG, OUTPUT); + digitalWrite(WIFI_ANT_CONFIG, HIGH); // select external antenna + + // ── GPIO init — all bus pins to safe idle states ────────────────────────── + pinMode(PIN_CLK1, OUTPUT); digitalWrite(PIN_CLK1, HIGH); // CLK1 idles HIGH + pinMode(PIN_CLK2, OUTPUT); digitalWrite(PIN_CLK2, HIGH); // CLK2 idles HIGH + pinMode(PIN_DIN, OUTPUT); digitalWrite(PIN_DIN, HIGH); // NSL/DIN idles HIGH + pinMode(PIN_DOUT, INPUT); + pinMode(PIN_SEL1, OUTPUT); digitalWrite(PIN_SEL1, LOW); // SPI/idle mode + pinMode(PIN_SEL2, OUTPUT); digitalWrite(PIN_SEL2, LOW); // SPI/idle mode + + // tPwrUp ~4 ms (datasheet p.6) + NSL must be HIGH >= 3 ms before first SSI read + delay(5); + + // ── Boot banner ─────────────────────────────────────────────────────────── + Serial.println(F("\n╔══════════════════════════════════════════════════════════╗")); + Serial.println(F( "║ AEAT-8800-Q24 DUAL | Star-CLK | XIAO ESP32-C6 ║")); + Serial.println(F( "╠══════════════════════════════════════════════════════════╣")); + Serial.println(F( "║ CLK1=GPIO19(D8) CLK2=GPIO23(D5) DIN=GPIO18(D10) ║")); + Serial.println(F( "║ DOUT=GPIO20(D9) SEL1=GPIO21(D3) SEL2=GPIO22(D4) ║")); + Serial.println(F( "║ Antenna : External [GPIO3=LOW, GPIO14=HIGH] ║")); + Serial.println(F( "╠══════════════════════════════════════════════════════════╣")); + + // ── Configure ENC1 ──────────────────────────────────────────────────────── + // Raise SEL2 (ENC2 → SSI/idle) so ENC2's DOUT does not contend the shared + // bus during ENC1's SPI readback transactions. + Serial.print(F( "║ Configuring ENC1 ... ")); + enc2.select(); + delayMicroseconds(1); + bool ok1 = enc1.configure(); // leaves ENC1 SEL LOW on exit + enc2.deselect(); + Serial.println(ok1 ? F("OK ║") + : F("FAILED ║")); + + // ── Configure ENC2 ──────────────────────────────────────────────────────── + Serial.print(F( "║ Configuring ENC2 ... ")); + enc1.select(); + delayMicroseconds(1); + bool ok2 = enc2.configure(); // leaves ENC2 SEL LOW on exit + enc1.deselect(); + Serial.println(ok2 ? F("OK ║") + : F("FAILED ║")); + + Serial.println(F( "╠══════════════════════════════════════════════════════════╣")); + + // ── Initial position read ───────────────────────────────────────────────── + uint16_t pos1 = 0, pos2 = 0; + bool r1 = false, r2 = false; + readBothEncoders(pos1, pos2, r1, r2); + + if (r1) + Serial.printf( "║ ENC1 Position : %5u counts (%.4f deg) ║\n", + pos1, (pos1 / 65536.0) * 360.0); + else + Serial.println(F("║ ENC1 Position : READ ERROR ║")); + + if (r2) + Serial.printf( "║ ENC2 Position : %5u counts (%.4f deg) ║\n", + pos2, (pos2 / 65536.0) * 360.0); + else + Serial.println(F("║ ENC2 Position : READ ERROR ║")); + + Serial.println(F( "╠══════════════════════════════════════════════════════════╣")); + Serial.println(F( "║ Type 'h' for command help ║")); + Serial.println(F( "╚══════════════════════════════════════════════════════════╝\n")); +} + +// ─── Main Loop ──────────────────────────────────────────────────────────────── + +void loop() { + handleSerial(); + + if (g_streaming && (millis() - g_lastStream >= 100)) { + g_lastStream = millis(); + + uint16_t pos1 = 0, pos2 = 0; + bool ok1 = false, ok2 = false; + readBothEncoders(pos1, pos2, ok1, ok2); + + float deg1 = ok1 ? (pos1 / 65536.0f) * 360.0f : -1.0f; + float deg2 = ok2 ? (pos2 / 65536.0f) * 360.0f : -1.0f; + + if (ok1 && ok2) { + Serial.printf("ENC1: %.4f deg | ENC2: %.4f deg\n", deg1, deg2); + } else if (!ok1 && !ok2) { + Serial.println(F("ENC1: ERR | ENC2: ERR")); + } else if (!ok1) { + Serial.printf("ENC1: ERR | ENC2: %.4f deg\n", deg2); + } else { + Serial.printf("ENC1: %.4f deg | ENC2: ERR\n", deg1); + } + } +} diff --git a/firmware/dual_encoder/dual_encoder.ino b/firmware/dual_encoder/dual_encoder.ino index e1ecd67..7096c07 100644 --- a/firmware/dual_encoder/dual_encoder.ino +++ b/firmware/dual_encoder/dual_encoder.ino @@ -1,270 +1,270 @@ -#include -#include -#include -#include -#include -#include -#include // For std::sort - -/** - * ============================================================ - * Dual Encoder BLE Firmware Template [firmware/dual_encoder/] - * Board: XIAO ESP32C3 / ESP32C6 - * Sensor: 2x AS5047D 14-bit Magnetic Rotary Encoder (SPI) - * - * Features: - * - Reads two encoders via SPI using two separate CS pins. - * - Sends data via BLE when connected (responds to "READ"). - * - Automatically reads and outputs to Serial every 500ms - * when BLE is disconnected. - * ============================================================ - */ - -// ============================================================ -// CONFIGURE BEFORE FLASHING -// ============================================================ -#define ESP_NAME "DUAL_ENCODER_ESP" -#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b" -#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8" -// ============================================================ - -// ---------------- AS5047D SETTINGS ---------------- -#define ANGLECOM 0x3FFF -#define DIAAGC 0x3FFC // Diagnostic and AGC register -#define MAG 0x3FFD // CORDIC magnitude register -#define RD 0x40 -#define NUM_BLOCKS 16 -#define SAMPLES_PER_BLOCK 256 -#define TOTAL_SAMPLES (NUM_BLOCKS * SAMPLES_PER_BLOCK) // 4096 - -// SPI Pins (XIAO ESP32C3/C6) -// SCK=D1, MISO=D0, MOSI=D10 -// Two CS pins for the two encoders -const int PIN_CS1 = D7; // CS for Encoder 1 -const int PIN_CS2 = D6; // CS for Encoder 2 - -const int PIN_SCK = D8; -const int PIN_MISO = D9; -const int PIN_MOSI = D10; - -SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); - -// ---------------- BLE STATE ---------------- -BLECharacteristic *pChar; -bool deviceConnected = false; -bool readRequested = false; -bool zeroRequest = false; -double zeroOffset1 = 0.0; -double zeroOffset2 = 0.0; -uint32_t packetIdx = 0; // Global packet counter - -// ---------------- BLE CALLBACKS ---------------- -class MyServerCallbacks : public BLEServerCallbacks { - void onConnect(BLEServer *pServer) { - deviceConnected = true; - Serial.println("[BLE] Connected"); - } - void onDisconnect(BLEServer *pServer) { - deviceConnected = false; - Serial.println("[BLE] Disconnected"); - BLEDevice::getAdvertising()->start(); - } -}; - -class WriteCallback : public BLECharacteristicCallbacks { - void onWrite(BLECharacteristic *pChar) { - String value = pChar->getValue().c_str(); - if (value == "READ") { - readRequested = true; - } else if (value == "ZERO") { - zeroRequest = true; - Serial.println("[CMD] Zero reset requested"); - } - } -}; - -// ---------------- UTILS ---------------- -uint16_t evenParityBit(uint16_t x) { - x &= 0x7FFF; - return __builtin_parity(x); -} - -uint16_t makeReadCmd(uint16_t addr) { - uint16_t cmd = (1 << 14) | (addr & 0x3FFF); - cmd |= (evenParityBit(cmd) << 15); - return cmd; -} - -uint16_t AS5047D_Read(int csPin) { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(csPin, LOW); - SPI.transfer16(makeReadCmd(ANGLECOM)); - digitalWrite(csPin, HIGH); - delayMicroseconds(1); - digitalWrite(csPin, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(csPin, HIGH); - SPI.endTransaction(); - return result; -} - -uint16_t AS5047D_ReadRegister(int csPin, uint16_t address) { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(csPin, LOW); - SPI.transfer16(makeReadCmd(address)); - digitalWrite(csPin, HIGH); - delayMicroseconds(1); - digitalWrite(csPin, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(csPin, HIGH); - SPI.endTransaction(); - return result; -} - -// ---------------- STATISTICS ---------------- -double getRobustMean(uint16_t *samples, int size) { - double sum = 0; - for (int i = 0; i < size; i++) - sum += (samples[i] & 0x3FFF); - double initialMean = sum / size; - - double robustSum = 0; - int count = 0; - for (int i = 0; i < size; i++) { - uint16_t val = samples[i] & 0x3FFF; - if (abs((double)val - initialMean) < 1.5) { - robustSum += val; - count++; - } - } - return (count > 0) ? (robustSum / count) : initialMean; -} - -double getMedian(double *values, int size) { - std::sort(values, values + size); - return values[size / 2]; -} - -double getUltraPrecisionReading(int csPin) { - double blockMeans[NUM_BLOCKS]; - uint16_t blockSamples[SAMPLES_PER_BLOCK]; - - for (int b = 0; b < NUM_BLOCKS; b++) { - for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { - uint16_t raw = AS5047D_Read(csPin); - if (((raw >> 15) & 1) == evenParityBit(raw)) { - blockSamples[s] = raw; - } else { - s--; // Retry on parity error - } - } - blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); - delayMicroseconds(50); // Brief yield to keep BLE alive - } - - double finalCounts = getMedian(blockMeans, NUM_BLOCKS); - double rawAngle = (finalCounts * 360.0) / 16384.0; - return rawAngle; -} - -// ---------------- SETUP ---------------- -void setup() { - Serial.begin(115200); - - pinMode(PIN_CS1, OUTPUT); - digitalWrite(PIN_CS1, HIGH); - pinMode(PIN_CS2, OUTPUT); - digitalWrite(PIN_CS2, HIGH); - - SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS1); - - BLEDevice::init(ESP_NAME); - BLEServer *pServer = BLEDevice::createServer(); - pServer->setCallbacks(new MyServerCallbacks()); - BLEService *pService = pServer->createService(SERVICE_UUID); - pChar = pService->createCharacteristic(CHARACTERISTIC_UUID, - BLECharacteristic::PROPERTY_NOTIFY | - BLECharacteristic::PROPERTY_WRITE); - pChar->addDescriptor(new BLE2902()); - pChar->setCallbacks(new WriteCallback()); - pService->start(); - BLEDevice::getAdvertising()->start(); - Serial.println("[BLE] Advertising as '" ESP_NAME "'"); -} - -// ---------------- MAIN LOOP ---------------- -void loop() { - - // Auto-trigger read every 500ms if BLE is not connected - if (!deviceConnected) { - static unsigned long lastAutoReadTime = 0; - if (millis() - lastAutoReadTime >= 500) { - lastAutoReadTime = millis(); - readRequested = true; - } - } - - // On-demand read from BLE, or auto read from loop above - if (readRequested) { - readRequested = false; - packetIdx++; - - double rawAngle1 = getUltraPrecisionReading(PIN_CS1); - double rawAngle2 = getUltraPrecisionReading(PIN_CS2); - - // Handle zero reset (applied on next READ after ZERO command) - if (zeroRequest) { - zeroOffset1 = rawAngle1; - zeroOffset2 = rawAngle2; - zeroRequest = false; - Serial.printf("[ZERO] Offsets set to %.5f and %.5f\n", zeroOffset1, zeroOffset2); - } - - // Apply zero offset and normalize to [0, 360) for Enc 1 - double angle1 = rawAngle1 - zeroOffset1; - if (angle1 < 0) angle1 += 360.0; - if (angle1 >= 360.0) angle1 -= 360.0; - int angleInt1 = (int)(angle1 * 10000.0); - - // Apply zero offset and normalize to [0, 360) for Enc 2 - double angle2 = rawAngle2 - zeroOffset2; - if (angle2 < 0) angle2 += 360.0; - if (angle2 >= 360.0) angle2 -= 360.0; - int angleInt2 = (int)(angle2 * 10000.0); - - // Read diagnostics Enc1 - uint16_t diaagc1 = AS5047D_ReadRegister(PIN_CS1, DIAAGC); - uint16_t mag1_reg = AS5047D_ReadRegister(PIN_CS1, MAG); - uint8_t agc1 = diaagc1 & 0xFF; - uint8_t cof1 = (diaagc1 >> 9) & 1; - uint8_t magl1 = (diaagc1 >> 10) & 1; - uint8_t magh1 = (diaagc1 >> 11) & 1; - uint16_t magnitude1 = mag1_reg & 0x3FFF; - - // Read diagnostics Enc2 - uint16_t diaagc2 = AS5047D_ReadRegister(PIN_CS2, DIAAGC); - uint16_t mag2_reg = AS5047D_ReadRegister(PIN_CS2, MAG); - uint8_t agc2 = diaagc2 & 0xFF; - uint8_t cof2 = (diaagc2 >> 9) & 1; - uint8_t magl2 = (diaagc2 >> 10) & 1; - uint8_t magh2 = (diaagc2 >> 11) & 1; - uint16_t magnitude2 = mag2_reg & 0x3FFF; - - // BLE message: single encoder uses same format, here we concatenate M0 and M1 - String bleMsg = String(packetIdx) - + "|M0:" + String(angleInt1) + "," + String(packetIdx) + "," + String(agc1) + "," + String(magnitude1) + "," + String(magl1) + "," + String(magh1) + "," + String(cof1) - + "|M1:" + String(angleInt2) + "," + String(packetIdx) + "," + String(agc2) + "," + String(magnitude2) + "," + String(magl2) + "," + String(magh2) + "," + String(cof2); - - if (deviceConnected && pChar) { - pChar->setValue(bleMsg.c_str()); - pChar->notify(); - } - - Serial.printf("[READ #%u] M0=%d (%.5f deg) | M1=%d (%.5f deg) \n", - packetIdx, angleInt1, angle1, angleInt2, angle2); - } - - delay(1); -} +#include +#include +#include +#include +#include +#include +#include // For std::sort + +/** + * ============================================================ + * Dual Encoder BLE Firmware Template [firmware/dual_encoder/] + * Board: XIAO ESP32C3 / ESP32C6 + * Sensor: 2x AS5047D 14-bit Magnetic Rotary Encoder (SPI) + * + * Features: + * - Reads two encoders via SPI using two separate CS pins. + * - Sends data via BLE when connected (responds to "READ"). + * - Automatically reads and outputs to Serial every 500ms + * when BLE is disconnected. + * ============================================================ + */ + +// ============================================================ +// CONFIGURE BEFORE FLASHING +// ============================================================ +#define ESP_NAME "DUAL_ENCODER_ESP" +#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b" +#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8" +// ============================================================ + +// ---------------- AS5047D SETTINGS ---------------- +#define ANGLECOM 0x3FFF +#define DIAAGC 0x3FFC // Diagnostic and AGC register +#define MAG 0x3FFD // CORDIC magnitude register +#define RD 0x40 +#define NUM_BLOCKS 16 +#define SAMPLES_PER_BLOCK 256 +#define TOTAL_SAMPLES (NUM_BLOCKS * SAMPLES_PER_BLOCK) // 4096 + +// SPI Pins (XIAO ESP32C3/C6) +// SCK=D1, MISO=D0, MOSI=D10 +// Two CS pins for the two encoders +const int PIN_CS1 = D7; // CS for Encoder 1 +const int PIN_CS2 = D6; // CS for Encoder 2 + +const int PIN_SCK = D8; +const int PIN_MISO = D9; +const int PIN_MOSI = D10; + +SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); + +// ---------------- BLE STATE ---------------- +BLECharacteristic *pChar; +bool deviceConnected = false; +bool readRequested = false; +bool zeroRequest = false; +double zeroOffset1 = 0.0; +double zeroOffset2 = 0.0; +uint32_t packetIdx = 0; // Global packet counter + +// ---------------- BLE CALLBACKS ---------------- +class MyServerCallbacks : public BLEServerCallbacks { + void onConnect(BLEServer *pServer) { + deviceConnected = true; + Serial.println("[BLE] Connected"); + } + void onDisconnect(BLEServer *pServer) { + deviceConnected = false; + Serial.println("[BLE] Disconnected"); + BLEDevice::getAdvertising()->start(); + } +}; + +class WriteCallback : public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *pChar) { + String value = pChar->getValue().c_str(); + if (value == "READ") { + readRequested = true; + } else if (value == "ZERO") { + zeroRequest = true; + Serial.println("[CMD] Zero reset requested"); + } + } +}; + +// ---------------- UTILS ---------------- +uint16_t evenParityBit(uint16_t x) { + x &= 0x7FFF; + return __builtin_parity(x); +} + +uint16_t makeReadCmd(uint16_t addr) { + uint16_t cmd = (1 << 14) | (addr & 0x3FFF); + cmd |= (evenParityBit(cmd) << 15); + return cmd; +} + +uint16_t AS5047D_Read(int csPin) { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(csPin, LOW); + SPI.transfer16(makeReadCmd(ANGLECOM)); + digitalWrite(csPin, HIGH); + delayMicroseconds(1); + digitalWrite(csPin, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(csPin, HIGH); + SPI.endTransaction(); + return result; +} + +uint16_t AS5047D_ReadRegister(int csPin, uint16_t address) { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(csPin, LOW); + SPI.transfer16(makeReadCmd(address)); + digitalWrite(csPin, HIGH); + delayMicroseconds(1); + digitalWrite(csPin, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(csPin, HIGH); + SPI.endTransaction(); + return result; +} + +// ---------------- STATISTICS ---------------- +double getRobustMean(uint16_t *samples, int size) { + double sum = 0; + for (int i = 0; i < size; i++) + sum += (samples[i] & 0x3FFF); + double initialMean = sum / size; + + double robustSum = 0; + int count = 0; + for (int i = 0; i < size; i++) { + uint16_t val = samples[i] & 0x3FFF; + if (abs((double)val - initialMean) < 1.5) { + robustSum += val; + count++; + } + } + return (count > 0) ? (robustSum / count) : initialMean; +} + +double getMedian(double *values, int size) { + std::sort(values, values + size); + return values[size / 2]; +} + +double getUltraPrecisionReading(int csPin) { + double blockMeans[NUM_BLOCKS]; + uint16_t blockSamples[SAMPLES_PER_BLOCK]; + + for (int b = 0; b < NUM_BLOCKS; b++) { + for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { + uint16_t raw = AS5047D_Read(csPin); + if (((raw >> 15) & 1) == evenParityBit(raw)) { + blockSamples[s] = raw; + } else { + s--; // Retry on parity error + } + } + blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); + delayMicroseconds(50); // Brief yield to keep BLE alive + } + + double finalCounts = getMedian(blockMeans, NUM_BLOCKS); + double rawAngle = (finalCounts * 360.0) / 16384.0; + return rawAngle; +} + +// ---------------- SETUP ---------------- +void setup() { + Serial.begin(115200); + + pinMode(PIN_CS1, OUTPUT); + digitalWrite(PIN_CS1, HIGH); + pinMode(PIN_CS2, OUTPUT); + digitalWrite(PIN_CS2, HIGH); + + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS1); + + BLEDevice::init(ESP_NAME); + BLEServer *pServer = BLEDevice::createServer(); + pServer->setCallbacks(new MyServerCallbacks()); + BLEService *pService = pServer->createService(SERVICE_UUID); + pChar = pService->createCharacteristic(CHARACTERISTIC_UUID, + BLECharacteristic::PROPERTY_NOTIFY | + BLECharacteristic::PROPERTY_WRITE); + pChar->addDescriptor(new BLE2902()); + pChar->setCallbacks(new WriteCallback()); + pService->start(); + BLEDevice::getAdvertising()->start(); + Serial.println("[BLE] Advertising as '" ESP_NAME "'"); +} + +// ---------------- MAIN LOOP ---------------- +void loop() { + + // Auto-trigger read every 500ms if BLE is not connected + if (!deviceConnected) { + static unsigned long lastAutoReadTime = 0; + if (millis() - lastAutoReadTime >= 500) { + lastAutoReadTime = millis(); + readRequested = true; + } + } + + // On-demand read from BLE, or auto read from loop above + if (readRequested) { + readRequested = false; + packetIdx++; + + double rawAngle1 = getUltraPrecisionReading(PIN_CS1); + double rawAngle2 = getUltraPrecisionReading(PIN_CS2); + + // Handle zero reset (applied on next READ after ZERO command) + if (zeroRequest) { + zeroOffset1 = rawAngle1; + zeroOffset2 = rawAngle2; + zeroRequest = false; + Serial.printf("[ZERO] Offsets set to %.5f and %.5f\n", zeroOffset1, zeroOffset2); + } + + // Apply zero offset and normalize to [0, 360) for Enc 1 + double angle1 = rawAngle1 - zeroOffset1; + if (angle1 < 0) angle1 += 360.0; + if (angle1 >= 360.0) angle1 -= 360.0; + int angleInt1 = (int)(angle1 * 10000.0); + + // Apply zero offset and normalize to [0, 360) for Enc 2 + double angle2 = rawAngle2 - zeroOffset2; + if (angle2 < 0) angle2 += 360.0; + if (angle2 >= 360.0) angle2 -= 360.0; + int angleInt2 = (int)(angle2 * 10000.0); + + // Read diagnostics Enc1 + uint16_t diaagc1 = AS5047D_ReadRegister(PIN_CS1, DIAAGC); + uint16_t mag1_reg = AS5047D_ReadRegister(PIN_CS1, MAG); + uint8_t agc1 = diaagc1 & 0xFF; + uint8_t cof1 = (diaagc1 >> 9) & 1; + uint8_t magl1 = (diaagc1 >> 10) & 1; + uint8_t magh1 = (diaagc1 >> 11) & 1; + uint16_t magnitude1 = mag1_reg & 0x3FFF; + + // Read diagnostics Enc2 + uint16_t diaagc2 = AS5047D_ReadRegister(PIN_CS2, DIAAGC); + uint16_t mag2_reg = AS5047D_ReadRegister(PIN_CS2, MAG); + uint8_t agc2 = diaagc2 & 0xFF; + uint8_t cof2 = (diaagc2 >> 9) & 1; + uint8_t magl2 = (diaagc2 >> 10) & 1; + uint8_t magh2 = (diaagc2 >> 11) & 1; + uint16_t magnitude2 = mag2_reg & 0x3FFF; + + // BLE message: single encoder uses same format, here we concatenate M0 and M1 + String bleMsg = String(packetIdx) + + "|M0:" + String(angleInt1) + "," + String(packetIdx) + "," + String(agc1) + "," + String(magnitude1) + "," + String(magl1) + "," + String(magh1) + "," + String(cof1) + + "|M1:" + String(angleInt2) + "," + String(packetIdx) + "," + String(agc2) + "," + String(magnitude2) + "," + String(magl2) + "," + String(magh2) + "," + String(cof2); + + if (deviceConnected && pChar) { + pChar->setValue(bleMsg.c_str()); + pChar->notify(); + } + + Serial.printf("[READ #%u] M0=%d (%.5f deg) | M1=%d (%.5f deg) \n", + packetIdx, angleInt1, angle1, angleInt2, angle2); + } + + delay(1); +} diff --git a/firmware/master/master.ino b/firmware/master/master.ino index a792f3b..d0a56af 100644 --- a/firmware/master/master.ino +++ b/firmware/master/master.ino @@ -1,469 +1,469 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* - * Oliver Master Gateway - Robust Discovery Edition - * Hardware: XIAO ESP32C6 - * Features: - * - 30s blocking discovery for all 5 slaves - * - Continuous re-discovery for missing slaves - * - BLE command interface for manual re-discovery - * - Detailed status reporting - * Author: Swaraj Dangare - */ - -#define NUM_SLAVES 1 // FIXED: Was 4, now 5 -#define WIFI_CHANNEL 11 // ESP-NOW channel (use 1, 6, or 11 to isolate from other Oliver sets) -#define SERVICE_UUID "6ab88bb9-cf50-4564-b1c4-f53be2abc53f" -#define CHARACTERISTIC_UUID "1d4cd358-172d-4c33-b0b2-ddce9a071aab" -#define COMMAND_UUID "308a0c43-80f0-4b01-81e5-bb2798eb92f9" - -typedef struct __attribute__((packed)) { - uint8_t id; - int value; // Angle * 10000 - uint32_t packetIdx; - uint8_t agc; // AS5047D AGC value (0-255) - uint16_t mag; // AS5047D CORDIC magnitude (14-bit) - uint8_t magl; // Magnetic field too low (0 or 1) - uint8_t magh; // Magnetic field too high (0 or 1) - uint8_t cof; // CORDIC overflow (0 or 1) -} Payload; - -uint8_t slaveMACs[NUM_SLAVES][6]; -bool slaveFound[NUM_SLAVES] = {false}; -Payload slaves[NUM_SLAVES]; -uint32_t gatewayPacketIdx = 0; -uint32_t lastSeenTime[NUM_SLAVES] = {0}; // Track last response time - -BLECharacteristic *pChar; -BLECharacteristic *pCommandChar; -bool pcConnected = false; -bool rediscoverRequested = false; -bool readRequested = false; -bool slaveResponded[NUM_SLAVES] = {false}; - -// ---------------- MASTER ENCODER SETTINGS ---------------- -#define ANGLECOM 0x3FFF -#define DIAAGC_REG 0x3FFC -#define MAG_REG 0x3FFD -#define RD 0x40 -#define NUM_BLOCKS 16 -#define SAMPLES_PER_BLOCK 256 - -const int PIN_CS = D7; -const int PIN_SCK = D1; -const int PIN_MISO = D0; -const int PIN_MOSI = D10; - -SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); - -// Master encoder data -Payload masterData = {255, 0, 0, 0, 0, 0, 0, 0}; // id=255 for master - -// ESP-NOW Receive Callback -void onEspNowRecv(const esp_now_recv_info_t *info, const uint8_t *data, - int len) { - if (len == 1) { // Discovery Response - uint8_t id = data[0]; - if (id < NUM_SLAVES) { - if (!slaveFound[id]) { - memcpy(slaveMACs[id], info->src_addr, 6); - slaveFound[id] = true; - esp_now_peer_info_t peer{}; - memcpy(peer.peer_addr, info->src_addr, 6); - peer.channel = WIFI_CHANNEL; - peer.encrypt = false; - esp_now_add_peer(&peer); - // Safe to print here - quick message - Serial.printf( - "[DISCOVERY] Found Slave %d: %02X:%02X:%02X:%02X:%02X:%02X\n", id, - info->src_addr[0], info->src_addr[1], info->src_addr[2], - info->src_addr[3], info->src_addr[4], info->src_addr[5]); - } - lastSeenTime[id] = millis(); - } - } else if (len == sizeof(Payload)) { // Data Response - Payload p; - memcpy(&p, data, sizeof(p)); - if (p.id < NUM_SLAVES) { - slaves[p.id] = p; - lastSeenTime[p.id] = millis(); - slaveResponded[p.id] = true; - } - } -} - -// ---------------- MASTER ENCODER FUNCTIONS ---------------- -uint16_t evenParityBit(uint16_t x) { - x &= 0x7FFF; - return __builtin_parity(x); -} - -uint16_t makeReadCmd(uint16_t addr) { - uint16_t cmd = (1 << 14) | (addr & 0x3FFF); - cmd |= (evenParityBit(cmd) << 15); - return cmd; -} - -uint16_t AS5047D_Read() { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(PIN_CS, LOW); - SPI.transfer16(makeReadCmd(ANGLECOM)); - digitalWrite(PIN_CS, HIGH); - delayMicroseconds(1); - digitalWrite(PIN_CS, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(PIN_CS, HIGH); - SPI.endTransaction(); - return result; -} - -uint16_t readRegister(uint16_t addr) { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(PIN_CS, LOW); - SPI.transfer16(makeReadCmd(addr)); - digitalWrite(PIN_CS, HIGH); - delayMicroseconds(1); - digitalWrite(PIN_CS, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(PIN_CS, HIGH); - SPI.endTransaction(); - return result & 0x3FFF; -} - -double getRobustMean(uint16_t *samples, int size) { - double sum = 0; - for (int i = 0; i < size; i++) - sum += (samples[i] & 0x3FFF); - double initialMean = sum / size; - - double robustSum = 0; - int count = 0; - for (int i = 0; i < size; i++) { - uint16_t val = samples[i] & 0x3FFF; - if (abs((double)val - initialMean) < 1.5) { - robustSum += val; - count++; - } - } - return (count > 0) ? (robustSum / count) : initialMean; -} - -double getMedian(double *values, int size) { - std::sort(values, values + size); - return values[size / 2]; -} - -double getUltraPrecisionReading() { - double blockMeans[NUM_BLOCKS]; - uint16_t blockSamples[SAMPLES_PER_BLOCK]; - - for (int b = 0; b < NUM_BLOCKS; b++) { - for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { - uint16_t raw = AS5047D_Read(); - if (((raw >> 15) & 1) == evenParityBit(raw)) { - blockSamples[s] = raw; - } else { - s--; - } - } - blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); - delayMicroseconds(50); - } - double finalCounts = getMedian(blockMeans, NUM_BLOCKS); - return (finalCounts * 360.0) / 16384.0; -} - -// BLE Callbacks -class ServerCallbacks : public BLEServerCallbacks { - void onConnect(BLEServer *) { - pcConnected = true; - Serial.println("[BLE] Client Connected"); - } - void onDisconnect(BLEServer *) { - pcConnected = false; - Serial.println("[BLE] Client Disconnected"); - BLEDevice::startAdvertising(); - } -}; - -// Command Handler -class CommandCallbacks : public BLECharacteristicCallbacks { - void onWrite(BLECharacteristic *pChar) { - String value = - pChar->getValue().c_str(); // Convert std::string to Arduino String - if (value == "READ") { - readRequested = true; - for (int i = 0; i < NUM_SLAVES; i++) - slaveResponded[i] = false; - } else if (value == "REDISCOVER") { - rediscoverRequested = true; - Serial.println("[CMD] Re-discovery requested from PC"); - } - } -}; - -// Discovery Function -void discoverSlaves(uint32_t timeoutMs) { - uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - uint32_t startTime = millis(); - - Serial.printf( - "[DISCOVERY] Starting discovery for %d slaves (timeout: %dms)...\n", - NUM_SLAVES, timeoutMs); - - while (millis() - startTime < timeoutMs) { - // Count found slaves - int foundCount = 0; - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i]) - foundCount++; - } - - // Exit early if all found - if (foundCount == NUM_SLAVES) { - Serial.println("[DISCOVERY] All slaves found!"); - break; - } - - // Send broadcast - uint8_t ping = 0xFF; - esp_now_send(broadcastAddr, &ping, 1); - - // Print status every 2 seconds - static uint32_t lastPrint = 0; - if (millis() - lastPrint > 2000) { - lastPrint = millis(); - Serial.printf("[DISCOVERY] Progress: %d/%d slaves found | Missing: ", - foundCount, NUM_SLAVES); - for (int i = 0; i < NUM_SLAVES; i++) { - if (!slaveFound[i]) - Serial.printf("S%d ", i); - } - Serial.println(); - } - - delay(200); // Broadcast every 200ms (was 500ms) - } - - // Final report - int finalCount = 0; - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i]) - finalCount++; - } - - Serial.println("\n" + String('=', 50)); - Serial.printf("[DISCOVERY] Complete: %d/%d slaves discovered\n", finalCount, - NUM_SLAVES); - if (finalCount < NUM_SLAVES) { - Serial.print("[WARNING] Missing slaves: "); - for (int i = 0; i < NUM_SLAVES; i++) { - if (!slaveFound[i]) - Serial.printf("S%d ", i); - } - Serial.println("\n[INFO] Will retry in background..."); - } - Serial.println(String('=', 50) + "\n"); -} - -void setup() { - Serial.begin(115200); - delay(1000); // Give serial time to initialize - - Serial.println("\n\n=== OLIVER MASTER GATEWAY ==="); - Serial.println("Hardware: XIAO ESP32C6"); - Serial.printf("Firmware: Robust Discovery v2.0\n\n"); - - // WiFi Init - WiFi.mode(WIFI_STA); - WiFi.disconnect(); - Serial.printf("[WIFI] MAC Address: %s\n", WiFi.macAddress().c_str()); - - // Master Encoder SPI Init - pinMode(PIN_CS, OUTPUT); - digitalWrite(PIN_CS, HIGH); - SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); - Serial.println("[SPI] Master encoder initialized"); - - // Force WiFi channel - esp_wifi_set_promiscuous(true); - esp_wifi_set_channel(WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE); - esp_wifi_set_promiscuous(false); - Serial.printf("[WIFI] Channel %d locked\n", WIFI_CHANNEL); - - // ESP-NOW Init - if (esp_now_init() != ESP_OK) { - Serial.println("[ERROR] ESP-NOW Init Failed!"); - return; - } - esp_now_register_recv_cb(onEspNowRecv); - Serial.println("[ESP-NOW] Initialized"); - - // Add Broadcast Peer - uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - esp_now_peer_info_t bcast{}; - memcpy(bcast.peer_addr, broadcastAddr, 6); - bcast.channel = WIFI_CHANNEL; - bcast.encrypt = false; - esp_now_add_peer(&bcast); - Serial.println("[ESP-NOW] Broadcast peer added\n"); - - // Initial Discovery (30 seconds, blocking) - discoverSlaves(30000); - - // BLE Init - Serial.println("[BLE] Initializing..."); - BLEDevice::init("Oliver_3"); - BLEServer *pServer = BLEDevice::createServer(); - pServer->setCallbacks(new ServerCallbacks()); - - BLEService *pService = pServer->createService(SERVICE_UUID); - - // Data characteristic (notify) - pChar = pService->createCharacteristic(CHARACTERISTIC_UUID, - BLECharacteristic::PROPERTY_NOTIFY); - pChar->addDescriptor(new BLE2902()); - - // Command characteristic (write) - pCommandChar = pService->createCharacteristic( - COMMAND_UUID, BLECharacteristic::PROPERTY_WRITE); - pCommandChar->setCallbacks(new CommandCallbacks()); - - pService->start(); - - BLEAdvertising *pAdvertising = BLEDevice::getAdvertising(); - pAdvertising->addServiceUUID(SERVICE_UUID); - pAdvertising->setScanResponse(true); - BLEDevice::startAdvertising(); - - Serial.println("[BLE] Advertising as 'Oliver_3'"); - Serial.println("\n=== GATEWAY READY ===\n"); -} - -void loop() { - static uint32_t lastRediscover = 0; - - // Handle manual re-discovery request - if (rediscoverRequested) { - rediscoverRequested = false; - Serial.println("\n[CMD] Manual re-discovery triggered!"); - for (int i = 0; i < NUM_SLAVES; i++) { - if (!slaveFound[i]) { - Serial.printf("[REDISCOVER] Will search for S%d\n", i); - } - } - discoverSlaves(15000); - } - - // Automatic re-discovery every 10 seconds for missing slaves - if (millis() - lastRediscover > 10000) { - lastRediscover = millis(); - int missingCount = 0; - for (int i = 0; i < NUM_SLAVES; i++) { - if (!slaveFound[i]) - missingCount++; - } - - if (missingCount > 0) { - Serial.printf("[AUTO-REDISCOVER] Searching for %d missing slaves...\n", - missingCount); - uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - for (int attempt = 0; attempt < 5; attempt++) { - uint8_t ping = 0xFF; - esp_now_send(broadcastAddr, &ping, 1); - delay(200); - } - } - } - - // ── On-demand read: triggered by BLE "READ" command from PC ── - if (readRequested) { - readRequested = false; - gatewayPacketIdx++; - - // 1. Read master's own encoder + diagnostics - double masterAngle = getUltraPrecisionReading(); - masterData.value = (int)(masterAngle * 10000.0); - masterData.packetIdx = gatewayPacketIdx; - - uint16_t diaagc = readRegister(DIAAGC_REG); - uint16_t mag = readRegister(MAG_REG); - masterData.agc = diaagc & 0xFF; - masterData.mag = mag & 0x3FFF; - masterData.magl = (diaagc >> 8) & 0x01; - masterData.magh = (diaagc >> 10) & 0x01; - masterData.cof = (diaagc >> 9) & 0x01; - - // 2. Request data from all discovered slaves - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i]) { - uint8_t req = i; - esp_now_send(slaveMACs[i], &req, 1); - } - } - - // 3. Wait for slave responses (up to 200ms timeout) - uint32_t waitStart = millis(); - while (millis() - waitStart < 200) { - bool allResponded = true; - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i] && !slaveResponded[i]) { - allResponded = false; - break; - } - } - if (allResponded) - break; - delay(1); - } - - // 4. Build BLE message (same 7-field format) - String bleMsg = String(gatewayPacketIdx); - - bleMsg += "|M0:" + String(masterData.value) + "," + - String(masterData.packetIdx) + "," + - String(masterData.agc) + "," + String(masterData.mag) + "," + - String(masterData.magl) + "," + String(masterData.magh) + "," + - String(masterData.cof); - - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i] && slaveResponded[i]) { - bleMsg += "|S" + String(i) + ":" + String(slaves[i].value) + "," + - String(slaves[i].packetIdx) + "," + - String(slaves[i].agc) + "," + String(slaves[i].mag) + "," + - String(slaves[i].magl) + "," + String(slaves[i].magh) + "," + - String(slaves[i].cof); - } else { - bleMsg += "|S" + String(i) + ":OFFLINE,0"; - } - } - - // 5. Send BLE notification - if (pcConnected && pChar) { - pChar->setValue(bleMsg.c_str()); - pChar->notify(); - } - - // 6. Serial log - Serial.printf("[READ #%u] M0=%d", gatewayPacketIdx, masterData.value); - for (int i = 0; i < NUM_SLAVES; i++) { - if (slaveFound[i] && slaveResponded[i]) - Serial.printf(" | S%d=%d", i, slaves[i].value); - else - Serial.printf(" | S%d=OFFLINE", i); - } - Serial.println(); - } - - delay(1); +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* + * Oliver Master Gateway - Robust Discovery Edition + * Hardware: XIAO ESP32C6 + * Features: + * - 30s blocking discovery for all 5 slaves + * - Continuous re-discovery for missing slaves + * - BLE command interface for manual re-discovery + * - Detailed status reporting + * Author: Swaraj Dangare + */ + +#define NUM_SLAVES 1 // FIXED: Was 4, now 5 +#define WIFI_CHANNEL 11 // ESP-NOW channel (use 1, 6, or 11 to isolate from other Oliver sets) +#define SERVICE_UUID "6ab88bb9-cf50-4564-b1c4-f53be2abc53f" +#define CHARACTERISTIC_UUID "1d4cd358-172d-4c33-b0b2-ddce9a071aab" +#define COMMAND_UUID "308a0c43-80f0-4b01-81e5-bb2798eb92f9" + +typedef struct __attribute__((packed)) { + uint8_t id; + int value; // Angle * 10000 + uint32_t packetIdx; + uint8_t agc; // AS5047D AGC value (0-255) + uint16_t mag; // AS5047D CORDIC magnitude (14-bit) + uint8_t magl; // Magnetic field too low (0 or 1) + uint8_t magh; // Magnetic field too high (0 or 1) + uint8_t cof; // CORDIC overflow (0 or 1) +} Payload; + +uint8_t slaveMACs[NUM_SLAVES][6]; +bool slaveFound[NUM_SLAVES] = {false}; +Payload slaves[NUM_SLAVES]; +uint32_t gatewayPacketIdx = 0; +uint32_t lastSeenTime[NUM_SLAVES] = {0}; // Track last response time + +BLECharacteristic *pChar; +BLECharacteristic *pCommandChar; +bool pcConnected = false; +bool rediscoverRequested = false; +bool readRequested = false; +bool slaveResponded[NUM_SLAVES] = {false}; + +// ---------------- MASTER ENCODER SETTINGS ---------------- +#define ANGLECOM 0x3FFF +#define DIAAGC_REG 0x3FFC +#define MAG_REG 0x3FFD +#define RD 0x40 +#define NUM_BLOCKS 16 +#define SAMPLES_PER_BLOCK 256 + +const int PIN_CS = D7; +const int PIN_SCK = D1; +const int PIN_MISO = D0; +const int PIN_MOSI = D10; + +SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); + +// Master encoder data +Payload masterData = {255, 0, 0, 0, 0, 0, 0, 0}; // id=255 for master + +// ESP-NOW Receive Callback +void onEspNowRecv(const esp_now_recv_info_t *info, const uint8_t *data, + int len) { + if (len == 1) { // Discovery Response + uint8_t id = data[0]; + if (id < NUM_SLAVES) { + if (!slaveFound[id]) { + memcpy(slaveMACs[id], info->src_addr, 6); + slaveFound[id] = true; + esp_now_peer_info_t peer{}; + memcpy(peer.peer_addr, info->src_addr, 6); + peer.channel = WIFI_CHANNEL; + peer.encrypt = false; + esp_now_add_peer(&peer); + // Safe to print here - quick message + Serial.printf( + "[DISCOVERY] Found Slave %d: %02X:%02X:%02X:%02X:%02X:%02X\n", id, + info->src_addr[0], info->src_addr[1], info->src_addr[2], + info->src_addr[3], info->src_addr[4], info->src_addr[5]); + } + lastSeenTime[id] = millis(); + } + } else if (len == sizeof(Payload)) { // Data Response + Payload p; + memcpy(&p, data, sizeof(p)); + if (p.id < NUM_SLAVES) { + slaves[p.id] = p; + lastSeenTime[p.id] = millis(); + slaveResponded[p.id] = true; + } + } +} + +// ---------------- MASTER ENCODER FUNCTIONS ---------------- +uint16_t evenParityBit(uint16_t x) { + x &= 0x7FFF; + return __builtin_parity(x); +} + +uint16_t makeReadCmd(uint16_t addr) { + uint16_t cmd = (1 << 14) | (addr & 0x3FFF); + cmd |= (evenParityBit(cmd) << 15); + return cmd; +} + +uint16_t AS5047D_Read() { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(PIN_CS, LOW); + SPI.transfer16(makeReadCmd(ANGLECOM)); + digitalWrite(PIN_CS, HIGH); + delayMicroseconds(1); + digitalWrite(PIN_CS, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(PIN_CS, HIGH); + SPI.endTransaction(); + return result; +} + +uint16_t readRegister(uint16_t addr) { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(PIN_CS, LOW); + SPI.transfer16(makeReadCmd(addr)); + digitalWrite(PIN_CS, HIGH); + delayMicroseconds(1); + digitalWrite(PIN_CS, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(PIN_CS, HIGH); + SPI.endTransaction(); + return result & 0x3FFF; +} + +double getRobustMean(uint16_t *samples, int size) { + double sum = 0; + for (int i = 0; i < size; i++) + sum += (samples[i] & 0x3FFF); + double initialMean = sum / size; + + double robustSum = 0; + int count = 0; + for (int i = 0; i < size; i++) { + uint16_t val = samples[i] & 0x3FFF; + if (abs((double)val - initialMean) < 1.5) { + robustSum += val; + count++; + } + } + return (count > 0) ? (robustSum / count) : initialMean; +} + +double getMedian(double *values, int size) { + std::sort(values, values + size); + return values[size / 2]; +} + +double getUltraPrecisionReading() { + double blockMeans[NUM_BLOCKS]; + uint16_t blockSamples[SAMPLES_PER_BLOCK]; + + for (int b = 0; b < NUM_BLOCKS; b++) { + for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { + uint16_t raw = AS5047D_Read(); + if (((raw >> 15) & 1) == evenParityBit(raw)) { + blockSamples[s] = raw; + } else { + s--; + } + } + blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); + delayMicroseconds(50); + } + double finalCounts = getMedian(blockMeans, NUM_BLOCKS); + return (finalCounts * 360.0) / 16384.0; +} + +// BLE Callbacks +class ServerCallbacks : public BLEServerCallbacks { + void onConnect(BLEServer *) { + pcConnected = true; + Serial.println("[BLE] Client Connected"); + } + void onDisconnect(BLEServer *) { + pcConnected = false; + Serial.println("[BLE] Client Disconnected"); + BLEDevice::startAdvertising(); + } +}; + +// Command Handler +class CommandCallbacks : public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *pChar) { + String value = + pChar->getValue().c_str(); // Convert std::string to Arduino String + if (value == "READ") { + readRequested = true; + for (int i = 0; i < NUM_SLAVES; i++) + slaveResponded[i] = false; + } else if (value == "REDISCOVER") { + rediscoverRequested = true; + Serial.println("[CMD] Re-discovery requested from PC"); + } + } +}; + +// Discovery Function +void discoverSlaves(uint32_t timeoutMs) { + uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + uint32_t startTime = millis(); + + Serial.printf( + "[DISCOVERY] Starting discovery for %d slaves (timeout: %dms)...\n", + NUM_SLAVES, timeoutMs); + + while (millis() - startTime < timeoutMs) { + // Count found slaves + int foundCount = 0; + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i]) + foundCount++; + } + + // Exit early if all found + if (foundCount == NUM_SLAVES) { + Serial.println("[DISCOVERY] All slaves found!"); + break; + } + + // Send broadcast + uint8_t ping = 0xFF; + esp_now_send(broadcastAddr, &ping, 1); + + // Print status every 2 seconds + static uint32_t lastPrint = 0; + if (millis() - lastPrint > 2000) { + lastPrint = millis(); + Serial.printf("[DISCOVERY] Progress: %d/%d slaves found | Missing: ", + foundCount, NUM_SLAVES); + for (int i = 0; i < NUM_SLAVES; i++) { + if (!slaveFound[i]) + Serial.printf("S%d ", i); + } + Serial.println(); + } + + delay(200); // Broadcast every 200ms (was 500ms) + } + + // Final report + int finalCount = 0; + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i]) + finalCount++; + } + + Serial.println("\n" + String('=', 50)); + Serial.printf("[DISCOVERY] Complete: %d/%d slaves discovered\n", finalCount, + NUM_SLAVES); + if (finalCount < NUM_SLAVES) { + Serial.print("[WARNING] Missing slaves: "); + for (int i = 0; i < NUM_SLAVES; i++) { + if (!slaveFound[i]) + Serial.printf("S%d ", i); + } + Serial.println("\n[INFO] Will retry in background..."); + } + Serial.println(String('=', 50) + "\n"); +} + +void setup() { + Serial.begin(115200); + delay(1000); // Give serial time to initialize + + Serial.println("\n\n=== OLIVER MASTER GATEWAY ==="); + Serial.println("Hardware: XIAO ESP32C6"); + Serial.printf("Firmware: Robust Discovery v2.0\n\n"); + + // WiFi Init + WiFi.mode(WIFI_STA); + WiFi.disconnect(); + Serial.printf("[WIFI] MAC Address: %s\n", WiFi.macAddress().c_str()); + + // Master Encoder SPI Init + pinMode(PIN_CS, OUTPUT); + digitalWrite(PIN_CS, HIGH); + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); + Serial.println("[SPI] Master encoder initialized"); + + // Force WiFi channel + esp_wifi_set_promiscuous(true); + esp_wifi_set_channel(WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE); + esp_wifi_set_promiscuous(false); + Serial.printf("[WIFI] Channel %d locked\n", WIFI_CHANNEL); + + // ESP-NOW Init + if (esp_now_init() != ESP_OK) { + Serial.println("[ERROR] ESP-NOW Init Failed!"); + return; + } + esp_now_register_recv_cb(onEspNowRecv); + Serial.println("[ESP-NOW] Initialized"); + + // Add Broadcast Peer + uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + esp_now_peer_info_t bcast{}; + memcpy(bcast.peer_addr, broadcastAddr, 6); + bcast.channel = WIFI_CHANNEL; + bcast.encrypt = false; + esp_now_add_peer(&bcast); + Serial.println("[ESP-NOW] Broadcast peer added\n"); + + // Initial Discovery (30 seconds, blocking) + discoverSlaves(30000); + + // BLE Init + Serial.println("[BLE] Initializing..."); + BLEDevice::init("Oliver_3"); + BLEServer *pServer = BLEDevice::createServer(); + pServer->setCallbacks(new ServerCallbacks()); + + BLEService *pService = pServer->createService(SERVICE_UUID); + + // Data characteristic (notify) + pChar = pService->createCharacteristic(CHARACTERISTIC_UUID, + BLECharacteristic::PROPERTY_NOTIFY); + pChar->addDescriptor(new BLE2902()); + + // Command characteristic (write) + pCommandChar = pService->createCharacteristic( + COMMAND_UUID, BLECharacteristic::PROPERTY_WRITE); + pCommandChar->setCallbacks(new CommandCallbacks()); + + pService->start(); + + BLEAdvertising *pAdvertising = BLEDevice::getAdvertising(); + pAdvertising->addServiceUUID(SERVICE_UUID); + pAdvertising->setScanResponse(true); + BLEDevice::startAdvertising(); + + Serial.println("[BLE] Advertising as 'Oliver_3'"); + Serial.println("\n=== GATEWAY READY ===\n"); +} + +void loop() { + static uint32_t lastRediscover = 0; + + // Handle manual re-discovery request + if (rediscoverRequested) { + rediscoverRequested = false; + Serial.println("\n[CMD] Manual re-discovery triggered!"); + for (int i = 0; i < NUM_SLAVES; i++) { + if (!slaveFound[i]) { + Serial.printf("[REDISCOVER] Will search for S%d\n", i); + } + } + discoverSlaves(15000); + } + + // Automatic re-discovery every 10 seconds for missing slaves + if (millis() - lastRediscover > 10000) { + lastRediscover = millis(); + int missingCount = 0; + for (int i = 0; i < NUM_SLAVES; i++) { + if (!slaveFound[i]) + missingCount++; + } + + if (missingCount > 0) { + Serial.printf("[AUTO-REDISCOVER] Searching for %d missing slaves...\n", + missingCount); + uint8_t broadcastAddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + for (int attempt = 0; attempt < 5; attempt++) { + uint8_t ping = 0xFF; + esp_now_send(broadcastAddr, &ping, 1); + delay(200); + } + } + } + + // ── On-demand read: triggered by BLE "READ" command from PC ── + if (readRequested) { + readRequested = false; + gatewayPacketIdx++; + + // 1. Read master's own encoder + diagnostics + double masterAngle = getUltraPrecisionReading(); + masterData.value = (int)(masterAngle * 10000.0); + masterData.packetIdx = gatewayPacketIdx; + + uint16_t diaagc = readRegister(DIAAGC_REG); + uint16_t mag = readRegister(MAG_REG); + masterData.agc = diaagc & 0xFF; + masterData.mag = mag & 0x3FFF; + masterData.magl = (diaagc >> 8) & 0x01; + masterData.magh = (diaagc >> 10) & 0x01; + masterData.cof = (diaagc >> 9) & 0x01; + + // 2. Request data from all discovered slaves + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i]) { + uint8_t req = i; + esp_now_send(slaveMACs[i], &req, 1); + } + } + + // 3. Wait for slave responses (up to 200ms timeout) + uint32_t waitStart = millis(); + while (millis() - waitStart < 200) { + bool allResponded = true; + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i] && !slaveResponded[i]) { + allResponded = false; + break; + } + } + if (allResponded) + break; + delay(1); + } + + // 4. Build BLE message (same 7-field format) + String bleMsg = String(gatewayPacketIdx); + + bleMsg += "|M0:" + String(masterData.value) + "," + + String(masterData.packetIdx) + "," + + String(masterData.agc) + "," + String(masterData.mag) + "," + + String(masterData.magl) + "," + String(masterData.magh) + "," + + String(masterData.cof); + + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i] && slaveResponded[i]) { + bleMsg += "|S" + String(i) + ":" + String(slaves[i].value) + "," + + String(slaves[i].packetIdx) + "," + + String(slaves[i].agc) + "," + String(slaves[i].mag) + "," + + String(slaves[i].magl) + "," + String(slaves[i].magh) + "," + + String(slaves[i].cof); + } else { + bleMsg += "|S" + String(i) + ":OFFLINE,0"; + } + } + + // 5. Send BLE notification + if (pcConnected && pChar) { + pChar->setValue(bleMsg.c_str()); + pChar->notify(); + } + + // 6. Serial log + Serial.printf("[READ #%u] M0=%d", gatewayPacketIdx, masterData.value); + for (int i = 0; i < NUM_SLAVES; i++) { + if (slaveFound[i] && slaveResponded[i]) + Serial.printf(" | S%d=%d", i, slaves[i].value); + else + Serial.printf(" | S%d=OFFLINE", i); + } + Serial.println(); + } + + delay(1); } \ No newline at end of file diff --git a/firmware/single_encoder/single_encoder.ino b/firmware/single_encoder/single_encoder.ino index d6a196c..f890e7b 100644 --- a/firmware/single_encoder/single_encoder.ino +++ b/firmware/single_encoder/single_encoder.ino @@ -1,249 +1,249 @@ -#include -#include -#include -#include -#include -#include -#include // For std::sort - -/** - * ============================================================ - * Single Encoder BLE Firmware Template [firmware/single_encoder/] - * Board: XIAO ESP32C3 / ESP32C6 - * Sensor: AS5047D 14-bit Magnetic Rotary Encoder (SPI) - * - * For multi-encoder (master+slave) projects, see: - * firmware/master/master.ino — ESP32C6 master - * firmware/slave/slave.ino — ESP32C3 slave - * ============================================================ - * - * CONFIGURE BEFORE FLASHING: - * Change ESP_NAME, SERVICE_UUID, and CHARACTERISTIC_UUID below. - * Everything else (SPI, sampling algorithm, BLE logic) does not need editing. - * - * Strategy: Hybrid Robust Filtering - * 1. 4096 Samples total. - * 2. Divided into 16 blocks of 256 samples each. - * 3. Each block: Robust Mean (discards samples > 1.5 LSB from mean). - * 4. Final Result: Median of the 16 block-means. - * ============================================================ - * Author: Swaraj Dangare - */ - -// ============================================================ -// CONFIGURE BEFORE FLASHING -// ============================================================ -#define ESP_NAME "YOUR_DEVICE_NAME" -#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b" -#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8" -// ============================================================ - -// ---------------- AS5047D SETTINGS ---------------- -#define ANGLECOM 0x3FFF -#define DIAAGC 0x3FFC // Diagnostic and AGC register -#define MAG 0x3FFD // CORDIC magnitude register -#define RD 0x40 -#define NUM_BLOCKS 16 -#define SAMPLES_PER_BLOCK 256 -#define TOTAL_SAMPLES (NUM_BLOCKS * SAMPLES_PER_BLOCK) // 4096 - -// SPI Pins (XIAO ESP32C3/C6) -// SCK=D1, MISO=D0, MOSI=D10, CS=D7 -const int PIN_CS = D7; -const int PIN_SCK = D1; -const int PIN_MISO = D0; -const int PIN_MOSI = D10; - -SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); - -// ---------------- BLE STATE ---------------- -BLECharacteristic *pChar; -bool deviceConnected = false; -bool readRequested = false; -bool zeroRequest = false; -double zeroOffset = 0.0; -uint32_t packetIdx = 0; // Global packet counter - -// ---------------- BLE CALLBACKS ---------------- -class MyServerCallbacks : public BLEServerCallbacks { - void onConnect(BLEServer *pServer) { - deviceConnected = true; - Serial.println("[BLE] Connected"); - } - void onDisconnect(BLEServer *pServer) { - deviceConnected = false; - Serial.println("[BLE] Disconnected"); - BLEDevice::getAdvertising()->start(); - } -}; - -class WriteCallback : public BLECharacteristicCallbacks { - void onWrite(BLECharacteristic *pChar) { - String value = pChar->getValue().c_str(); - if (value == "READ") { - readRequested = true; - } else if (value == "ZERO") { - zeroRequest = true; - Serial.println("[CMD] Zero reset requested"); - } - } -}; - -// ---------------- UTILS ---------------- -uint16_t evenParityBit(uint16_t x) { - x &= 0x7FFF; - return __builtin_parity(x); -} - -uint16_t makeReadCmd(uint16_t addr) { - uint16_t cmd = (1 << 14) | (addr & 0x3FFF); - cmd |= (evenParityBit(cmd) << 15); - return cmd; -} - -uint16_t AS5047D_Read() { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(PIN_CS, LOW); - SPI.transfer16(makeReadCmd(ANGLECOM)); - digitalWrite(PIN_CS, HIGH); - delayMicroseconds(1); - digitalWrite(PIN_CS, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(PIN_CS, HIGH); - SPI.endTransaction(); - return result; -} - -uint16_t AS5047D_ReadRegister(uint16_t address) { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(PIN_CS, LOW); - SPI.transfer16(makeReadCmd(address)); - digitalWrite(PIN_CS, HIGH); - delayMicroseconds(1); - digitalWrite(PIN_CS, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(PIN_CS, HIGH); - SPI.endTransaction(); - return result; -} - -// ---------------- STATISTICS ---------------- -double getRobustMean(uint16_t *samples, int size) { - double sum = 0; - for (int i = 0; i < size; i++) - sum += (samples[i] & 0x3FFF); - double initialMean = sum / size; - - double robustSum = 0; - int count = 0; - for (int i = 0; i < size; i++) { - uint16_t val = samples[i] & 0x3FFF; - if (abs((double)val - initialMean) < 1.5) { - robustSum += val; - count++; - } - } - return (count > 0) ? (robustSum / count) : initialMean; -} - -double getMedian(double *values, int size) { - std::sort(values, values + size); - return values[size / 2]; -} - -// ---------------- SETUP ---------------- -void setup() { - Serial.begin(115200); - pinMode(PIN_CS, OUTPUT); - digitalWrite(PIN_CS, HIGH); - SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); - - BLEDevice::init(ESP_NAME); - BLEServer *pServer = BLEDevice::createServer(); - pServer->setCallbacks(new MyServerCallbacks()); - BLEService *pService = pServer->createService(SERVICE_UUID); - pChar = pService->createCharacteristic(CHARACTERISTIC_UUID, - BLECharacteristic::PROPERTY_NOTIFY | - BLECharacteristic::PROPERTY_WRITE); - pChar->addDescriptor(new BLE2902()); - pChar->setCallbacks(new WriteCallback()); - pService->start(); - BLEDevice::getAdvertising()->start(); - Serial.println("[BLE] Advertising as '" ESP_NAME "' — awaiting READ command"); -} - -// ---------------- MAIN LOOP ---------------- -void loop() { - // On-demand read: only fires when PC writes "READ" over BLE - if (readRequested) { - readRequested = false; - packetIdx++; - - Serial.printf("[READ #%u] Sampling...\n", packetIdx); - double blockMeans[NUM_BLOCKS]; - uint16_t blockSamples[SAMPLES_PER_BLOCK]; - - for (int b = 0; b < NUM_BLOCKS; b++) { - for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { - uint16_t raw = AS5047D_Read(); - if (((raw >> 15) & 1) == evenParityBit(raw)) { - blockSamples[s] = raw; - } else { - s--; // Retry on parity error - } - } - blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); - delayMicroseconds(50); // Brief yield to keep BLE alive - } - - double finalCounts = getMedian(blockMeans, NUM_BLOCKS); - double rawAngle = (finalCounts * 360.0) / 16384.0; - - // Handle zero reset (applied on next READ after ZERO command) - if (zeroRequest) { - zeroOffset = rawAngle; - zeroRequest = false; - Serial.printf("[ZERO] Offset set to %.5f\n", zeroOffset); - } - - // Apply zero offset and normalize to [0, 360) - double angle = rawAngle - zeroOffset; - if (angle < 0) angle += 360.0; - if (angle >= 360.0) angle -= 360.0; - - // Angle as integer x 10000 (matches Oliver master format) - int angleInt = (int)(angle * 10000.0); - - // Read diagnostics - uint16_t diaagc = AS5047D_ReadRegister(DIAAGC); - uint16_t mag = AS5047D_ReadRegister(MAG); - uint8_t agc = diaagc & 0xFF; - uint8_t cof = (diaagc >> 9) & 1; - uint8_t magl = (diaagc >> 10) & 1; - uint8_t magh = (diaagc >> 11) & 1; - uint16_t magnitude = mag & 0x3FFF; - - // BLE message: same 7-field format as Oliver master, single encoder only - // |M0:,,,,,, - String bleMsg = String(packetIdx); - bleMsg += "|M0:" + String(angleInt) + "," - + String(packetIdx) + "," - + String(agc) + "," - + String(magnitude) + "," - + String(magl) + "," - + String(magh) + "," - + String(cof); - - if (deviceConnected && pChar) { - pChar->setValue(bleMsg.c_str()); - pChar->notify(); - } - - Serial.printf("[READ #%u] M0=%d (%.5f deg) | AGC:%u MAG:%u MAGL:%u MAGH:%u COF:%u\n", - packetIdx, angleInt, angle, agc, magnitude, magl, magh, cof); - } - - delay(1); -} +#include +#include +#include +#include +#include +#include +#include // For std::sort + +/** + * ============================================================ + * Single Encoder BLE Firmware Template [firmware/single_encoder/] + * Board: XIAO ESP32C3 / ESP32C6 + * Sensor: AS5047D 14-bit Magnetic Rotary Encoder (SPI) + * + * For multi-encoder (master+slave) projects, see: + * firmware/master/master.ino — ESP32C6 master + * firmware/slave/slave.ino — ESP32C3 slave + * ============================================================ + * + * CONFIGURE BEFORE FLASHING: + * Change ESP_NAME, SERVICE_UUID, and CHARACTERISTIC_UUID below. + * Everything else (SPI, sampling algorithm, BLE logic) does not need editing. + * + * Strategy: Hybrid Robust Filtering + * 1. 4096 Samples total. + * 2. Divided into 16 blocks of 256 samples each. + * 3. Each block: Robust Mean (discards samples > 1.5 LSB from mean). + * 4. Final Result: Median of the 16 block-means. + * ============================================================ + * Author: Swaraj Dangare + */ + +// ============================================================ +// CONFIGURE BEFORE FLASHING +// ============================================================ +#define ESP_NAME "YOUR_DEVICE_NAME" +#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b" +#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8" +// ============================================================ + +// ---------------- AS5047D SETTINGS ---------------- +#define ANGLECOM 0x3FFF +#define DIAAGC 0x3FFC // Diagnostic and AGC register +#define MAG 0x3FFD // CORDIC magnitude register +#define RD 0x40 +#define NUM_BLOCKS 16 +#define SAMPLES_PER_BLOCK 256 +#define TOTAL_SAMPLES (NUM_BLOCKS * SAMPLES_PER_BLOCK) // 4096 + +// SPI Pins (XIAO ESP32C3/C6) +// SCK=D1, MISO=D0, MOSI=D10, CS=D7 +const int PIN_CS = D7; +const int PIN_SCK = D1; +const int PIN_MISO = D0; +const int PIN_MOSI = D10; + +SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); + +// ---------------- BLE STATE ---------------- +BLECharacteristic *pChar; +bool deviceConnected = false; +bool readRequested = false; +bool zeroRequest = false; +double zeroOffset = 0.0; +uint32_t packetIdx = 0; // Global packet counter + +// ---------------- BLE CALLBACKS ---------------- +class MyServerCallbacks : public BLEServerCallbacks { + void onConnect(BLEServer *pServer) { + deviceConnected = true; + Serial.println("[BLE] Connected"); + } + void onDisconnect(BLEServer *pServer) { + deviceConnected = false; + Serial.println("[BLE] Disconnected"); + BLEDevice::getAdvertising()->start(); + } +}; + +class WriteCallback : public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *pChar) { + String value = pChar->getValue().c_str(); + if (value == "READ") { + readRequested = true; + } else if (value == "ZERO") { + zeroRequest = true; + Serial.println("[CMD] Zero reset requested"); + } + } +}; + +// ---------------- UTILS ---------------- +uint16_t evenParityBit(uint16_t x) { + x &= 0x7FFF; + return __builtin_parity(x); +} + +uint16_t makeReadCmd(uint16_t addr) { + uint16_t cmd = (1 << 14) | (addr & 0x3FFF); + cmd |= (evenParityBit(cmd) << 15); + return cmd; +} + +uint16_t AS5047D_Read() { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(PIN_CS, LOW); + SPI.transfer16(makeReadCmd(ANGLECOM)); + digitalWrite(PIN_CS, HIGH); + delayMicroseconds(1); + digitalWrite(PIN_CS, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(PIN_CS, HIGH); + SPI.endTransaction(); + return result; +} + +uint16_t AS5047D_ReadRegister(uint16_t address) { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(PIN_CS, LOW); + SPI.transfer16(makeReadCmd(address)); + digitalWrite(PIN_CS, HIGH); + delayMicroseconds(1); + digitalWrite(PIN_CS, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(PIN_CS, HIGH); + SPI.endTransaction(); + return result; +} + +// ---------------- STATISTICS ---------------- +double getRobustMean(uint16_t *samples, int size) { + double sum = 0; + for (int i = 0; i < size; i++) + sum += (samples[i] & 0x3FFF); + double initialMean = sum / size; + + double robustSum = 0; + int count = 0; + for (int i = 0; i < size; i++) { + uint16_t val = samples[i] & 0x3FFF; + if (abs((double)val - initialMean) < 1.5) { + robustSum += val; + count++; + } + } + return (count > 0) ? (robustSum / count) : initialMean; +} + +double getMedian(double *values, int size) { + std::sort(values, values + size); + return values[size / 2]; +} + +// ---------------- SETUP ---------------- +void setup() { + Serial.begin(115200); + pinMode(PIN_CS, OUTPUT); + digitalWrite(PIN_CS, HIGH); + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); + + BLEDevice::init(ESP_NAME); + BLEServer *pServer = BLEDevice::createServer(); + pServer->setCallbacks(new MyServerCallbacks()); + BLEService *pService = pServer->createService(SERVICE_UUID); + pChar = pService->createCharacteristic(CHARACTERISTIC_UUID, + BLECharacteristic::PROPERTY_NOTIFY | + BLECharacteristic::PROPERTY_WRITE); + pChar->addDescriptor(new BLE2902()); + pChar->setCallbacks(new WriteCallback()); + pService->start(); + BLEDevice::getAdvertising()->start(); + Serial.println("[BLE] Advertising as '" ESP_NAME "' — awaiting READ command"); +} + +// ---------------- MAIN LOOP ---------------- +void loop() { + // On-demand read: only fires when PC writes "READ" over BLE + if (readRequested) { + readRequested = false; + packetIdx++; + + Serial.printf("[READ #%u] Sampling...\n", packetIdx); + double blockMeans[NUM_BLOCKS]; + uint16_t blockSamples[SAMPLES_PER_BLOCK]; + + for (int b = 0; b < NUM_BLOCKS; b++) { + for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { + uint16_t raw = AS5047D_Read(); + if (((raw >> 15) & 1) == evenParityBit(raw)) { + blockSamples[s] = raw; + } else { + s--; // Retry on parity error + } + } + blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); + delayMicroseconds(50); // Brief yield to keep BLE alive + } + + double finalCounts = getMedian(blockMeans, NUM_BLOCKS); + double rawAngle = (finalCounts * 360.0) / 16384.0; + + // Handle zero reset (applied on next READ after ZERO command) + if (zeroRequest) { + zeroOffset = rawAngle; + zeroRequest = false; + Serial.printf("[ZERO] Offset set to %.5f\n", zeroOffset); + } + + // Apply zero offset and normalize to [0, 360) + double angle = rawAngle - zeroOffset; + if (angle < 0) angle += 360.0; + if (angle >= 360.0) angle -= 360.0; + + // Angle as integer x 10000 (matches Oliver master format) + int angleInt = (int)(angle * 10000.0); + + // Read diagnostics + uint16_t diaagc = AS5047D_ReadRegister(DIAAGC); + uint16_t mag = AS5047D_ReadRegister(MAG); + uint8_t agc = diaagc & 0xFF; + uint8_t cof = (diaagc >> 9) & 1; + uint8_t magl = (diaagc >> 10) & 1; + uint8_t magh = (diaagc >> 11) & 1; + uint16_t magnitude = mag & 0x3FFF; + + // BLE message: same 7-field format as Oliver master, single encoder only + // |M0:,,,,,, + String bleMsg = String(packetIdx); + bleMsg += "|M0:" + String(angleInt) + "," + + String(packetIdx) + "," + + String(agc) + "," + + String(magnitude) + "," + + String(magl) + "," + + String(magh) + "," + + String(cof); + + if (deviceConnected && pChar) { + pChar->setValue(bleMsg.c_str()); + pChar->notify(); + } + + Serial.printf("[READ #%u] M0=%d (%.5f deg) | AGC:%u MAG:%u MAGL:%u MAGH:%u COF:%u\n", + packetIdx, angleInt, angle, agc, magnitude, magl, magh, cof); + } + + delay(1); +} diff --git a/firmware/slave/slave.ino b/firmware/slave/slave.ino index ca5f5c0..894f8ea 100644 --- a/firmware/slave/slave.ino +++ b/firmware/slave/slave.ino @@ -1,207 +1,207 @@ -#include -#include -#include -#include -#include - -/** - * Project: Wireless Ultra-Precision Slave (On-Demand) - * Protocol: ESP-NOW (Request -> Response) - * Sensor: AS5047D (SPI) - * Logic: Wait for Request -> Measure (~15ms) -> Reply - * Author: Swaraj Dangare - */ - -#define SLAVE_ID 0 //<--- CHANGE THIS FOR EACH BOARD: 0, 1, 2, 3, 4 -#define WIFI_CHANNEL 11 // Must match master's WIFI_CHANNEL (use 1, 6, or 11) - -// ---------------- COMMUNICATION ---------------- -typedef struct __attribute__((packed)) { - uint8_t id; - int value; // Angle * 10000 - uint32_t packetIdx; - uint8_t agc; // AS5047D AGC value (0-255) - uint16_t mag; // AS5047D CORDIC magnitude (14-bit) - uint8_t magl; // Magnetic field too low (0 or 1) - uint8_t magh; // Magnetic field too high (0 or 1) - uint8_t cof; // CORDIC overflow (0 or 1) -} Payload; - -Payload pkt; -uint32_t packet_counter = 0; - -// ---------------- AS5047D SETTINGS ---------------- -#define ANGLECOM 0x3FFF -#define DIAAGC_REG 0x3FFC -#define MAG_REG 0x3FFD -#define RD 0x40 -#define NUM_BLOCKS 16 -#define SAMPLES_PER_BLOCK 256 -#define TOTAL_SAMPLES (NUM_BLOCKS * SAMPLES_PER_BLOCK) // 4096 - -// XIAO ESP32C3 SPI Pins (Standard) -const int PIN_CS = D7; -const int PIN_SCK = D1; -const int PIN_MISO = D0; -const int PIN_MOSI = D10; - -SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); - -// ---------------- UTILS & ENCODER ---------------- -uint16_t evenParityBit(uint16_t x) { - x &= 0x7FFF; - return __builtin_parity(x); -} - -uint16_t makeReadCmd(uint16_t addr) { - uint16_t cmd = (1 << 14) | (addr & 0x3FFF); - cmd |= (evenParityBit(cmd) << 15); - return cmd; -} - -uint16_t AS5047D_Read() { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(PIN_CS, LOW); - SPI.transfer16(makeReadCmd(ANGLECOM)); - digitalWrite(PIN_CS, HIGH); - delayMicroseconds(1); - digitalWrite(PIN_CS, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(PIN_CS, HIGH); - SPI.endTransaction(); - return result; -} - -uint16_t readRegister(uint16_t addr) { - uint16_t result; - SPI.beginTransaction(spiSettings); - digitalWrite(PIN_CS, LOW); - SPI.transfer16(makeReadCmd(addr)); - digitalWrite(PIN_CS, HIGH); - delayMicroseconds(1); - digitalWrite(PIN_CS, LOW); - result = SPI.transfer16(0x0000); - digitalWrite(PIN_CS, HIGH); - SPI.endTransaction(); - return result & 0x3FFF; -} - -// Robust Mean: Calculate mean of samples within 1.5 LSB distance from initial -// median -double getRobustMean(uint16_t *samples, int size) { - double sum = 0; - for (int i = 0; i < size; i++) - sum += (samples[i] & 0x3FFF); - double initialMean = sum / size; - - double robustSum = 0; - int count = 0; - for (int i = 0; i < size; i++) { - uint16_t val = samples[i] & 0x3FFF; - if (abs((double)val - initialMean) < 1.5) { - robustSum += val; - count++; - } - } - return (count > 0) ? (robustSum / count) : initialMean; -} - -double getMedian(double *values, int size) { - std::sort(values, values + size); - return values[size / 2]; -} - -double getUltraPrecisionReading() { - double blockMeans[NUM_BLOCKS]; - uint16_t blockSamples[SAMPLES_PER_BLOCK]; - - for (int b = 0; b < NUM_BLOCKS; b++) { - for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { - uint16_t raw = AS5047D_Read(); - if (((raw >> 15) & 1) == evenParityBit(raw)) { - blockSamples[s] = raw; - } else { - s--; - } - } - blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); - delayMicroseconds(50); // Small yield - } - double finalCounts = getMedian(blockMeans, NUM_BLOCKS); - return (finalCounts * 360.0) / 16384.0; -} - -// ---------------- ESP-NOW CALLBACK ---------------- -void onDataRecv(const esp_now_recv_info_t *info, const uint8_t *data, int len) { - // 1. Discovery Response (Master sends 0xFF) - if (len == 1 && data[0] == 0xFF) { - // Add small random delay to prevent collision when multiple slaves respond - // This staggers responses across 10-30ms window - delayMicroseconds(random(10000, 30000)); // 10-30ms - - uint8_t reply = SLAVE_ID; - // Add master as peer dynamically so we can reply - if (!esp_now_is_peer_exist(info->src_addr)) { - esp_now_peer_info_t peer{}; - memcpy(peer.peer_addr, info->src_addr, 6); - peer.channel = WIFI_CHANNEL; - peer.encrypt = false; - esp_now_add_peer(&peer); - } - esp_now_send(info->src_addr, &reply, 1); - } - - // 2. Data Request (Master sends Slave ID) - if (len == 1 && data[0] == SLAVE_ID) { - // Perform Measurement (Block-Blocking but fast enough ~15-20ms) - double angle = getUltraPrecisionReading(); - - // Read diagnostic registers - uint16_t diaagc = readRegister(DIAAGC_REG); - uint16_t mag = readRegister(MAG_REG); - - pkt.id = SLAVE_ID; - pkt.value = (int)(angle * 10000.0); - pkt.packetIdx = packet_counter++; - pkt.agc = diaagc & 0xFF; - pkt.mag = mag & 0x3FFF; - pkt.magl = (diaagc >> 8) & 0x01; - pkt.magh = (diaagc >> 10) & 0x01; - pkt.cof = (diaagc >> 9) & 0x01; - - esp_now_send(info->src_addr, (uint8_t *)&pkt, sizeof(pkt)); - } -} - -void setup() { - Serial.begin(115200); - Serial.println("init"); - // SPI Setup - pinMode(PIN_CS, OUTPUT); - digitalWrite(PIN_CS, HIGH); - SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); - - // WiFi / ESP-NOW Setup - WiFi.mode(WIFI_STA); - WiFi.disconnect(); - - // Force WiFi channel (must match master) - esp_wifi_set_promiscuous(true); - esp_wifi_set_channel(WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE); - esp_wifi_set_promiscuous(false); - - if (esp_now_init() != ESP_OK) { - Serial.println("ESP-NOW Init Failed"); - return; - } - esp_now_register_recv_cb(onDataRecv); - - Serial.printf("Slave %d Ready | Channel %d | MAC: %s\n", SLAVE_ID, - WIFI_CHANNEL, WiFi.macAddress().c_str()); -} - -void loop() { - // Nothing here - totally interrupt driven - delay(100); +#include +#include +#include +#include +#include + +/** + * Project: Wireless Ultra-Precision Slave (On-Demand) + * Protocol: ESP-NOW (Request -> Response) + * Sensor: AS5047D (SPI) + * Logic: Wait for Request -> Measure (~15ms) -> Reply + * Author: Swaraj Dangare + */ + +#define SLAVE_ID 0 //<--- CHANGE THIS FOR EACH BOARD: 0, 1, 2, 3, 4 +#define WIFI_CHANNEL 11 // Must match master's WIFI_CHANNEL (use 1, 6, or 11) + +// ---------------- COMMUNICATION ---------------- +typedef struct __attribute__((packed)) { + uint8_t id; + int value; // Angle * 10000 + uint32_t packetIdx; + uint8_t agc; // AS5047D AGC value (0-255) + uint16_t mag; // AS5047D CORDIC magnitude (14-bit) + uint8_t magl; // Magnetic field too low (0 or 1) + uint8_t magh; // Magnetic field too high (0 or 1) + uint8_t cof; // CORDIC overflow (0 or 1) +} Payload; + +Payload pkt; +uint32_t packet_counter = 0; + +// ---------------- AS5047D SETTINGS ---------------- +#define ANGLECOM 0x3FFF +#define DIAAGC_REG 0x3FFC +#define MAG_REG 0x3FFD +#define RD 0x40 +#define NUM_BLOCKS 16 +#define SAMPLES_PER_BLOCK 256 +#define TOTAL_SAMPLES (NUM_BLOCKS * SAMPLES_PER_BLOCK) // 4096 + +// XIAO ESP32C3 SPI Pins (Standard) +const int PIN_CS = D7; +const int PIN_SCK = D1; +const int PIN_MISO = D0; +const int PIN_MOSI = D10; + +SPISettings spiSettings(10000000, MSBFIRST, SPI_MODE1); + +// ---------------- UTILS & ENCODER ---------------- +uint16_t evenParityBit(uint16_t x) { + x &= 0x7FFF; + return __builtin_parity(x); +} + +uint16_t makeReadCmd(uint16_t addr) { + uint16_t cmd = (1 << 14) | (addr & 0x3FFF); + cmd |= (evenParityBit(cmd) << 15); + return cmd; +} + +uint16_t AS5047D_Read() { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(PIN_CS, LOW); + SPI.transfer16(makeReadCmd(ANGLECOM)); + digitalWrite(PIN_CS, HIGH); + delayMicroseconds(1); + digitalWrite(PIN_CS, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(PIN_CS, HIGH); + SPI.endTransaction(); + return result; +} + +uint16_t readRegister(uint16_t addr) { + uint16_t result; + SPI.beginTransaction(spiSettings); + digitalWrite(PIN_CS, LOW); + SPI.transfer16(makeReadCmd(addr)); + digitalWrite(PIN_CS, HIGH); + delayMicroseconds(1); + digitalWrite(PIN_CS, LOW); + result = SPI.transfer16(0x0000); + digitalWrite(PIN_CS, HIGH); + SPI.endTransaction(); + return result & 0x3FFF; +} + +// Robust Mean: Calculate mean of samples within 1.5 LSB distance from initial +// median +double getRobustMean(uint16_t *samples, int size) { + double sum = 0; + for (int i = 0; i < size; i++) + sum += (samples[i] & 0x3FFF); + double initialMean = sum / size; + + double robustSum = 0; + int count = 0; + for (int i = 0; i < size; i++) { + uint16_t val = samples[i] & 0x3FFF; + if (abs((double)val - initialMean) < 1.5) { + robustSum += val; + count++; + } + } + return (count > 0) ? (robustSum / count) : initialMean; +} + +double getMedian(double *values, int size) { + std::sort(values, values + size); + return values[size / 2]; +} + +double getUltraPrecisionReading() { + double blockMeans[NUM_BLOCKS]; + uint16_t blockSamples[SAMPLES_PER_BLOCK]; + + for (int b = 0; b < NUM_BLOCKS; b++) { + for (int s = 0; s < SAMPLES_PER_BLOCK; s++) { + uint16_t raw = AS5047D_Read(); + if (((raw >> 15) & 1) == evenParityBit(raw)) { + blockSamples[s] = raw; + } else { + s--; + } + } + blockMeans[b] = getRobustMean(blockSamples, SAMPLES_PER_BLOCK); + delayMicroseconds(50); // Small yield + } + double finalCounts = getMedian(blockMeans, NUM_BLOCKS); + return (finalCounts * 360.0) / 16384.0; +} + +// ---------------- ESP-NOW CALLBACK ---------------- +void onDataRecv(const esp_now_recv_info_t *info, const uint8_t *data, int len) { + // 1. Discovery Response (Master sends 0xFF) + if (len == 1 && data[0] == 0xFF) { + // Add small random delay to prevent collision when multiple slaves respond + // This staggers responses across 10-30ms window + delayMicroseconds(random(10000, 30000)); // 10-30ms + + uint8_t reply = SLAVE_ID; + // Add master as peer dynamically so we can reply + if (!esp_now_is_peer_exist(info->src_addr)) { + esp_now_peer_info_t peer{}; + memcpy(peer.peer_addr, info->src_addr, 6); + peer.channel = WIFI_CHANNEL; + peer.encrypt = false; + esp_now_add_peer(&peer); + } + esp_now_send(info->src_addr, &reply, 1); + } + + // 2. Data Request (Master sends Slave ID) + if (len == 1 && data[0] == SLAVE_ID) { + // Perform Measurement (Block-Blocking but fast enough ~15-20ms) + double angle = getUltraPrecisionReading(); + + // Read diagnostic registers + uint16_t diaagc = readRegister(DIAAGC_REG); + uint16_t mag = readRegister(MAG_REG); + + pkt.id = SLAVE_ID; + pkt.value = (int)(angle * 10000.0); + pkt.packetIdx = packet_counter++; + pkt.agc = diaagc & 0xFF; + pkt.mag = mag & 0x3FFF; + pkt.magl = (diaagc >> 8) & 0x01; + pkt.magh = (diaagc >> 10) & 0x01; + pkt.cof = (diaagc >> 9) & 0x01; + + esp_now_send(info->src_addr, (uint8_t *)&pkt, sizeof(pkt)); + } +} + +void setup() { + Serial.begin(115200); + Serial.println("init"); + // SPI Setup + pinMode(PIN_CS, OUTPUT); + digitalWrite(PIN_CS, HIGH); + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); + + // WiFi / ESP-NOW Setup + WiFi.mode(WIFI_STA); + WiFi.disconnect(); + + // Force WiFi channel (must match master) + esp_wifi_set_promiscuous(true); + esp_wifi_set_channel(WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE); + esp_wifi_set_promiscuous(false); + + if (esp_now_init() != ESP_OK) { + Serial.println("ESP-NOW Init Failed"); + return; + } + esp_now_register_recv_cb(onDataRecv); + + Serial.printf("Slave %d Ready | Channel %d | MAC: %s\n", SLAVE_ID, + WIFI_CHANNEL, WiFi.macAddress().c_str()); +} + +void loop() { + // Nothing here - totally interrupt driven + delay(100); } \ No newline at end of file diff --git a/projects/block_height/BLADE_OFFSET_PROBE.py b/projects/block_height/BLADE_OFFSET_PROBE.py index 5c09682..7385e8c 100644 --- a/projects/block_height/BLADE_OFFSET_PROBE.py +++ b/projects/block_height/BLADE_OFFSET_PROBE.py @@ -1,326 +1,326 @@ -""" -Author: Swaraj Dangare -""" -import asyncio -import time -import sys -import select -import numpy as np -from collections import deque -from bleak import BleakClient, BleakScanner -import os -import csv -from datetime import datetime - -# BLE configuration -DEVICE_NAME = "AIRPROBE_MASTER_GATEWAY" -CHARACTERISTIC_UUID = "beb5483e-36e1-4688-b7f5-ea07361b26a8" -COMMAND_UUID = "8d53dc1d-1db7-4cd3-868b-8a527460aa84" - - -class GatewayInterface: - def __init__(self): - self.start_time = time.time() - self.last_g_idx = -1 - self.missed_packets = 0 - self.client = None - - # Manual sampling mode - self.manual_mode = True - self.samples_per_measurement = 5 - self.current_samples = { - 'M0': [], 'S0': [] - } - self.sample_metadata = { - 'M0': [], 'S0': [] - } - self.collecting = False - self.measurement_id = 0 - - # Legacy variance tracking (kept for reference) - self.sample_size = 100 - self.encoder_samples = { - 'M0': deque(maxlen=self.sample_size), - 'S0': deque(maxlen=self.sample_size), - } - - # ---------- CSV LOGGING ---------- - os.makedirs("logs", exist_ok=True) - ts = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") - self.csv_path = f"logs/airprobe_log_{ts}.csv" - self.csv_file = open(self.csv_path, "w", newline="") - self.csv_writer = csv.writer(self.csv_file) - - self.csv_writer.writerow([ - "timestamp", - "uptime_s", - "measurement_id", - "sample_num", - "gateway_idx", - "encoder", - "angle_deg", - "median", - "jitter", - "precision_um", - "pkt", - "agc", - "mag", - "magl", - "magh", - "cof", - "status" - ]) - - print(f"[LOG] CSV logging to {self.csv_path}") - - def variance(self, key): - vals = list(self.encoder_samples[key]) - return np.var(vals) if len(vals) > 1 else 0.0 - - def start_measurement(self): - """Initialize a new measurement session""" - self.measurement_id += 1 - self.collecting = True - for key in self.current_samples: - self.current_samples[key] = [] - self.sample_metadata[key] = [] - print("\nCollecting 5 samples", end="", flush=True) - - def calculate_precision_metrics(self, samples): - """Calculate median, jitter, and precision from samples""" - if len(samples) < 3: - return None, None, None - - median_angle = np.median(samples) - jitter = max(samples) - min(samples) - # Assuming 183mm reach (from reference script) - precision_um = 183 * (jitter * np.pi / 180.0) * 1000 - return median_angle, jitter, precision_um - - def display_results(self): - """Display measurement results with precision metrics""" - print("\n\n" + "="*130) - print(f"=== AIRPROBE PRECISION MEASUREMENT #{self.measurement_id} | Uptime {int(time.time() - self.start_time)}s ===") - print("="*130) - print( - f"{'ENC':<4} | {'MEDIAN (deg)':>12} | {'JITTER (deg)':>13} | {'PRECISION (μm)':>15} | " - f"{'AGC':<4} | {'MAG':<5} | {'STATUS':<12}" - ) - print("-" * 130) - - for enc_name in ['M0', 'S0']: - samples = self.current_samples[enc_name] - - if len(samples) == 0: - print(f"{enc_name:<4} | {'---':>12} | {'---':>13} | {'---':>15} | {'---':<4} | {'---':<5} | OFFLINE") - continue - - median, jitter, precision = self.calculate_precision_metrics(samples) - - if median is None: - print(f"{enc_name:<4} | {'INSUFFICIENT':>12} | {'DATA':>13} | {'---':>15} | {'---':<4} | {'---':<5} | ERROR") - continue - - # Get metadata from last sample - meta = self.sample_metadata[enc_name][-1] if self.sample_metadata[enc_name] else {} - agc = meta.get('agc', '---') - mag = meta.get('mag', '---') - status = meta.get('status', 'OK') - - print( - f"{enc_name:<4} | {median:12.5f} | {jitter:13.5f} | {precision:15.1f} | " - f"{agc:<4} | {mag:<5} | {status:<12}" - ) - - print("="*130) - print("\nPress Enter to measure again (Q to quit)") - - def log_measurement(self): - """Log all samples from current measurement to CSV""" - for enc_name in ['M0', 'S0']: - samples = self.current_samples[enc_name] - median, jitter, precision = self.calculate_precision_metrics(samples) - - for i, angle in enumerate(samples): - meta = self.sample_metadata[enc_name][i] if i < len(self.sample_metadata[enc_name]) else {} - - self.csv_writer.writerow([ - time.time(), - round(time.time() - self.start_time, 3), - self.measurement_id, - i + 1, - meta.get('g_idx', ''), - enc_name, - angle, - median if median is not None else '', - jitter if jitter is not None else '', - precision if precision is not None else '', - meta.get('pkt', ''), - meta.get('agc', ''), - meta.get('mag', ''), - meta.get('magl', ''), - meta.get('magh', ''), - meta.get('cof', ''), - meta.get('status', '') - ]) - - self.csv_file.flush() - - def handle_data(self, payload): - # Handle incoming BLE data - collect samples in manual mode - parts = payload.split('|') - try: - g_idx = int(parts[0]) - - if self.last_g_idx != -1 and g_idx - self.last_g_idx > 1: - self.missed_packets += g_idx - self.last_g_idx - 1 - self.last_g_idx = g_idx - - if not self.collecting: - return # Ignore data when not actively collecting - - # Progress indicator - print(".", end="", flush=True) - - # -------- MASTER -------- - _, d = parts[1].split(":") - fields = d.split(",") - - ang = float(fields[0]) / 10000.0 - pkt = int(fields[1]) - agc = int(fields[2]) - mag = int(fields[3]) - magl = fields[4] - magh = fields[5] - cof = fields[6] - - status = "MASTER" - if cof == "1": - status = "CORDIC_ERR" - elif magl == "1": - status = "FIELD_HIGH" - elif magh == "1": - status = "FIELD_LOW" - - self.current_samples['M0'].append(ang) - self.sample_metadata['M0'].append({ - 'g_idx': g_idx, 'pkt': pkt, 'agc': agc, 'mag': mag, - 'magl': magl, 'magh': magh, 'cof': cof, 'status': status - }) - - # -------- SLAVE (S0 only) -------- - name = "S0" - idx = 2 # S0 is at index 2 in the parts array - - if idx < len(parts): - _, d = parts[idx].split(":") - fields = d.split(",") - - if fields[0] != "OFFLINE": - ang = float(fields[0]) / 10000.0 - pkt = int(fields[1]) - agc = int(fields[2]) - mag = int(fields[3]) - magl = fields[4] - magh = fields[5] - cof = fields[6] - - status = "OK" - if cof == "1": - status = "CORDIC_ERR" - elif magl == "1": - status = "FIELD_HIGH" - elif magh == "1": - status = "FIELD_LOW" - - self.current_samples[name].append(ang) - self.sample_metadata[name].append({ - 'g_idx': g_idx, 'pkt': pkt, 'agc': agc, 'mag': mag, - 'magl': magl, 'magh': magh, 'cof': cof, 'status': status - }) - - # Check if we've collected enough samples - # Use M0 as reference (master is always present) - if len(self.current_samples['M0']) >= self.samples_per_measurement: - self.collecting = False - self.display_results() - self.log_measurement() - - except Exception as e: - print(f"\nParse error: {e}") - print(payload) - - async def send_cmd(self, cmd): - if self.client and self.client.is_connected: - try: - await self.client.write_gatt_char(COMMAND_UUID, cmd.encode()) - print(f"\n✓ Sent command: {cmd}\n") - except Exception: - print("\n✗ Command characteristic not available on gateway\n") - - -iface = GatewayInterface() - - -def notify(_, data): - iface.handle_data(data.decode()) - - -async def get_key(): - if sys.platform == "win32": - return None - if select.select([sys.stdin], [], [], 0)[0]: - return sys.stdin.read(1) - return None - - -async def main(): - print("Scanning for gateway...") - dev = await BleakScanner.find_device_by_filter(lambda d, a: d.name == DEVICE_NAME) - if not dev: - print("✗ Gateway not found") - return - - async with BleakClient(dev) as c: - iface.client = c - await c.start_notify(CHARACTERISTIC_UUID, notify) - print("✓ Connected to AIRPROBE_MASTER_GATEWAY\n") - - print("=" * 80) - print(" AIRPROBE PRECISION MEASUREMENT SYSTEM") - print(" Mode: Manual sampling (5 samples per encoder)") - print(" Encoders: M0 (Master) + S0 (Slave)") - print("=" * 80) - print("\nPress Enter to start a measurement (Q to quit)\n") - - try: - while c.is_connected: - k = await get_key() - if k: - if k.lower() == 'q': - print("\nDisconnecting...") - break - elif k == '\n': - # Start a new measurement - iface.start_measurement() - # Data will be collected via notify callback - # When 5 samples collected, results will auto-display - await asyncio.sleep(0.1) - except KeyboardInterrupt: - print("\n\nDisconnecting...") - - -if __name__ == "__main__": - if sys.platform != "win32": - import tty, termios - old = termios.tcgetattr(sys.stdin) - try: - tty.setcbreak(sys.stdin.fileno()) - asyncio.run(main()) - finally: - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old) - iface.csv_file.close() - else: - asyncio.run(main()) - iface.csv_file.close() - +""" +Author: Swaraj Dangare +""" +import asyncio +import time +import sys +import select +import numpy as np +from collections import deque +from bleak import BleakClient, BleakScanner +import os +import csv +from datetime import datetime + +# BLE configuration +DEVICE_NAME = "AIRPROBE_MASTER_GATEWAY" +CHARACTERISTIC_UUID = "beb5483e-36e1-4688-b7f5-ea07361b26a8" +COMMAND_UUID = "8d53dc1d-1db7-4cd3-868b-8a527460aa84" + + +class GatewayInterface: + def __init__(self): + self.start_time = time.time() + self.last_g_idx = -1 + self.missed_packets = 0 + self.client = None + + # Manual sampling mode + self.manual_mode = True + self.samples_per_measurement = 5 + self.current_samples = { + 'M0': [], 'S0': [] + } + self.sample_metadata = { + 'M0': [], 'S0': [] + } + self.collecting = False + self.measurement_id = 0 + + # Legacy variance tracking (kept for reference) + self.sample_size = 100 + self.encoder_samples = { + 'M0': deque(maxlen=self.sample_size), + 'S0': deque(maxlen=self.sample_size), + } + + # ---------- CSV LOGGING ---------- + os.makedirs("logs", exist_ok=True) + ts = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + self.csv_path = f"logs/airprobe_log_{ts}.csv" + self.csv_file = open(self.csv_path, "w", newline="") + self.csv_writer = csv.writer(self.csv_file) + + self.csv_writer.writerow([ + "timestamp", + "uptime_s", + "measurement_id", + "sample_num", + "gateway_idx", + "encoder", + "angle_deg", + "median", + "jitter", + "precision_um", + "pkt", + "agc", + "mag", + "magl", + "magh", + "cof", + "status" + ]) + + print(f"[LOG] CSV logging to {self.csv_path}") + + def variance(self, key): + vals = list(self.encoder_samples[key]) + return np.var(vals) if len(vals) > 1 else 0.0 + + def start_measurement(self): + """Initialize a new measurement session""" + self.measurement_id += 1 + self.collecting = True + for key in self.current_samples: + self.current_samples[key] = [] + self.sample_metadata[key] = [] + print("\nCollecting 5 samples", end="", flush=True) + + def calculate_precision_metrics(self, samples): + """Calculate median, jitter, and precision from samples""" + if len(samples) < 3: + return None, None, None + + median_angle = np.median(samples) + jitter = max(samples) - min(samples) + # Assuming 183mm reach (from reference script) + precision_um = 183 * (jitter * np.pi / 180.0) * 1000 + return median_angle, jitter, precision_um + + def display_results(self): + """Display measurement results with precision metrics""" + print("\n\n" + "="*130) + print(f"=== AIRPROBE PRECISION MEASUREMENT #{self.measurement_id} | Uptime {int(time.time() - self.start_time)}s ===") + print("="*130) + print( + f"{'ENC':<4} | {'MEDIAN (deg)':>12} | {'JITTER (deg)':>13} | {'PRECISION (μm)':>15} | " + f"{'AGC':<4} | {'MAG':<5} | {'STATUS':<12}" + ) + print("-" * 130) + + for enc_name in ['M0', 'S0']: + samples = self.current_samples[enc_name] + + if len(samples) == 0: + print(f"{enc_name:<4} | {'---':>12} | {'---':>13} | {'---':>15} | {'---':<4} | {'---':<5} | OFFLINE") + continue + + median, jitter, precision = self.calculate_precision_metrics(samples) + + if median is None: + print(f"{enc_name:<4} | {'INSUFFICIENT':>12} | {'DATA':>13} | {'---':>15} | {'---':<4} | {'---':<5} | ERROR") + continue + + # Get metadata from last sample + meta = self.sample_metadata[enc_name][-1] if self.sample_metadata[enc_name] else {} + agc = meta.get('agc', '---') + mag = meta.get('mag', '---') + status = meta.get('status', 'OK') + + print( + f"{enc_name:<4} | {median:12.5f} | {jitter:13.5f} | {precision:15.1f} | " + f"{agc:<4} | {mag:<5} | {status:<12}" + ) + + print("="*130) + print("\nPress Enter to measure again (Q to quit)") + + def log_measurement(self): + """Log all samples from current measurement to CSV""" + for enc_name in ['M0', 'S0']: + samples = self.current_samples[enc_name] + median, jitter, precision = self.calculate_precision_metrics(samples) + + for i, angle in enumerate(samples): + meta = self.sample_metadata[enc_name][i] if i < len(self.sample_metadata[enc_name]) else {} + + self.csv_writer.writerow([ + time.time(), + round(time.time() - self.start_time, 3), + self.measurement_id, + i + 1, + meta.get('g_idx', ''), + enc_name, + angle, + median if median is not None else '', + jitter if jitter is not None else '', + precision if precision is not None else '', + meta.get('pkt', ''), + meta.get('agc', ''), + meta.get('mag', ''), + meta.get('magl', ''), + meta.get('magh', ''), + meta.get('cof', ''), + meta.get('status', '') + ]) + + self.csv_file.flush() + + def handle_data(self, payload): + # Handle incoming BLE data - collect samples in manual mode + parts = payload.split('|') + try: + g_idx = int(parts[0]) + + if self.last_g_idx != -1 and g_idx - self.last_g_idx > 1: + self.missed_packets += g_idx - self.last_g_idx - 1 + self.last_g_idx = g_idx + + if not self.collecting: + return # Ignore data when not actively collecting + + # Progress indicator + print(".", end="", flush=True) + + # -------- MASTER -------- + _, d = parts[1].split(":") + fields = d.split(",") + + ang = float(fields[0]) / 10000.0 + pkt = int(fields[1]) + agc = int(fields[2]) + mag = int(fields[3]) + magl = fields[4] + magh = fields[5] + cof = fields[6] + + status = "MASTER" + if cof == "1": + status = "CORDIC_ERR" + elif magl == "1": + status = "FIELD_HIGH" + elif magh == "1": + status = "FIELD_LOW" + + self.current_samples['M0'].append(ang) + self.sample_metadata['M0'].append({ + 'g_idx': g_idx, 'pkt': pkt, 'agc': agc, 'mag': mag, + 'magl': magl, 'magh': magh, 'cof': cof, 'status': status + }) + + # -------- SLAVE (S0 only) -------- + name = "S0" + idx = 2 # S0 is at index 2 in the parts array + + if idx < len(parts): + _, d = parts[idx].split(":") + fields = d.split(",") + + if fields[0] != "OFFLINE": + ang = float(fields[0]) / 10000.0 + pkt = int(fields[1]) + agc = int(fields[2]) + mag = int(fields[3]) + magl = fields[4] + magh = fields[5] + cof = fields[6] + + status = "OK" + if cof == "1": + status = "CORDIC_ERR" + elif magl == "1": + status = "FIELD_HIGH" + elif magh == "1": + status = "FIELD_LOW" + + self.current_samples[name].append(ang) + self.sample_metadata[name].append({ + 'g_idx': g_idx, 'pkt': pkt, 'agc': agc, 'mag': mag, + 'magl': magl, 'magh': magh, 'cof': cof, 'status': status + }) + + # Check if we've collected enough samples + # Use M0 as reference (master is always present) + if len(self.current_samples['M0']) >= self.samples_per_measurement: + self.collecting = False + self.display_results() + self.log_measurement() + + except Exception as e: + print(f"\nParse error: {e}") + print(payload) + + async def send_cmd(self, cmd): + if self.client and self.client.is_connected: + try: + await self.client.write_gatt_char(COMMAND_UUID, cmd.encode()) + print(f"\n✓ Sent command: {cmd}\n") + except Exception: + print("\n✗ Command characteristic not available on gateway\n") + + +iface = GatewayInterface() + + +def notify(_, data): + iface.handle_data(data.decode()) + + +async def get_key(): + if sys.platform == "win32": + return None + if select.select([sys.stdin], [], [], 0)[0]: + return sys.stdin.read(1) + return None + + +async def main(): + print("Scanning for gateway...") + dev = await BleakScanner.find_device_by_filter(lambda d, a: d.name == DEVICE_NAME) + if not dev: + print("✗ Gateway not found") + return + + async with BleakClient(dev) as c: + iface.client = c + await c.start_notify(CHARACTERISTIC_UUID, notify) + print("✓ Connected to AIRPROBE_MASTER_GATEWAY\n") + + print("=" * 80) + print(" AIRPROBE PRECISION MEASUREMENT SYSTEM") + print(" Mode: Manual sampling (5 samples per encoder)") + print(" Encoders: M0 (Master) + S0 (Slave)") + print("=" * 80) + print("\nPress Enter to start a measurement (Q to quit)\n") + + try: + while c.is_connected: + k = await get_key() + if k: + if k.lower() == 'q': + print("\nDisconnecting...") + break + elif k == '\n': + # Start a new measurement + iface.start_measurement() + # Data will be collected via notify callback + # When 5 samples collected, results will auto-display + await asyncio.sleep(0.1) + except KeyboardInterrupt: + print("\n\nDisconnecting...") + + +if __name__ == "__main__": + if sys.platform != "win32": + import tty, termios + old = termios.tcgetattr(sys.stdin) + try: + tty.setcbreak(sys.stdin.fileno()) + asyncio.run(main()) + finally: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old) + iface.csv_file.close() + else: + asyncio.run(main()) + iface.csv_file.close() + diff --git a/projects/block_height/README.md b/projects/block_height/README.md index 43d129a..6352157 100644 --- a/projects/block_height/README.md +++ b/projects/block_height/README.md @@ -1,184 +1,184 @@ -# Block Height Measurement System - -## Overview -Measure vertical height of blocks using a 40mm link attached to an encoder shaft. The system calculates height from rotation angle using trigonometry. - -## Files -- `block_height_encoder.ino` - ESP32 firmware (same as ultra precision encoder) -- `block_height_receiver.py` - Python receiver with height calculation - -## Setup -1. Attach 40mm link perpendicular to encoder shaft -2. Upload `block_height_encoder.ino` to ESP32C6 -3. Install dependencies: `pip install bleak numpy scipy` -4. Run `python block_height_receiver.py` - -## Usage - -### Measuring Block Height -1. **Zero at ground**: Touch link end to ground, press `Z` -2. **Measure block**: Touch link end to block top, press `Enter` -3. **Read height**: System displays vertical distance in mm - -### Commands -- `Enter` - Measure angle and calculate height -- `Z` - Zero encoder at current position (ground reference) -- `D` - Toggle diagnostic data display (ON/OFF) -- `L` - Set link length (default: 40mm) -- `C` - **Calibrate starting angle** (new!) -- `Ctrl+C` - Quit - -## Calibration Mode (NEW!) - -The `starting_angle` parameter accounts for the initial angle of the link when the encoder is at zero. Use calibration mode to determine the optimal value. - -### How to Calibrate - -1. Press `C` to enter calibration mode -2. For each of 5 measurements: - - Position link at a known height - - Press Enter to measure angle - - Input the actual height in mm -3. System calculates optimal `starting_angle` using least-squares optimization -4. Review the verification errors -5. Apply or discard the calibration - -### Example Calibration Session - -``` ->> Command [Enter/Z/D/L/C]: c - -============================================================ - STARTING ANGLE CALIBRATION MODE -============================================================ - -Instructions: -1. Position the link at a known height -2. Press Enter to measure the angle -3. Enter the actual height in mm -4. Repeat for 5 different heights - -[1/5] Press Enter to measure... - Measured angle: 0.15234° - Enter actual height (mm): 5.0 - ✓ Point 1 recorded - -[2/5] Press Enter to measure... - Measured angle: 2.45123° - Enter actual height (mm): 10.0 - ✓ Point 2 recorded - -... (3 more measurements) - -Calculating optimal starting angle... - -============================================================ - CALIBRATION RESULTS -============================================================ - -Old starting angle: 59.200° -New starting angle: 58.743° - -Verification: - Point 1: Actual=5.00mm, Predicted=5.02mm, Error=+0.02mm - Point 2: Actual=10.00mm, Predicted=9.98mm, Error=-0.02mm - Point 3: Actual=15.00mm, Predicted=15.01mm, Error=+0.01mm - Point 4: Actual=20.00mm, Predicted=19.99mm, Error=-0.01mm - Point 5: Actual=25.00mm, Predicted=25.00mm, Error=+0.00mm - -Apply this calibration? (y/n): y -✓ Starting angle updated to 58.743° -Note: This value is not saved permanently. Update it in the code if needed. -``` - -## Mathematics - -### Height Calculation Formula -``` -height = (L × cos(θ₀)) - (L × cos(θ₀ + θ)) - -Where: - L = link length (40mm) - θ₀ = starting_angle (calibrated parameter) - θ = measured angle from encoder -``` - -### Calibration Algorithm - -The calibration uses **least-squares optimization** to find the `starting_angle` that minimizes prediction errors: - -1. Collect N measurements: (θᵢ, hᵢ) where hᵢ is actual height -2. Define error function: - ``` - E(θ₀) = Σ [h_predicted(θ₀, θᵢ) - hᵢ]² - ``` -3. Minimize E(θ₀) using scipy.optimize.minimize (Nelder-Mead method) -4. Return optimal θ₀ - -**Why this works:** The starting angle affects all measurements systematically. By measuring multiple known heights, we can back-calculate the angle that makes all predictions match reality. - -## Features - -### Color-Coded Output -- **Cyan (Bold)** - Angle measurements -- **Green (Bold)** - Height measurements -- **Yellow** - Diagnostic data -- **Magenta** - Info messages -- **Red** - Critical warnings -- **Blue** - Calibration mode - -### Diagnostic Toggle -- Press 'D' to show/hide diagnostic data -- Keeps display clean during measurements -- Still monitors for warnings (MAGL, MAGH, COF) - -### Angle Unwrapping -- Automatically handles 0°/360° boundary crossings -- Prevents false high jitter readings near zero -- Ensures accurate statistics - -### High Precision -- 4096-sample averaging per measurement -- Median of 5 measurements -- Sub-micron angle precision -- Height precision depends on link length and angle - -## Output Example - -``` ------------------------------------------------------------- - ANGLE : 15.23400 degrees - VERTICAL HEIGHT: 10.523 mm ------------------------------------------------------------- - Read SUCCESS -``` - -With diagnostics enabled (press 'D'): -``` ------------------------------------------------------------- - ANGLE : 15.23400 degrees - VERTICAL HEIGHT: 10.523 mm - - DIAGNOSTIC DATA: - RAW JITTER : 0.00120 deg (0.8 microns) - ALL READINGS : [15.23222, 15.23252, 15.23459, 15.23559, 15.23407] - AGC (Gain) : 70 - MAG (Magn.) : 4640 - MAGL (High) : 0 - MAGH (Low) : 0 - COF (Ovflow) : 0 ------------------------------------------------------------- - Read SUCCESS -``` - -## Notes -- Firmware includes angle inversion (360° - angle) for correct orientation -- Zero offset stored in RAM (resets on ESP32 power cycle) -- **Starting angle calibration not saved permanently** - update `self.starting_angle` in code after calibration -- Link must be perpendicular to shaft for accurate measurements -- AGC value of ~128 indicates optimal magnetic field strength -- For best calibration results, use heights spanning your measurement range - ---- - -**Author:** Swaraj Dangare +# Block Height Measurement System + +## Overview +Measure vertical height of blocks using a 40mm link attached to an encoder shaft. The system calculates height from rotation angle using trigonometry. + +## Files +- `block_height_encoder.ino` - ESP32 firmware (same as ultra precision encoder) +- `block_height_receiver.py` - Python receiver with height calculation + +## Setup +1. Attach 40mm link perpendicular to encoder shaft +2. Upload `block_height_encoder.ino` to ESP32C6 +3. Install dependencies: `pip install bleak numpy scipy` +4. Run `python block_height_receiver.py` + +## Usage + +### Measuring Block Height +1. **Zero at ground**: Touch link end to ground, press `Z` +2. **Measure block**: Touch link end to block top, press `Enter` +3. **Read height**: System displays vertical distance in mm + +### Commands +- `Enter` - Measure angle and calculate height +- `Z` - Zero encoder at current position (ground reference) +- `D` - Toggle diagnostic data display (ON/OFF) +- `L` - Set link length (default: 40mm) +- `C` - **Calibrate starting angle** (new!) +- `Ctrl+C` - Quit + +## Calibration Mode (NEW!) + +The `starting_angle` parameter accounts for the initial angle of the link when the encoder is at zero. Use calibration mode to determine the optimal value. + +### How to Calibrate + +1. Press `C` to enter calibration mode +2. For each of 5 measurements: + - Position link at a known height + - Press Enter to measure angle + - Input the actual height in mm +3. System calculates optimal `starting_angle` using least-squares optimization +4. Review the verification errors +5. Apply or discard the calibration + +### Example Calibration Session + +``` +>> Command [Enter/Z/D/L/C]: c + +============================================================ + STARTING ANGLE CALIBRATION MODE +============================================================ + +Instructions: +1. Position the link at a known height +2. Press Enter to measure the angle +3. Enter the actual height in mm +4. Repeat for 5 different heights + +[1/5] Press Enter to measure... + Measured angle: 0.15234° + Enter actual height (mm): 5.0 + ✓ Point 1 recorded + +[2/5] Press Enter to measure... + Measured angle: 2.45123° + Enter actual height (mm): 10.0 + ✓ Point 2 recorded + +... (3 more measurements) + +Calculating optimal starting angle... + +============================================================ + CALIBRATION RESULTS +============================================================ + +Old starting angle: 59.200° +New starting angle: 58.743° + +Verification: + Point 1: Actual=5.00mm, Predicted=5.02mm, Error=+0.02mm + Point 2: Actual=10.00mm, Predicted=9.98mm, Error=-0.02mm + Point 3: Actual=15.00mm, Predicted=15.01mm, Error=+0.01mm + Point 4: Actual=20.00mm, Predicted=19.99mm, Error=-0.01mm + Point 5: Actual=25.00mm, Predicted=25.00mm, Error=+0.00mm + +Apply this calibration? (y/n): y +✓ Starting angle updated to 58.743° +Note: This value is not saved permanently. Update it in the code if needed. +``` + +## Mathematics + +### Height Calculation Formula +``` +height = (L × cos(θ₀)) - (L × cos(θ₀ + θ)) + +Where: + L = link length (40mm) + θ₀ = starting_angle (calibrated parameter) + θ = measured angle from encoder +``` + +### Calibration Algorithm + +The calibration uses **least-squares optimization** to find the `starting_angle` that minimizes prediction errors: + +1. Collect N measurements: (θᵢ, hᵢ) where hᵢ is actual height +2. Define error function: + ``` + E(θ₀) = Σ [h_predicted(θ₀, θᵢ) - hᵢ]² + ``` +3. Minimize E(θ₀) using scipy.optimize.minimize (Nelder-Mead method) +4. Return optimal θ₀ + +**Why this works:** The starting angle affects all measurements systematically. By measuring multiple known heights, we can back-calculate the angle that makes all predictions match reality. + +## Features + +### Color-Coded Output +- **Cyan (Bold)** - Angle measurements +- **Green (Bold)** - Height measurements +- **Yellow** - Diagnostic data +- **Magenta** - Info messages +- **Red** - Critical warnings +- **Blue** - Calibration mode + +### Diagnostic Toggle +- Press 'D' to show/hide diagnostic data +- Keeps display clean during measurements +- Still monitors for warnings (MAGL, MAGH, COF) + +### Angle Unwrapping +- Automatically handles 0°/360° boundary crossings +- Prevents false high jitter readings near zero +- Ensures accurate statistics + +### High Precision +- 4096-sample averaging per measurement +- Median of 5 measurements +- Sub-micron angle precision +- Height precision depends on link length and angle + +## Output Example + +``` +------------------------------------------------------------ + ANGLE : 15.23400 degrees + VERTICAL HEIGHT: 10.523 mm +------------------------------------------------------------ + Read SUCCESS +``` + +With diagnostics enabled (press 'D'): +``` +------------------------------------------------------------ + ANGLE : 15.23400 degrees + VERTICAL HEIGHT: 10.523 mm + + DIAGNOSTIC DATA: + RAW JITTER : 0.00120 deg (0.8 microns) + ALL READINGS : [15.23222, 15.23252, 15.23459, 15.23559, 15.23407] + AGC (Gain) : 70 + MAG (Magn.) : 4640 + MAGL (High) : 0 + MAGH (Low) : 0 + COF (Ovflow) : 0 +------------------------------------------------------------ + Read SUCCESS +``` + +## Notes +- Firmware includes angle inversion (360° - angle) for correct orientation +- Zero offset stored in RAM (resets on ESP32 power cycle) +- **Starting angle calibration not saved permanently** - update `self.starting_angle` in code after calibration +- Link must be perpendicular to shaft for accurate measurements +- AGC value of ~128 indicates optimal magnetic field strength +- For best calibration results, use heights spanning your measurement range + +--- + +**Author:** Swaraj Dangare diff --git a/projects/block_height/README_polynomial.md b/projects/block_height/README_polynomial.md index e3e59a3..14228f4 100644 --- a/projects/block_height/README_polynomial.md +++ b/projects/block_height/README_polynomial.md @@ -1,134 +1,134 @@ -# Polynomial Curve Fitting Version - -## Overview -This is a **simplified version** that uses polynomial regression to directly map angle → height, **without any trigonometry or starting_angle parameter**. - -## Key Differences from Trigonometric Version - -| Feature | Trigonometric Version | Polynomial Version | -|---------|----------------------|-------------------| -| **Model** | `height = L×cos(θ₀) - L×cos(θ₀+θ)` | `height = a₀ + a₁×θ + a₂×θ² + ...` | -| **Parameters** | `starting_angle`, `link_length` | Polynomial coefficients | -| **Calibration** | Optimizes `starting_angle` | Fits polynomial curve | -| **Complexity** | Physics-based (requires understanding) | Pure data fitting (black box) | -| **Extrapolation** | Good (follows physics) | Poor (can diverge outside range) | - -## Usage - -### 1. Run the Script -```bash -python3 block_height_receiver_polynomial.py -``` - -### 2. Calibrate First (REQUIRED) -Press `C` to enter calibration mode: -- Choose polynomial degree (1-5, default=2) - - **1 = Linear**: Simple, but may not fit well - - **2 = Quadratic**: Good balance (recommended) - - **3 = Cubic**: More flexible - - **4-5 = Higher order**: Risk of overfitting -- Measure at least `degree + 2` points (e.g., 4 points for quadratic) -- More points = better fit -- Type `done` when finished - -### 3. Measure Heights -After calibration, press `Enter` to measure. - -## Commands -- `Enter` - Measure angle and calculate height -- `Z` - Zero encoder at current position -- `D` - Toggle diagnostic display -- `C` - Calibrate polynomial curve -- `Ctrl+C` - Quit - -## Example Calibration - -``` ->> Command [Enter/Z/D/C]: c - -============================================================ - POLYNOMIAL CURVE FITTING CALIBRATION -============================================================ - -Polynomial degree (1=linear, 2=quadratic, 3=cubic) [default=2]: 2 - -Using 2-degree polynomial - -[Point 1] Press Enter to measure... - Measured angle: 0.00000° - Enter actual height (mm): 0.0 - ✓ Point 1 recorded - -[Point 2] Press Enter to measure... - Measured angle: 4.98470° - Enter actual height (mm): 3.0 - ✓ Point 2 recorded - -... (collect more points) - -[Point 10] Press Enter to measure... - Measured angle: 22.52680° - Enter actual height (mm): 14.455 - ✓ Point 10 recorded - -[Point 11] Press Enter to measure (or type 'done' to finish)...done - -Fitting 2-degree polynomial... - -============================================================ - CALIBRATION RESULTS -============================================================ - -Polynomial degree: 2 -Number of points: 10 -R² (fit quality): 0.999876 (1.0 = perfect) -RMS Error: 0.0523 mm - -Polynomial coefficients: - a2: -1.234567e-03 - a1: 6.543210e-01 - a0: 1.234567e-02 - -height = 0.0123 + 0.6543*angle - 0.0012*angle^2 - -Verification: -Point Angle Actual Predicted Error ------------------------------------------------------------- -1 0.00000 0.000 0.012 +0.012 -2 4.98470 3.000 2.997 -0.003 -... -10 22.52680 14.455 14.450 -0.005 - -✓ Calibration complete! Polynomial coefficients saved. -``` - -## Advantages -✅ **No physics knowledge needed** - Just fit a curve to data -✅ **No starting_angle** - One less parameter to worry about -✅ **Flexible** - Can fit any smooth curve -✅ **Simple** - Easy to understand - -## Disadvantages -❌ **Black box** - No physical meaning -❌ **Poor extrapolation** - Don't measure outside calibrated range -❌ **Overfitting risk** - High-degree polynomials can be unstable -❌ **Needs recalibration** - If link length changes - -## Recommendations -- Use **2nd degree (quadratic)** for most cases -- Calibrate with **8-15 points** spread across your measurement range -- Include points at 0mm (ground reference) -- Don't extrapolate beyond calibrated range -- R² > 0.999 indicates excellent fit - -## When to Use This Version -- You want simplicity over physics -- You don't care about the physical model -- You're measuring within a fixed range -- You want to avoid trigonometry - -## When to Use Trigonometric Version -- You want physically meaningful parameters -- You need to extrapolate beyond calibrated range -- You want to understand the system behavior -- Link length might change +# Polynomial Curve Fitting Version + +## Overview +This is a **simplified version** that uses polynomial regression to directly map angle → height, **without any trigonometry or starting_angle parameter**. + +## Key Differences from Trigonometric Version + +| Feature | Trigonometric Version | Polynomial Version | +|---------|----------------------|-------------------| +| **Model** | `height = L×cos(θ₀) - L×cos(θ₀+θ)` | `height = a₀ + a₁×θ + a₂×θ² + ...` | +| **Parameters** | `starting_angle`, `link_length` | Polynomial coefficients | +| **Calibration** | Optimizes `starting_angle` | Fits polynomial curve | +| **Complexity** | Physics-based (requires understanding) | Pure data fitting (black box) | +| **Extrapolation** | Good (follows physics) | Poor (can diverge outside range) | + +## Usage + +### 1. Run the Script +```bash +python3 block_height_receiver_polynomial.py +``` + +### 2. Calibrate First (REQUIRED) +Press `C` to enter calibration mode: +- Choose polynomial degree (1-5, default=2) + - **1 = Linear**: Simple, but may not fit well + - **2 = Quadratic**: Good balance (recommended) + - **3 = Cubic**: More flexible + - **4-5 = Higher order**: Risk of overfitting +- Measure at least `degree + 2` points (e.g., 4 points for quadratic) +- More points = better fit +- Type `done` when finished + +### 3. Measure Heights +After calibration, press `Enter` to measure. + +## Commands +- `Enter` - Measure angle and calculate height +- `Z` - Zero encoder at current position +- `D` - Toggle diagnostic display +- `C` - Calibrate polynomial curve +- `Ctrl+C` - Quit + +## Example Calibration + +``` +>> Command [Enter/Z/D/C]: c + +============================================================ + POLYNOMIAL CURVE FITTING CALIBRATION +============================================================ + +Polynomial degree (1=linear, 2=quadratic, 3=cubic) [default=2]: 2 + +Using 2-degree polynomial + +[Point 1] Press Enter to measure... + Measured angle: 0.00000° + Enter actual height (mm): 0.0 + ✓ Point 1 recorded + +[Point 2] Press Enter to measure... + Measured angle: 4.98470° + Enter actual height (mm): 3.0 + ✓ Point 2 recorded + +... (collect more points) + +[Point 10] Press Enter to measure... + Measured angle: 22.52680° + Enter actual height (mm): 14.455 + ✓ Point 10 recorded + +[Point 11] Press Enter to measure (or type 'done' to finish)...done + +Fitting 2-degree polynomial... + +============================================================ + CALIBRATION RESULTS +============================================================ + +Polynomial degree: 2 +Number of points: 10 +R² (fit quality): 0.999876 (1.0 = perfect) +RMS Error: 0.0523 mm + +Polynomial coefficients: + a2: -1.234567e-03 + a1: 6.543210e-01 + a0: 1.234567e-02 + +height = 0.0123 + 0.6543*angle - 0.0012*angle^2 + +Verification: +Point Angle Actual Predicted Error +------------------------------------------------------------ +1 0.00000 0.000 0.012 +0.012 +2 4.98470 3.000 2.997 -0.003 +... +10 22.52680 14.455 14.450 -0.005 + +✓ Calibration complete! Polynomial coefficients saved. +``` + +## Advantages +✅ **No physics knowledge needed** - Just fit a curve to data +✅ **No starting_angle** - One less parameter to worry about +✅ **Flexible** - Can fit any smooth curve +✅ **Simple** - Easy to understand + +## Disadvantages +❌ **Black box** - No physical meaning +❌ **Poor extrapolation** - Don't measure outside calibrated range +❌ **Overfitting risk** - High-degree polynomials can be unstable +❌ **Needs recalibration** - If link length changes + +## Recommendations +- Use **2nd degree (quadratic)** for most cases +- Calibrate with **8-15 points** spread across your measurement range +- Include points at 0mm (ground reference) +- Don't extrapolate beyond calibrated range +- R² > 0.999 indicates excellent fit + +## When to Use This Version +- You want simplicity over physics +- You don't care about the physical model +- You're measuring within a fixed range +- You want to avoid trigonometry + +## When to Use Trigonometric Version +- You want physically meaningful parameters +- You need to extrapolate beyond calibrated range +- You want to understand the system behavior +- Link length might change diff --git a/projects/block_height/block_height_receiver.py b/projects/block_height/block_height_receiver.py index d093297..a768e84 100644 --- a/projects/block_height/block_height_receiver.py +++ b/projects/block_height/block_height_receiver.py @@ -1,356 +1,356 @@ -""" -Author: Swaraj Dangare -""" -import asyncio -import statistics -import math -from bleak import BleakClient, BleakScanner -import numpy as np -from scipy.optimize import least_squares - -# BLE Configuration - -ESP_NAME = "BlockOffsetEncoder" -SERVICE_UUID = "4fafc201-1fb5-459e-8fcc-c5c9c331914b" -CHAR_UUID = "beb5483e-36e1-4688-b7f5-ea07361b26a8" - -# ANSI Color Codes -class Colors: - CYAN = '\033[96m' - GREEN = '\033[92m' - YELLOW = '\033[93m' - RED = '\033[91m' - MAGENTA = '\033[95m' - BLUE = '\033[94m' - BOLD = '\033[1m' - RESET = '\033[0m' - -class HighPrecisionReceiver: - def __init__(self): - - self.latest_angle = None - self.latest_diagnostics = {} # Store AGC, MAG, MAGL, MAGH, COF - self.device = None - self.message_received = asyncio.Event() - self.starting_angle = 56.809 #59.2 56.809 - # Block height measurement configuration - self.link_length = 40.0 # mm - self.show_diagnostics = False # Toggle for diagnostic display - - def notification_handler(self, sender, data): - msg = data.decode() - try: - # New Format: "E0:66.4340,AGC:128,MAG:5432,MAGL:0,MAGH:0,COF:0" - # Parse all fields - parts = msg.split(',') - - # Extract angle from first part - if ':' in parts[0]: - _, angle_str = parts[0].split(':') - self.latest_angle = float(angle_str) - - # Extract diagnostic data - self.latest_diagnostics = {} - for part in parts[1:]: - if ':' in part: - key, value = part.split(':') - self.latest_diagnostics[key] = int(value) - - self.message_received.set() - except Exception as e: - print(f" [!] Parse Error: {e}") - - async def get_single_reading(self, client): - self.message_received.clear() - await client.write_gatt_char(CHAR_UUID, b"X") - try: - # The Ultra-Precision firmware takes ~160ms to sample 4096 times (16 blocks) - await asyncio.wait_for(self.message_received.wait(), timeout=5.0) - return self.latest_angle - except asyncio.TimeoutError: - return None - - async def zero_encoder(self, client): - """Send 'Z' command to reset encoder to zero""" - await client.write_gatt_char(CHAR_UUID, b"Z") - await asyncio.sleep(0.1) # Give firmware time to process - print(f"{Colors.MAGENTA} [INFO] Zero reset command sent to encoder{Colors.RESET}") - - def set_link_length(self): - """Allow user to configure link length""" - try: - new_length = float(input(f"Enter link length in mm (current: {self.link_length}mm): ")) - if new_length > 0: - self.link_length = new_length - print(f"{Colors.MAGENTA} [INFO] Link length set to {self.link_length}mm{Colors.RESET}") - else: - print(f"{Colors.RED} [ERROR] Link length must be positive{Colors.RESET}") - except ValueError: - print(f"{Colors.RED} [ERROR] Invalid input{Colors.RESET}") - - def calculate_height(self, angle_deg): - """Calculate vertical height from angle using trigonometry""" - # Height = link_length × sin(angle) - angle_rad = math.radians(self.starting_angle+angle_deg) - height_mm = (self.link_length * math.cos(math.radians(self.starting_angle)))-(self.link_length * math.cos(angle_rad)) - return height_mm - - async def calibrate_starting_angle(self, client): - """Calibration mode: collect 10 measurements and back-calculate starting_angle""" - print("=" * 60) - print(" STARTING ANGLE CALIBRATION MODE") - print("=" * 60) - print(f"\n{Colors.YELLOW}Instructions:{Colors.RESET}") - print("1. Position the link at a known height") - print("2. Press Enter to measure the angle") - print("3. Enter the actual height in mm") - print("4. Repeat for 10 different heights\n") - - calibration_data = [] # Store (angle, actual_height) pairs - - for i in range(10): - input(f"\n{Colors.GREEN}[{i+1}/10] Press Enter to measure...{Colors.RESET}") - - # Get angle measurement - results = [] - for _ in range(5): - val = await self.get_single_reading(client) - if val is not None: - results.append(val) - await asyncio.sleep(0.1) - - if len(results) >= 3: - # Apply inversion and unwrapping - inverted_results = [360 - r for r in results] - has_low = any(r < 10 for r in inverted_results) - has_high = any(r > 350 for r in inverted_results) - - if has_low and has_high: - unwrapped = [r if r < 180 else r - 360 for r in inverted_results] - angle = statistics.median(unwrapped) - if angle < 0: - angle += 360 - else: - angle = statistics.median(inverted_results) - - print(f" Measured angle: {Colors.CYAN}{angle:.5f}°{Colors.RESET}") - - # Get actual height from user - while True: - try: - actual_height = float(input(f" Enter actual height (mm): ")) - break - except ValueError: - print(f" {Colors.RED}Invalid input, try again{Colors.RESET}") - - calibration_data.append((angle, actual_height)) - print(f" {Colors.GREEN}✓ Point {i+1} recorded{Colors.RESET}") - else: - print(f" {Colors.RED}Failed to get reading, try again{Colors.RESET}") - return - - # Back-calculate optimal starting_angle using least-squares - print(f"\n{Colors.YELLOW}Calculating optimal starting angle...{Colors.RESET}") - - # Store original starting angle (don't modify during calibration) - original_starting_angle = self.starting_angle - - def residuals(starting_angle_guess): - """Calculate residuals (errors) for least_squares optimization""" - errors = [] - for angle_deg, actual_height in calibration_data: - # Calculate predicted height - angle_rad = math.radians(starting_angle_guess[0] + angle_deg) - predicted_height = (self.link_length * math.cos(math.radians(starting_angle_guess[0]))) - \ - (self.link_length * math.cos(angle_rad)) - error = predicted_height - actual_height - errors.append(error) - return errors - - # Optimize starting_angle using least_squares (Levenberg-Marquardt) - # This is much faster and more accurate than Nelder-Mead for this problem - result = least_squares(residuals, [original_starting_angle], method='lm') - optimal_angle = result.x[0] - - print(f"\n{Colors.BOLD}{Colors.GREEN}=" * 60) - print(" CALIBRATION RESULTS") - print("=" * 60 + Colors.RESET) - print(f"\nOld starting angle: {Colors.YELLOW}{original_starting_angle:.3f}°{Colors.RESET}") - print(f"New starting angle: {Colors.GREEN}{Colors.BOLD}{optimal_angle:.3f}°{Colors.RESET}") - - # Show prediction errors - print(f"\n{Colors.CYAN}Verification:{Colors.RESET}") - for i, (angle_deg, actual_height) in enumerate(calibration_data, 1): - angle_rad = math.radians(optimal_angle + angle_deg) - predicted_height = (self.link_length * math.cos(math.radians(optimal_angle))) - \ - (self.link_length * math.cos(angle_rad)) - error = predicted_height - actual_height - print(f" Point {i}: Actual={actual_height:.2f}mm, Predicted={predicted_height:.2f}mm, Error={error:+.2f}mm") - - # Ask user to apply - apply = input(f"\n{Colors.YELLOW}Apply this calibration? (y/n): {Colors.RESET}").strip().lower() - if apply == 'y': - self.starting_angle = optimal_angle - print(f"{Colors.GREEN}✓ Starting angle updated to {optimal_angle:.3f}°{Colors.RESET}") - print(f"{Colors.MAGENTA}Note: This value is not saved permanently. Update it in the code if needed.{Colors.RESET}") - else: - print(f"{Colors.YELLOW}Calibration discarded{Colors.RESET}") - - async def run(self): - print(f"Scanning for {ESP_NAME}...") - self.device = await BleakScanner.find_device_by_name(ESP_NAME) - - if not self.device: - print(f"Error: Could not find device named '{ESP_NAME}'") - return - - print(f"Found {self.device.name} ({self.device.address}). Connecting...") - - # Retry mechanism for connection - max_retries = 3 - client = None - - for attempt in range(max_retries): - try: - client = BleakClient(self.device, timeout=10.0) - await client.connect() - if client.is_connected: - break - except Exception as e: - print(f"Connection attempt {attempt+1}/{max_retries} failed: {e}") - if attempt < max_retries - 1: - await asyncio.sleep(2.0) - else: - print("Failed to connect after multiple retries.") - return - - # Proceed if connected - try: - print(f"Connected: {client.is_connected}") - await client.start_notify(CHAR_UUID, self.notification_handler) - - print("\n" + "="*60) - print(f"{Colors.BOLD}{Colors.CYAN} BLOCK HEIGHT MEASUREMENT SYSTEM{Colors.RESET}") - print(f" Link Length: {Colors.YELLOW}{self.link_length}mm{Colors.RESET}") - print(" Logic: Median of 5 blocks (4096-sample filtered each)") - print("="*60) - print(f"\n{Colors.BOLD}Commands:{Colors.RESET}") - print(f" {Colors.GREEN}Enter{Colors.RESET} - Measure angle and height") - print(f" {Colors.YELLOW}Z{Colors.RESET} - Zero at ground reference") - print(f" {Colors.CYAN}D{Colors.RESET} - Toggle diagnostic display") - print(f" {Colors.MAGENTA}L{Colors.RESET} - Set link length") - print(f" {Colors.BLUE}C{Colors.RESET} - Calibrate starting angle") - print(f" {Colors.RED}Ctrl+C{Colors.RESET} - Quit\n") - - try: - while True: - user_input = input("\n>> Command [Enter/Z/D/L/C]: ").strip().upper() - - if user_input == 'Z': - await self.zero_encoder(client) - continue - elif user_input == 'D': - self.show_diagnostics = not self.show_diagnostics - status = "ON" if self.show_diagnostics else "OFF" - print(f"{Colors.MAGENTA} [INFO] Diagnostic display: {status}{Colors.RESET}") - continue - elif user_input == 'L': - self.set_link_length() - continue - elif user_input == 'C': - await self.calibrate_starting_angle(client) - continue - - # print("Sampling 5 sets of data from ESP32...", end="", flush=True) - - results = [] - for i in range(5): - val = await self.get_single_reading(client) - if val is not None: - results.append(val) - print(".", end="", flush=True) - await asyncio.sleep(0.1) # Brief gap between requests - - if len(results) >= 3: - # Apply 360° inversion first - inverted_results = [360 - r for r in results] - - # Detect angle wrap-around at 0°/360° boundary - # If we have values both near 0° and near 360°, unwrap them - has_low = any(r < 10 for r in inverted_results) - has_high = any(r > 350 for r in inverted_results) - - if has_low and has_high: - # Unwrap: convert 350-360° to negative equivalent (-10 to 0°) - unwrapped = [r if r < 180 else r - 360 for r in inverted_results] - final_median = statistics.median(unwrapped) - range_val = max(unwrapped) - min(unwrapped) - - # Convert back to positive 0-360° range if needed - if final_median < 0: - final_median += 360 - else: - # No wrap-around, use normal calculation - final_median = statistics.median(inverted_results) - range_val = max(inverted_results) - min(inverted_results) - - # Calculate vertical height from angle - height_mm = self.calculate_height(final_median) - - # Calculate physical precision for link - # angle_deg * (pi/180) * link_length - precision_microns = self.link_length * (range_val * 3.14159 / 180.0) * 1000 - - print(f"\n" + "-"*60) - print(f"{Colors.CYAN}{Colors.BOLD} ANGLE : {final_median:.5f} degrees{Colors.RESET}") - print(f"{Colors.GREEN}{Colors.BOLD} VERTICAL HEIGHT: {height_mm:.3f} mm{Colors.RESET}") - # print(f" RAW JITTER : {range_val:.5f} deg ({precision_microns:.1f} microns)") - # print(f" ALL READINGS : {[round(r, 5) for r in results]}") - - # Display diagnostic data only if enabled - if self.show_diagnostics and self.latest_diagnostics: - print(f"\n{Colors.YELLOW} DIAGNOSTIC DATA:{Colors.RESET}") - print(f" RAW JITTER : {range_val:.5f} deg ({precision_microns:.1f} microns)") - print(f" ALL READINGS : {[round(r, 5) for r in results]}") - print(f" AGC (Gain) : {self.latest_diagnostics.get('AGC', 'N/A')}") - print(f" MAG (Magn.) : {self.latest_diagnostics.get('MAG', 'N/A')}") - print(f" MAGL (High) : {self.latest_diagnostics.get('MAGL', 'N/A')}") - print(f" MAGH (Low) : {self.latest_diagnostics.get('MAGH', 'N/A')}") - print(f" COF (Ovflow) : {self.latest_diagnostics.get('COF', 'N/A')}") - - print("-"*60) - - # Check for diagnostic warnings - if self.latest_diagnostics.get('MAGL', 0) == 1: - print(f"{Colors.RED} [WARNING] Magnetic field too HIGH (AGC=0x00)!{Colors.RESET}") - print(f"{Colors.RED} Action: Move magnet further from sensor.{Colors.RESET}") - elif self.latest_diagnostics.get('MAGH', 0) == 1: - print(f"{Colors.RED} [WARNING] Magnetic field too LOW (AGC=0xFF)!{Colors.RESET}") - print(f"{Colors.RED} Action: Move magnet closer to sensor.{Colors.RESET}") - elif self.latest_diagnostics.get('COF', 0) == 1: - print(f"{Colors.RED} [WARNING] CORDIC overflow detected!{Colors.RESET}") - print(f"{Colors.RED} Action: Check sensor alignment and magnetic field.{Colors.RESET}") - elif precision_microns > 50: - print(f"{Colors.YELLOW} [WARNING] Jitter is {precision_microns:.1f}u, which exceeds 50u limit.{Colors.RESET}") - print(f"{Colors.YELLOW} Check for vibrations or move the arm slightly.{Colors.RESET}") - else: - print(f"{Colors.GREEN} Read SUCCESS{Colors.RESET}") - else: - print("\n[ERROR] Failed to collect enough samples from BLE.") - - except KeyboardInterrupt: - print("\nDisconnecting...") - finally: - if client and client.is_connected: - await client.stop_notify(CHAR_UUID) - await client.disconnect() - - except Exception as e: - print(f" [!] Unexpected Error: {e}") - -if __name__ == "__main__": - receiver = HighPrecisionReceiver() - try: - asyncio.run(receiver.run()) - except KeyboardInterrupt: - pass +""" +Author: Swaraj Dangare +""" +import asyncio +import statistics +import math +from bleak import BleakClient, BleakScanner +import numpy as np +from scipy.optimize import least_squares + +# BLE Configuration + +ESP_NAME = "BlockOffsetEncoder" +SERVICE_UUID = "4fafc201-1fb5-459e-8fcc-c5c9c331914b" +CHAR_UUID = "beb5483e-36e1-4688-b7f5-ea07361b26a8" + +# ANSI Color Codes +class Colors: + CYAN = '\033[96m' + GREEN = '\033[92m' + YELLOW = '\033[93m' + RED = '\033[91m' + MAGENTA = '\033[95m' + BLUE = '\033[94m' + BOLD = '\033[1m' + RESET = '\033[0m' + +class HighPrecisionReceiver: + def __init__(self): + + self.latest_angle = None + self.latest_diagnostics = {} # Store AGC, MAG, MAGL, MAGH, COF + self.device = None + self.message_received = asyncio.Event() + self.starting_angle = 56.809 #59.2 56.809 + # Block height measurement configuration + self.link_length = 40.0 # mm + self.show_diagnostics = False # Toggle for diagnostic display + + def notification_handler(self, sender, data): + msg = data.decode() + try: + # New Format: "E0:66.4340,AGC:128,MAG:5432,MAGL:0,MAGH:0,COF:0" + # Parse all fields + parts = msg.split(',') + + # Extract angle from first part + if ':' in parts[0]: + _, angle_str = parts[0].split(':') + self.latest_angle = float(angle_str) + + # Extract diagnostic data + self.latest_diagnostics = {} + for part in parts[1:]: + if ':' in part: + key, value = part.split(':') + self.latest_diagnostics[key] = int(value) + + self.message_received.set() + except Exception as e: + print(f" [!] Parse Error: {e}") + + async def get_single_reading(self, client): + self.message_received.clear() + await client.write_gatt_char(CHAR_UUID, b"X") + try: + # The Ultra-Precision firmware takes ~160ms to sample 4096 times (16 blocks) + await asyncio.wait_for(self.message_received.wait(), timeout=5.0) + return self.latest_angle + except asyncio.TimeoutError: + return None + + async def zero_encoder(self, client): + """Send 'Z' command to reset encoder to zero""" + await client.write_gatt_char(CHAR_UUID, b"Z") + await asyncio.sleep(0.1) # Give firmware time to process + print(f"{Colors.MAGENTA} [INFO] Zero reset command sent to encoder{Colors.RESET}") + + def set_link_length(self): + """Allow user to configure link length""" + try: + new_length = float(input(f"Enter link length in mm (current: {self.link_length}mm): ")) + if new_length > 0: + self.link_length = new_length + print(f"{Colors.MAGENTA} [INFO] Link length set to {self.link_length}mm{Colors.RESET}") + else: + print(f"{Colors.RED} [ERROR] Link length must be positive{Colors.RESET}") + except ValueError: + print(f"{Colors.RED} [ERROR] Invalid input{Colors.RESET}") + + def calculate_height(self, angle_deg): + """Calculate vertical height from angle using trigonometry""" + # Height = link_length × sin(angle) + angle_rad = math.radians(self.starting_angle+angle_deg) + height_mm = (self.link_length * math.cos(math.radians(self.starting_angle)))-(self.link_length * math.cos(angle_rad)) + return height_mm + + async def calibrate_starting_angle(self, client): + """Calibration mode: collect 10 measurements and back-calculate starting_angle""" + print("=" * 60) + print(" STARTING ANGLE CALIBRATION MODE") + print("=" * 60) + print(f"\n{Colors.YELLOW}Instructions:{Colors.RESET}") + print("1. Position the link at a known height") + print("2. Press Enter to measure the angle") + print("3. Enter the actual height in mm") + print("4. Repeat for 10 different heights\n") + + calibration_data = [] # Store (angle, actual_height) pairs + + for i in range(10): + input(f"\n{Colors.GREEN}[{i+1}/10] Press Enter to measure...{Colors.RESET}") + + # Get angle measurement + results = [] + for _ in range(5): + val = await self.get_single_reading(client) + if val is not None: + results.append(val) + await asyncio.sleep(0.1) + + if len(results) >= 3: + # Apply inversion and unwrapping + inverted_results = [360 - r for r in results] + has_low = any(r < 10 for r in inverted_results) + has_high = any(r > 350 for r in inverted_results) + + if has_low and has_high: + unwrapped = [r if r < 180 else r - 360 for r in inverted_results] + angle = statistics.median(unwrapped) + if angle < 0: + angle += 360 + else: + angle = statistics.median(inverted_results) + + print(f" Measured angle: {Colors.CYAN}{angle:.5f}°{Colors.RESET}") + + # Get actual height from user + while True: + try: + actual_height = float(input(f" Enter actual height (mm): ")) + break + except ValueError: + print(f" {Colors.RED}Invalid input, try again{Colors.RESET}") + + calibration_data.append((angle, actual_height)) + print(f" {Colors.GREEN}✓ Point {i+1} recorded{Colors.RESET}") + else: + print(f" {Colors.RED}Failed to get reading, try again{Colors.RESET}") + return + + # Back-calculate optimal starting_angle using least-squares + print(f"\n{Colors.YELLOW}Calculating optimal starting angle...{Colors.RESET}") + + # Store original starting angle (don't modify during calibration) + original_starting_angle = self.starting_angle + + def residuals(starting_angle_guess): + """Calculate residuals (errors) for least_squares optimization""" + errors = [] + for angle_deg, actual_height in calibration_data: + # Calculate predicted height + angle_rad = math.radians(starting_angle_guess[0] + angle_deg) + predicted_height = (self.link_length * math.cos(math.radians(starting_angle_guess[0]))) - \ + (self.link_length * math.cos(angle_rad)) + error = predicted_height - actual_height + errors.append(error) + return errors + + # Optimize starting_angle using least_squares (Levenberg-Marquardt) + # This is much faster and more accurate than Nelder-Mead for this problem + result = least_squares(residuals, [original_starting_angle], method='lm') + optimal_angle = result.x[0] + + print(f"\n{Colors.BOLD}{Colors.GREEN}=" * 60) + print(" CALIBRATION RESULTS") + print("=" * 60 + Colors.RESET) + print(f"\nOld starting angle: {Colors.YELLOW}{original_starting_angle:.3f}°{Colors.RESET}") + print(f"New starting angle: {Colors.GREEN}{Colors.BOLD}{optimal_angle:.3f}°{Colors.RESET}") + + # Show prediction errors + print(f"\n{Colors.CYAN}Verification:{Colors.RESET}") + for i, (angle_deg, actual_height) in enumerate(calibration_data, 1): + angle_rad = math.radians(optimal_angle + angle_deg) + predicted_height = (self.link_length * math.cos(math.radians(optimal_angle))) - \ + (self.link_length * math.cos(angle_rad)) + error = predicted_height - actual_height + print(f" Point {i}: Actual={actual_height:.2f}mm, Predicted={predicted_height:.2f}mm, Error={error:+.2f}mm") + + # Ask user to apply + apply = input(f"\n{Colors.YELLOW}Apply this calibration? (y/n): {Colors.RESET}").strip().lower() + if apply == 'y': + self.starting_angle = optimal_angle + print(f"{Colors.GREEN}✓ Starting angle updated to {optimal_angle:.3f}°{Colors.RESET}") + print(f"{Colors.MAGENTA}Note: This value is not saved permanently. Update it in the code if needed.{Colors.RESET}") + else: + print(f"{Colors.YELLOW}Calibration discarded{Colors.RESET}") + + async def run(self): + print(f"Scanning for {ESP_NAME}...") + self.device = await BleakScanner.find_device_by_name(ESP_NAME) + + if not self.device: + print(f"Error: Could not find device named '{ESP_NAME}'") + return + + print(f"Found {self.device.name} ({self.device.address}). Connecting...") + + # Retry mechanism for connection + max_retries = 3 + client = None + + for attempt in range(max_retries): + try: + client = BleakClient(self.device, timeout=10.0) + await client.connect() + if client.is_connected: + break + except Exception as e: + print(f"Connection attempt {attempt+1}/{max_retries} failed: {e}") + if attempt < max_retries - 1: + await asyncio.sleep(2.0) + else: + print("Failed to connect after multiple retries.") + return + + # Proceed if connected + try: + print(f"Connected: {client.is_connected}") + await client.start_notify(CHAR_UUID, self.notification_handler) + + print("\n" + "="*60) + print(f"{Colors.BOLD}{Colors.CYAN} BLOCK HEIGHT MEASUREMENT SYSTEM{Colors.RESET}") + print(f" Link Length: {Colors.YELLOW}{self.link_length}mm{Colors.RESET}") + print(" Logic: Median of 5 blocks (4096-sample filtered each)") + print("="*60) + print(f"\n{Colors.BOLD}Commands:{Colors.RESET}") + print(f" {Colors.GREEN}Enter{Colors.RESET} - Measure angle and height") + print(f" {Colors.YELLOW}Z{Colors.RESET} - Zero at ground reference") + print(f" {Colors.CYAN}D{Colors.RESET} - Toggle diagnostic display") + print(f" {Colors.MAGENTA}L{Colors.RESET} - Set link length") + print(f" {Colors.BLUE}C{Colors.RESET} - Calibrate starting angle") + print(f" {Colors.RED}Ctrl+C{Colors.RESET} - Quit\n") + + try: + while True: + user_input = input("\n>> Command [Enter/Z/D/L/C]: ").strip().upper() + + if user_input == 'Z': + await self.zero_encoder(client) + continue + elif user_input == 'D': + self.show_diagnostics = not self.show_diagnostics + status = "ON" if self.show_diagnostics else "OFF" + print(f"{Colors.MAGENTA} [INFO] Diagnostic display: {status}{Colors.RESET}") + continue + elif user_input == 'L': + self.set_link_length() + continue + elif user_input == 'C': + await self.calibrate_starting_angle(client) + continue + + # print("Sampling 5 sets of data from ESP32...", end="", flush=True) + + results = [] + for i in range(5): + val = await self.get_single_reading(client) + if val is not None: + results.append(val) + print(".", end="", flush=True) + await asyncio.sleep(0.1) # Brief gap between requests + + if len(results) >= 3: + # Apply 360° inversion first + inverted_results = [360 - r for r in results] + + # Detect angle wrap-around at 0°/360° boundary + # If we have values both near 0° and near 360°, unwrap them + has_low = any(r < 10 for r in inverted_results) + has_high = any(r > 350 for r in inverted_results) + + if has_low and has_high: + # Unwrap: convert 350-360° to negative equivalent (-10 to 0°) + unwrapped = [r if r < 180 else r - 360 for r in inverted_results] + final_median = statistics.median(unwrapped) + range_val = max(unwrapped) - min(unwrapped) + + # Convert back to positive 0-360° range if needed + if final_median < 0: + final_median += 360 + else: + # No wrap-around, use normal calculation + final_median = statistics.median(inverted_results) + range_val = max(inverted_results) - min(inverted_results) + + # Calculate vertical height from angle + height_mm = self.calculate_height(final_median) + + # Calculate physical precision for link + # angle_deg * (pi/180) * link_length + precision_microns = self.link_length * (range_val * 3.14159 / 180.0) * 1000 + + print(f"\n" + "-"*60) + print(f"{Colors.CYAN}{Colors.BOLD} ANGLE : {final_median:.5f} degrees{Colors.RESET}") + print(f"{Colors.GREEN}{Colors.BOLD} VERTICAL HEIGHT: {height_mm:.3f} mm{Colors.RESET}") + # print(f" RAW JITTER : {range_val:.5f} deg ({precision_microns:.1f} microns)") + # print(f" ALL READINGS : {[round(r, 5) for r in results]}") + + # Display diagnostic data only if enabled + if self.show_diagnostics and self.latest_diagnostics: + print(f"\n{Colors.YELLOW} DIAGNOSTIC DATA:{Colors.RESET}") + print(f" RAW JITTER : {range_val:.5f} deg ({precision_microns:.1f} microns)") + print(f" ALL READINGS : {[round(r, 5) for r in results]}") + print(f" AGC (Gain) : {self.latest_diagnostics.get('AGC', 'N/A')}") + print(f" MAG (Magn.) : {self.latest_diagnostics.get('MAG', 'N/A')}") + print(f" MAGL (High) : {self.latest_diagnostics.get('MAGL', 'N/A')}") + print(f" MAGH (Low) : {self.latest_diagnostics.get('MAGH', 'N/A')}") + print(f" COF (Ovflow) : {self.latest_diagnostics.get('COF', 'N/A')}") + + print("-"*60) + + # Check for diagnostic warnings + if self.latest_diagnostics.get('MAGL', 0) == 1: + print(f"{Colors.RED} [WARNING] Magnetic field too HIGH (AGC=0x00)!{Colors.RESET}") + print(f"{Colors.RED} Action: Move magnet further from sensor.{Colors.RESET}") + elif self.latest_diagnostics.get('MAGH', 0) == 1: + print(f"{Colors.RED} [WARNING] Magnetic field too LOW (AGC=0xFF)!{Colors.RESET}") + print(f"{Colors.RED} Action: Move magnet closer to sensor.{Colors.RESET}") + elif self.latest_diagnostics.get('COF', 0) == 1: + print(f"{Colors.RED} [WARNING] CORDIC overflow detected!{Colors.RESET}") + print(f"{Colors.RED} Action: Check sensor alignment and magnetic field.{Colors.RESET}") + elif precision_microns > 50: + print(f"{Colors.YELLOW} [WARNING] Jitter is {precision_microns:.1f}u, which exceeds 50u limit.{Colors.RESET}") + print(f"{Colors.YELLOW} Check for vibrations or move the arm slightly.{Colors.RESET}") + else: + print(f"{Colors.GREEN} Read SUCCESS{Colors.RESET}") + else: + print("\n[ERROR] Failed to collect enough samples from BLE.") + + except KeyboardInterrupt: + print("\nDisconnecting...") + finally: + if client and client.is_connected: + await client.stop_notify(CHAR_UUID) + await client.disconnect() + + except Exception as e: + print(f" [!] Unexpected Error: {e}") + +if __name__ == "__main__": + receiver = HighPrecisionReceiver() + try: + asyncio.run(receiver.run()) + except KeyboardInterrupt: + pass diff --git a/projects/block_height/block_height_receiver_polynomial.py b/projects/block_height/block_height_receiver_polynomial.py index 53b9396..49ded21 100644 --- a/projects/block_height/block_height_receiver_polynomial.py +++ b/projects/block_height/block_height_receiver_polynomial.py @@ -1,354 +1,354 @@ -""" -Author: Swaraj Dangare -""" -import asyncio -import statistics -import math -from bleak import BleakClient, BleakScanner -import numpy as np - -# BLE Configuration - -ESP_NAME = "BlockOffsetEncoder" -SERVICE_UUID = "4fafc201-1fb5-459e-8fcc-c5c9c331914b" -CHAR_UUID = "beb5483e-36e1-4688-b7f5-ea07361b26a8" - -# ANSI Color Codes -class Colors: - CYAN = '\033[96m' - GREEN = '\033[92m' - YELLOW = '\033[93m' - RED = '\033[91m' - MAGENTA = '\033[95m' - BLUE = '\033[94m' - BOLD = '\033[1m' - RESET = '\033[0m' - -class PolynomialHeightReceiver: - def __init__(self): - - self.latest_angle = None - self.latest_diagnostics = {} # Store AGC, MAG, MAGL, MAGH, COF - self.device = None - self.message_received = asyncio.Event() - - # Polynomial coefficients (will be set during calibration) - # Default: 3rd order polynomial [a0, a1, a2, a3] where height = a0 + a1*angle + a2*angle^2 + a3*angle^3 - # Pre-calibrated coefficients (from user's calibration): - self.poly_coeffs = np.array([-1.065580e-05, 2.174321e-03, 5.983315e-01, -3.448675e-02]) - self.poly_degree = 3 # Default to 3rd order (cubic) - - # Configuration - self.show_diagnostics = False # Toggle for diagnostic display - - def notification_handler(self, sender, data): - msg = data.decode() - try: - # New Format: "E0:66.4340,AGC:128,MAG:5432,MAGL:0,MAGH:0,COF:0" - # Parse all fields - parts = msg.split(',') - - # Extract angle from first part - if ':' in parts[0]: - _, angle_str = parts[0].split(':') - self.latest_angle = float(angle_str) - - # Extract diagnostic data - self.latest_diagnostics = {} - for part in parts[1:]: - if ':' in part: - key, value = part.split(':') - self.latest_diagnostics[key] = int(value) - - self.message_received.set() - except Exception as e: - print(f" [!] Parse Error: {e}") - - async def get_single_reading(self, client): - self.message_received.clear() - await client.write_gatt_char(CHAR_UUID, b"X") - try: - # The Ultra-Precision firmware takes ~160ms to sample 4096 times (16 blocks) - await asyncio.wait_for(self.message_received.wait(), timeout=5.0) - return self.latest_angle - except asyncio.TimeoutError: - return None - - async def zero_encoder(self, client): - """Send 'Z' command to reset encoder to zero""" - await client.write_gatt_char(CHAR_UUID, b"Z") - await asyncio.sleep(0.1) # Give firmware time to process - print(f"{Colors.MAGENTA} [INFO] Zero reset command sent to encoder{Colors.RESET}") - - def calculate_height(self, angle_deg): - """Calculate height from angle using polynomial fit""" - if self.poly_coeffs is None: - print(f"{Colors.RED} [ERROR] No calibration data! Run calibration first (press 'C'){Colors.RESET}") - return 0.0 - - # Evaluate polynomial: height = a0 + a1*x + a2*x^2 + ... - height_mm = np.polyval(self.poly_coeffs, angle_deg) - return height_mm - - async def calibrate_polynomial(self, client): - """Calibration mode: collect measurements and fit polynomial curve""" - print("=" * 60) - print(" POLYNOMIAL CURVE FITTING CALIBRATION") - print("=" * 60) - print(f"\n{Colors.YELLOW}Instructions:{Colors.RESET}") - print("1. Position the link at a known height") - print("2. Press Enter to measure the angle") - print("3. Enter the actual height in mm") - print("4. Collect at least 5 points (more is better!)") - print("5. Type 'done' when finished\n") - - # Ask for polynomial degree - while True: - try: - degree_input = input(f"Polynomial degree (1=linear, 2=quadratic, 3=cubic) [default=3]: ").strip() - if degree_input == "": - self.poly_degree = 3 - break - degree = int(degree_input) - if 1 <= degree <= 5: - self.poly_degree = degree - break - else: - print(f"{Colors.RED}Please enter a degree between 1 and 5{Colors.RESET}") - except ValueError: - print(f"{Colors.RED}Invalid input{Colors.RESET}") - - print(f"\n{Colors.CYAN}Using {self.poly_degree}-degree polynomial{Colors.RESET}\n") - - calibration_data = [] # Store (angle, actual_height) pairs - point_num = 1 - - while True: - user_input = input(f"\n{Colors.GREEN}[Point {point_num}] Press Enter to measure (or type 'done' to finish)...{Colors.RESET}").strip().lower() - - if user_input == 'done': - if len(calibration_data) < self.poly_degree + 2: - print(f"{Colors.RED}Need at least {self.poly_degree + 2} points for {self.poly_degree}-degree polynomial!{Colors.RESET}") - continue - break - - # Get angle measurement - results = [] - for _ in range(5): - val = await self.get_single_reading(client) - if val is not None: - results.append(val) - await asyncio.sleep(0.1) - - if len(results) >= 3: - # Apply inversion and unwrapping - inverted_results = [360 - r for r in results] - has_low = any(r < 10 for r in inverted_results) - has_high = any(r > 350 for r in inverted_results) - - if has_low and has_high: - unwrapped = [r if r < 180 else r - 360 for r in inverted_results] - angle = statistics.median(unwrapped) - if angle < 0: - angle += 360 - else: - angle = statistics.median(inverted_results) - - print(f" Measured angle: {Colors.CYAN}{angle:.5f}°{Colors.RESET}") - - # Get actual height from user - while True: - try: - actual_height = float(input(f" Enter actual height (mm): ")) - break - except ValueError: - print(f" {Colors.RED}Invalid input, try again{Colors.RESET}") - - calibration_data.append((angle, actual_height)) - print(f" {Colors.GREEN}✓ Point {point_num} recorded{Colors.RESET}") - point_num += 1 - else: - print(f" {Colors.RED}Failed to get reading, try again{Colors.RESET}") - - # Fit polynomial curve - print(f"\n{Colors.YELLOW}Fitting {self.poly_degree}-degree polynomial...{Colors.RESET}") - - angles = np.array([d[0] for d in calibration_data]) - heights = np.array([d[1] for d in calibration_data]) - - # Fit polynomial using numpy polyfit - self.poly_coeffs = np.polyfit(angles, heights, self.poly_degree) - - # Calculate R² (coefficient of determination) - predicted_heights = np.polyval(self.poly_coeffs, angles) - residuals = heights - predicted_heights - ss_res = np.sum(residuals**2) - ss_tot = np.sum((heights - np.mean(heights))**2) - r_squared = 1 - (ss_res / ss_tot) - rms_error = np.sqrt(np.mean(residuals**2)) - - print(f"\n{Colors.BOLD}{Colors.GREEN}=" * 60) - print(" CALIBRATION RESULTS") - print("=" * 60 + Colors.RESET) - print(f"\nPolynomial degree: {self.poly_degree}") - print(f"Number of points: {len(calibration_data)}") - print(f"R² (fit quality): {Colors.GREEN}{r_squared:.6f}{Colors.RESET} (1.0 = perfect)") - print(f"RMS Error: {rms_error:.4f} mm") - - # Show coefficients - print(f"\n{Colors.CYAN}Polynomial coefficients:{Colors.RESET}") - for i, coeff in enumerate(self.poly_coeffs): - power = self.poly_degree - i - print(f" a{power}: {coeff:.6e}") - - # Show equation - equation = "height = " - for i, coeff in enumerate(self.poly_coeffs): - power = self.poly_degree - i - if i > 0: - equation += " + " if coeff >= 0 else " - " - equation += f"{abs(coeff):.4f}" - else: - equation += f"{coeff:.4f}" - - if power > 0: - equation += f"*angle^{power}" if power > 1 else "*angle" - print(f"\n{Colors.YELLOW}{equation}{Colors.RESET}") - - # Show prediction errors - print(f"\n{Colors.CYAN}Verification:{Colors.RESET}") - print(f"{'Point':<8} {'Angle':<12} {'Actual':<10} {'Predicted':<12} {'Error':<10}") - print("-" * 60) - - for i, (angle_deg, actual_height) in enumerate(calibration_data, 1): - predicted_height = np.polyval(self.poly_coeffs, angle_deg) - error = predicted_height - actual_height - print(f"{i:<8} {angle_deg:<12.5f} {actual_height:<10.3f} {predicted_height:<12.3f} {error:+10.3f}") - - print(f"\n{Colors.GREEN}✓ Calibration complete! Polynomial coefficients saved.{Colors.RESET}") - print(f"{Colors.MAGENTA}Note: Coefficients are stored in memory only. They will be lost on restart.{Colors.RESET}") - - async def run(self): - print(f"Scanning for {ESP_NAME}...") - self.device = await BleakScanner.find_device_by_name(ESP_NAME) - - if not self.device: - print(f"Error: Could not find device named '{ESP_NAME}'") - return - - print(f"Found {self.device.name} ({self.device.address}). Connecting...") - - # Retry mechanism for connection - max_retries = 3 - client = None - - for attempt in range(max_retries): - try: - client = BleakClient(self.device, timeout=10.0) - await client.connect() - if client.is_connected: - break - except Exception as e: - print(f"Connection attempt {attempt+1}/{max_retries} failed: {e}") - if attempt < max_retries - 1: - await asyncio.sleep(2.0) - else: - print("Failed to connect after multiple retries.") - return - - # Proceed if connected - try: - print(f"Connected: {client.is_connected}") - await client.start_notify(CHAR_UUID, self.notification_handler) - - print("\n" + "="*60) - print(f"{Colors.BOLD}{Colors.CYAN} POLYNOMIAL HEIGHT MEASUREMENT SYSTEM{Colors.RESET}") - print(" Logic: Polynomial curve fitting (angle → height)") - print("="*60) - print(f"\n{Colors.BOLD}Commands:{Colors.RESET}") - print(f" {Colors.GREEN}Enter{Colors.RESET} - Measure angle and height") - print(f" {Colors.YELLOW}Z{Colors.RESET} - Zero at ground reference") - print(f" {Colors.CYAN}D{Colors.RESET} - Toggle diagnostic display") - print(f" {Colors.BLUE}C{Colors.RESET} - Calibrate polynomial curve") - print(f" {Colors.RED}Ctrl+C{Colors.RESET} - Quit\n") - - try: - while True: - user_input = input("\n>> Command [Enter/Z/D/C]: ").strip().upper() - - if user_input == 'Z': - await self.zero_encoder(client) - continue - elif user_input == 'D': - self.show_diagnostics = not self.show_diagnostics - status = "ON" if self.show_diagnostics else "OFF" - print(f"{Colors.MAGENTA} [INFO] Diagnostic display: {status}{Colors.RESET}") - continue - elif user_input == 'C': - await self.calibrate_polynomial(client) - continue - - results = [] - for i in range(5): - val = await self.get_single_reading(client) - if val is not None: - results.append(val) - print(".", end="", flush=True) - await asyncio.sleep(0.1) - - if len(results) >= 3: - # Apply inversion and unwrapping - inverted_results = [360 - r for r in results] - has_low = any(r < 10 for r in inverted_results) - has_high = any(r > 350 for r in inverted_results) - - if has_low and has_high: - unwrapped = [r if r < 180 else r - 360 for r in inverted_results] - final_median = statistics.median(unwrapped) - range_val = max(unwrapped) - min(unwrapped) - if final_median < 0: - final_median += 360 - else: - final_median = statistics.median(inverted_results) - range_val = max(inverted_results) - min(inverted_results) - - # Calculate height using polynomial - height_mm = self.calculate_height(final_median) - - print(f"\n" + "-"*60) - print(f"{Colors.CYAN}{Colors.BOLD} ANGLE : {final_median:.5f} degrees{Colors.RESET}") - print(f"{Colors.GREEN}{Colors.BOLD} HEIGHT : {height_mm:.3f} mm{Colors.RESET}") - - # Display diagnostic data only if enabled - if self.show_diagnostics and self.latest_diagnostics: - precision_microns = 40.0 * (range_val * 3.14159 / 180.0) * 1000 - print(f"\n{Colors.YELLOW} DIAGNOSTIC DATA:{Colors.RESET}") - print(f" RAW JITTER : {range_val:.5f} deg ({precision_microns:.1f} microns)") - print(f" ALL READINGS : {[round(r, 5) for r in results]}") - print(f" AGC (Gain) : {self.latest_diagnostics.get('AGC', 'N/A')}") - print(f" MAG (Magn.) : {self.latest_diagnostics.get('MAG', 'N/A')}") - print(f" MAGL (High) : {self.latest_diagnostics.get('MAGL', 'N/A')}") - print(f" MAGH (Low) : {self.latest_diagnostics.get('MAGH', 'N/A')}") - print(f" COF (Ovflow) : {self.latest_diagnostics.get('COF', 'N/A')}") - - print("-"*60) - print(f"{Colors.GREEN} Read SUCCESS{Colors.RESET}") - else: - print("\n[ERROR] Failed to collect enough samples from BLE.") - - except KeyboardInterrupt: - print("\nDisconnecting...") - finally: - if client and client.is_connected: - await client.stop_notify(CHAR_UUID) - await client.disconnect() - - except Exception as e: - print(f" [!] Unexpected Error: {e}") - -if __name__ == "__main__": - receiver = PolynomialHeightReceiver() - try: - asyncio.run(receiver.run()) - except KeyboardInterrupt: - pass +""" +Author: Swaraj Dangare +""" +import asyncio +import statistics +import math +from bleak import BleakClient, BleakScanner +import numpy as np + +# BLE Configuration + +ESP_NAME = "BlockOffsetEncoder" +SERVICE_UUID = "4fafc201-1fb5-459e-8fcc-c5c9c331914b" +CHAR_UUID = "beb5483e-36e1-4688-b7f5-ea07361b26a8" + +# ANSI Color Codes +class Colors: + CYAN = '\033[96m' + GREEN = '\033[92m' + YELLOW = '\033[93m' + RED = '\033[91m' + MAGENTA = '\033[95m' + BLUE = '\033[94m' + BOLD = '\033[1m' + RESET = '\033[0m' + +class PolynomialHeightReceiver: + def __init__(self): + + self.latest_angle = None + self.latest_diagnostics = {} # Store AGC, MAG, MAGL, MAGH, COF + self.device = None + self.message_received = asyncio.Event() + + # Polynomial coefficients (will be set during calibration) + # Default: 3rd order polynomial [a0, a1, a2, a3] where height = a0 + a1*angle + a2*angle^2 + a3*angle^3 + # Pre-calibrated coefficients (from user's calibration): + self.poly_coeffs = np.array([-1.065580e-05, 2.174321e-03, 5.983315e-01, -3.448675e-02]) + self.poly_degree = 3 # Default to 3rd order (cubic) + + # Configuration + self.show_diagnostics = False # Toggle for diagnostic display + + def notification_handler(self, sender, data): + msg = data.decode() + try: + # New Format: "E0:66.4340,AGC:128,MAG:5432,MAGL:0,MAGH:0,COF:0" + # Parse all fields + parts = msg.split(',') + + # Extract angle from first part + if ':' in parts[0]: + _, angle_str = parts[0].split(':') + self.latest_angle = float(angle_str) + + # Extract diagnostic data + self.latest_diagnostics = {} + for part in parts[1:]: + if ':' in part: + key, value = part.split(':') + self.latest_diagnostics[key] = int(value) + + self.message_received.set() + except Exception as e: + print(f" [!] Parse Error: {e}") + + async def get_single_reading(self, client): + self.message_received.clear() + await client.write_gatt_char(CHAR_UUID, b"X") + try: + # The Ultra-Precision firmware takes ~160ms to sample 4096 times (16 blocks) + await asyncio.wait_for(self.message_received.wait(), timeout=5.0) + return self.latest_angle + except asyncio.TimeoutError: + return None + + async def zero_encoder(self, client): + """Send 'Z' command to reset encoder to zero""" + await client.write_gatt_char(CHAR_UUID, b"Z") + await asyncio.sleep(0.1) # Give firmware time to process + print(f"{Colors.MAGENTA} [INFO] Zero reset command sent to encoder{Colors.RESET}") + + def calculate_height(self, angle_deg): + """Calculate height from angle using polynomial fit""" + if self.poly_coeffs is None: + print(f"{Colors.RED} [ERROR] No calibration data! Run calibration first (press 'C'){Colors.RESET}") + return 0.0 + + # Evaluate polynomial: height = a0 + a1*x + a2*x^2 + ... + height_mm = np.polyval(self.poly_coeffs, angle_deg) + return height_mm + + async def calibrate_polynomial(self, client): + """Calibration mode: collect measurements and fit polynomial curve""" + print("=" * 60) + print(" POLYNOMIAL CURVE FITTING CALIBRATION") + print("=" * 60) + print(f"\n{Colors.YELLOW}Instructions:{Colors.RESET}") + print("1. Position the link at a known height") + print("2. Press Enter to measure the angle") + print("3. Enter the actual height in mm") + print("4. Collect at least 5 points (more is better!)") + print("5. Type 'done' when finished\n") + + # Ask for polynomial degree + while True: + try: + degree_input = input(f"Polynomial degree (1=linear, 2=quadratic, 3=cubic) [default=3]: ").strip() + if degree_input == "": + self.poly_degree = 3 + break + degree = int(degree_input) + if 1 <= degree <= 5: + self.poly_degree = degree + break + else: + print(f"{Colors.RED}Please enter a degree between 1 and 5{Colors.RESET}") + except ValueError: + print(f"{Colors.RED}Invalid input{Colors.RESET}") + + print(f"\n{Colors.CYAN}Using {self.poly_degree}-degree polynomial{Colors.RESET}\n") + + calibration_data = [] # Store (angle, actual_height) pairs + point_num = 1 + + while True: + user_input = input(f"\n{Colors.GREEN}[Point {point_num}] Press Enter to measure (or type 'done' to finish)...{Colors.RESET}").strip().lower() + + if user_input == 'done': + if len(calibration_data) < self.poly_degree + 2: + print(f"{Colors.RED}Need at least {self.poly_degree + 2} points for {self.poly_degree}-degree polynomial!{Colors.RESET}") + continue + break + + # Get angle measurement + results = [] + for _ in range(5): + val = await self.get_single_reading(client) + if val is not None: + results.append(val) + await asyncio.sleep(0.1) + + if len(results) >= 3: + # Apply inversion and unwrapping + inverted_results = [360 - r for r in results] + has_low = any(r < 10 for r in inverted_results) + has_high = any(r > 350 for r in inverted_results) + + if has_low and has_high: + unwrapped = [r if r < 180 else r - 360 for r in inverted_results] + angle = statistics.median(unwrapped) + if angle < 0: + angle += 360 + else: + angle = statistics.median(inverted_results) + + print(f" Measured angle: {Colors.CYAN}{angle:.5f}°{Colors.RESET}") + + # Get actual height from user + while True: + try: + actual_height = float(input(f" Enter actual height (mm): ")) + break + except ValueError: + print(f" {Colors.RED}Invalid input, try again{Colors.RESET}") + + calibration_data.append((angle, actual_height)) + print(f" {Colors.GREEN}✓ Point {point_num} recorded{Colors.RESET}") + point_num += 1 + else: + print(f" {Colors.RED}Failed to get reading, try again{Colors.RESET}") + + # Fit polynomial curve + print(f"\n{Colors.YELLOW}Fitting {self.poly_degree}-degree polynomial...{Colors.RESET}") + + angles = np.array([d[0] for d in calibration_data]) + heights = np.array([d[1] for d in calibration_data]) + + # Fit polynomial using numpy polyfit + self.poly_coeffs = np.polyfit(angles, heights, self.poly_degree) + + # Calculate R² (coefficient of determination) + predicted_heights = np.polyval(self.poly_coeffs, angles) + residuals = heights - predicted_heights + ss_res = np.sum(residuals**2) + ss_tot = np.sum((heights - np.mean(heights))**2) + r_squared = 1 - (ss_res / ss_tot) + rms_error = np.sqrt(np.mean(residuals**2)) + + print(f"\n{Colors.BOLD}{Colors.GREEN}=" * 60) + print(" CALIBRATION RESULTS") + print("=" * 60 + Colors.RESET) + print(f"\nPolynomial degree: {self.poly_degree}") + print(f"Number of points: {len(calibration_data)}") + print(f"R² (fit quality): {Colors.GREEN}{r_squared:.6f}{Colors.RESET} (1.0 = perfect)") + print(f"RMS Error: {rms_error:.4f} mm") + + # Show coefficients + print(f"\n{Colors.CYAN}Polynomial coefficients:{Colors.RESET}") + for i, coeff in enumerate(self.poly_coeffs): + power = self.poly_degree - i + print(f" a{power}: {coeff:.6e}") + + # Show equation + equation = "height = " + for i, coeff in enumerate(self.poly_coeffs): + power = self.poly_degree - i + if i > 0: + equation += " + " if coeff >= 0 else " - " + equation += f"{abs(coeff):.4f}" + else: + equation += f"{coeff:.4f}" + + if power > 0: + equation += f"*angle^{power}" if power > 1 else "*angle" + print(f"\n{Colors.YELLOW}{equation}{Colors.RESET}") + + # Show prediction errors + print(f"\n{Colors.CYAN}Verification:{Colors.RESET}") + print(f"{'Point':<8} {'Angle':<12} {'Actual':<10} {'Predicted':<12} {'Error':<10}") + print("-" * 60) + + for i, (angle_deg, actual_height) in enumerate(calibration_data, 1): + predicted_height = np.polyval(self.poly_coeffs, angle_deg) + error = predicted_height - actual_height + print(f"{i:<8} {angle_deg:<12.5f} {actual_height:<10.3f} {predicted_height:<12.3f} {error:+10.3f}") + + print(f"\n{Colors.GREEN}✓ Calibration complete! Polynomial coefficients saved.{Colors.RESET}") + print(f"{Colors.MAGENTA}Note: Coefficients are stored in memory only. They will be lost on restart.{Colors.RESET}") + + async def run(self): + print(f"Scanning for {ESP_NAME}...") + self.device = await BleakScanner.find_device_by_name(ESP_NAME) + + if not self.device: + print(f"Error: Could not find device named '{ESP_NAME}'") + return + + print(f"Found {self.device.name} ({self.device.address}). Connecting...") + + # Retry mechanism for connection + max_retries = 3 + client = None + + for attempt in range(max_retries): + try: + client = BleakClient(self.device, timeout=10.0) + await client.connect() + if client.is_connected: + break + except Exception as e: + print(f"Connection attempt {attempt+1}/{max_retries} failed: {e}") + if attempt < max_retries - 1: + await asyncio.sleep(2.0) + else: + print("Failed to connect after multiple retries.") + return + + # Proceed if connected + try: + print(f"Connected: {client.is_connected}") + await client.start_notify(CHAR_UUID, self.notification_handler) + + print("\n" + "="*60) + print(f"{Colors.BOLD}{Colors.CYAN} POLYNOMIAL HEIGHT MEASUREMENT SYSTEM{Colors.RESET}") + print(" Logic: Polynomial curve fitting (angle → height)") + print("="*60) + print(f"\n{Colors.BOLD}Commands:{Colors.RESET}") + print(f" {Colors.GREEN}Enter{Colors.RESET} - Measure angle and height") + print(f" {Colors.YELLOW}Z{Colors.RESET} - Zero at ground reference") + print(f" {Colors.CYAN}D{Colors.RESET} - Toggle diagnostic display") + print(f" {Colors.BLUE}C{Colors.RESET} - Calibrate polynomial curve") + print(f" {Colors.RED}Ctrl+C{Colors.RESET} - Quit\n") + + try: + while True: + user_input = input("\n>> Command [Enter/Z/D/C]: ").strip().upper() + + if user_input == 'Z': + await self.zero_encoder(client) + continue + elif user_input == 'D': + self.show_diagnostics = not self.show_diagnostics + status = "ON" if self.show_diagnostics else "OFF" + print(f"{Colors.MAGENTA} [INFO] Diagnostic display: {status}{Colors.RESET}") + continue + elif user_input == 'C': + await self.calibrate_polynomial(client) + continue + + results = [] + for i in range(5): + val = await self.get_single_reading(client) + if val is not None: + results.append(val) + print(".", end="", flush=True) + await asyncio.sleep(0.1) + + if len(results) >= 3: + # Apply inversion and unwrapping + inverted_results = [360 - r for r in results] + has_low = any(r < 10 for r in inverted_results) + has_high = any(r > 350 for r in inverted_results) + + if has_low and has_high: + unwrapped = [r if r < 180 else r - 360 for r in inverted_results] + final_median = statistics.median(unwrapped) + range_val = max(unwrapped) - min(unwrapped) + if final_median < 0: + final_median += 360 + else: + final_median = statistics.median(inverted_results) + range_val = max(inverted_results) - min(inverted_results) + + # Calculate height using polynomial + height_mm = self.calculate_height(final_median) + + print(f"\n" + "-"*60) + print(f"{Colors.CYAN}{Colors.BOLD} ANGLE : {final_median:.5f} degrees{Colors.RESET}") + print(f"{Colors.GREEN}{Colors.BOLD} HEIGHT : {height_mm:.3f} mm{Colors.RESET}") + + # Display diagnostic data only if enabled + if self.show_diagnostics and self.latest_diagnostics: + precision_microns = 40.0 * (range_val * 3.14159 / 180.0) * 1000 + print(f"\n{Colors.YELLOW} DIAGNOSTIC DATA:{Colors.RESET}") + print(f" RAW JITTER : {range_val:.5f} deg ({precision_microns:.1f} microns)") + print(f" ALL READINGS : {[round(r, 5) for r in results]}") + print(f" AGC (Gain) : {self.latest_diagnostics.get('AGC', 'N/A')}") + print(f" MAG (Magn.) : {self.latest_diagnostics.get('MAG', 'N/A')}") + print(f" MAGL (High) : {self.latest_diagnostics.get('MAGL', 'N/A')}") + print(f" MAGH (Low) : {self.latest_diagnostics.get('MAGH', 'N/A')}") + print(f" COF (Ovflow) : {self.latest_diagnostics.get('COF', 'N/A')}") + + print("-"*60) + print(f"{Colors.GREEN} Read SUCCESS{Colors.RESET}") + else: + print("\n[ERROR] Failed to collect enough samples from BLE.") + + except KeyboardInterrupt: + print("\nDisconnecting...") + finally: + if client and client.is_connected: + await client.stop_notify(CHAR_UUID) + await client.disconnect() + + except Exception as e: + print(f" [!] Unexpected Error: {e}") + +if __name__ == "__main__": + receiver = PolynomialHeightReceiver() + try: + asyncio.run(receiver.run()) + except KeyboardInterrupt: + pass diff --git a/projects/block_height/calibrate_offline.py b/projects/block_height/calibrate_offline.py index 45c9a1d..785c7f4 100644 --- a/projects/block_height/calibrate_offline.py +++ b/projects/block_height/calibrate_offline.py @@ -1,69 +1,69 @@ -#!/usr/bin/env python3 -""" -Offline calibration script to calculate optimal starting_angle from collected data -Author: Swaraj Dangare -""" - -import math -from scipy.optimize import least_squares - -# Calibration data: (angle_deg, actual_height_mm) -calibration_data = [ - (359.99749, 0.00), - (0.86023, 0.5), - (1.69302, 1.005), - (1.79689, 1.05), - (1.99437, 1.180), - (4.98561, 3.00), - (8.52139, 5.185), - (12.08255, 7.50), - (15.24499, 9.055), - (22.52680, 14.455), -] - -# System parameters -link_length = 40.0 # mm -initial_guess = 59.2 # degrees (current starting_angle) - -def residuals(starting_angle_guess): - """Calculate residuals (errors) for least_squares optimization""" - errors = [] - for angle_deg, actual_height in calibration_data: - # Calculate predicted height - angle_rad = math.radians(starting_angle_guess[0] + angle_deg) - predicted_height = (link_length * math.cos(math.radians(starting_angle_guess[0]))) - \ - (link_length * math.cos(angle_rad)) - error = predicted_height - actual_height - errors.append(error) - return errors - -# Optimize starting_angle using least_squares (Levenberg-Marquardt) -print("Calculating optimal starting angle...") -result = least_squares(residuals, [initial_guess], method='lm') -optimal_angle = result.x[0] - -print("\n" + "="*60) -print(" CALIBRATION RESULTS") -print("="*60) -print(f"\nOld starting angle: {initial_guess:.3f}°") -print(f"New starting angle: {optimal_angle:.3f}°") - -# Show prediction errors -print(f"\nVerification:") -print(f"{'Point':<8} {'Angle':<12} {'Actual':<10} {'Predicted':<12} {'Error':<10}") -print("-" * 60) - -total_squared_error = 0 -for i, (angle_deg, actual_height) in enumerate(calibration_data, 1): - angle_rad = math.radians(optimal_angle + angle_deg) - predicted_height = (link_length * math.cos(math.radians(optimal_angle))) - \ - (link_length * math.cos(angle_rad)) - error = predicted_height - actual_height - total_squared_error += error**2 - print(f"{i:<8} {angle_deg:<12.5f} {actual_height:<10.3f} {predicted_height:<12.3f} {error:+10.3f}") - -rms_error = math.sqrt(total_squared_error / len(calibration_data)) -print("-" * 60) -print(f"RMS Error: {rms_error:.4f} mm") - -print(f"\n✓ Update self.starting_angle = {optimal_angle:.3f} in your code") +#!/usr/bin/env python3 +""" +Offline calibration script to calculate optimal starting_angle from collected data +Author: Swaraj Dangare +""" + +import math +from scipy.optimize import least_squares + +# Calibration data: (angle_deg, actual_height_mm) +calibration_data = [ + (359.99749, 0.00), + (0.86023, 0.5), + (1.69302, 1.005), + (1.79689, 1.05), + (1.99437, 1.180), + (4.98561, 3.00), + (8.52139, 5.185), + (12.08255, 7.50), + (15.24499, 9.055), + (22.52680, 14.455), +] + +# System parameters +link_length = 40.0 # mm +initial_guess = 59.2 # degrees (current starting_angle) + +def residuals(starting_angle_guess): + """Calculate residuals (errors) for least_squares optimization""" + errors = [] + for angle_deg, actual_height in calibration_data: + # Calculate predicted height + angle_rad = math.radians(starting_angle_guess[0] + angle_deg) + predicted_height = (link_length * math.cos(math.radians(starting_angle_guess[0]))) - \ + (link_length * math.cos(angle_rad)) + error = predicted_height - actual_height + errors.append(error) + return errors + +# Optimize starting_angle using least_squares (Levenberg-Marquardt) +print("Calculating optimal starting angle...") +result = least_squares(residuals, [initial_guess], method='lm') +optimal_angle = result.x[0] + +print("\n" + "="*60) +print(" CALIBRATION RESULTS") +print("="*60) +print(f"\nOld starting angle: {initial_guess:.3f}°") +print(f"New starting angle: {optimal_angle:.3f}°") + +# Show prediction errors +print(f"\nVerification:") +print(f"{'Point':<8} {'Angle':<12} {'Actual':<10} {'Predicted':<12} {'Error':<10}") +print("-" * 60) + +total_squared_error = 0 +for i, (angle_deg, actual_height) in enumerate(calibration_data, 1): + angle_rad = math.radians(optimal_angle + angle_deg) + predicted_height = (link_length * math.cos(math.radians(optimal_angle))) - \ + (link_length * math.cos(angle_rad)) + error = predicted_height - actual_height + total_squared_error += error**2 + print(f"{i:<8} {angle_deg:<12.5f} {actual_height:<10.3f} {predicted_height:<12.3f} {error:+10.3f}") + +rms_error = math.sqrt(total_squared_error / len(calibration_data)) +print("-" * 60) +print(f"RMS Error: {rms_error:.4f} mm") + +print(f"\n✓ Update self.starting_angle = {optimal_angle:.3f} in your code") diff --git a/setup.sh b/setup.sh index c03383e..372c650 100755 --- a/setup.sh +++ b/setup.sh @@ -1,43 +1,43 @@ -#!/bin/bash -# ESP_Encoder - Environment Setup Script -# Sets up a shared Python virtual environment for all projects in this repo - -set -e - -echo "=== ESP Encoder Setup ===" -echo - -# Check if Python 3 is available -if ! command -v python3 &> /dev/null; then - echo "Error: python3 is not installed" - exit 1 -fi - -PYTHON_VERSION=$(python3 --version) -echo "Found: $PYTHON_VERSION" - -# Create virtual environment -if [ -d ".venv" ]; then - echo "Removing existing .venv..." - rm -rf .venv -fi - -echo "Creating virtual environment..." -python3 -m venv .venv - -# Activate and install -echo "Installing dependencies..." -.venv/bin/pip install --upgrade pip -.venv/bin/pip install -r requirements.txt - -source .venv/bin/activate - -echo -echo "=== Setup Complete ===" -echo -echo "To activate the environment:" -echo " source .venv/bin/activate" -echo -echo "Then navigate to any project directory to run its scripts." -echo "See each project's README for usage details." -echo +#!/bin/bash +# ESP_Encoder - Environment Setup Script +# Sets up a shared Python virtual environment for all projects in this repo + +set -e + +echo "=== ESP Encoder Setup ===" +echo + +# Check if Python 3 is available +if ! command -v python3 &> /dev/null; then + echo "Error: python3 is not installed" + exit 1 +fi + +PYTHON_VERSION=$(python3 --version) +echo "Found: $PYTHON_VERSION" + +# Create virtual environment +if [ -d ".venv" ]; then + echo "Removing existing .venv..." + rm -rf .venv +fi + +echo "Creating virtual environment..." +python3 -m venv .venv + +# Activate and install +echo "Installing dependencies..." +.venv/bin/pip install --upgrade pip +.venv/bin/pip install -r requirements.txt + +source .venv/bin/activate + +echo +echo "=== Setup Complete ===" +echo +echo "To activate the environment:" +echo " source .venv/bin/activate" +echo +echo "Then navigate to any project directory to run its scripts." +echo "See each project's README for usage details." +echo