Hello everyone, need some serious help here. I am currently working on my project which is human following robot and for that I am using DW1000 UWB technology. What I have done up till now is that I have calibrated each of three anchors antenna delay with the tag and I ca see all three distances from each anchor to the tag when opening serial monitor of tag. Then I send all these three distances from tag to the third anchor’ microcontroller using ESP NOW. In my case each anchor has its own esp32 and specifically talking about third anchor’s microcontroller I thought it will be a good idea to use esp32 of third anchor as a main controller board of my robot. So, this controller will have a third anchor and basic functioning of robot.
What I want is that now as all three distances are available on single microcontroller I want to do localization to know the exact position of tag. I tried using various AI generated codes for localization but failed to do so. So anyone please help me so that I can do this localization. Any code, recourse will be appreciated. The code of anchor3 and tag is shown below. Thanks in advance. Waiting for someone’s help…
Anchor3 Code:
/**************************************************************
- Anchor #3 + Main Controller
-
- Acts as a DW1000 anchor (ID = 85).
-
- Receives 3 distances from the Tag (via ESP-NOW).
-
- In the future, you can add robot motor control,
- obstacle avoidance, etc. in this code.
**************************************************************/
#include <SPI.h>
#include “DW1000Ranging.h”
#include “DW1000.h”
#include <WiFi.h>
#include <esp_now.h>
// ---------------------
// DW1000 Setup
// ---------------------
char anchor_addr = “85:00:5B:D5:A9:9A:E2:9C”;
uint16_t Adelay = 16608;
#define SPI_SCK 18
#define SPI_MISO 19
#define SPI_MOSI 23
#define DW_CS 5
const uint8_t PIN_RST = 16;
const uint8_t PIN_IRQ = 17;
const uint8_t PIN_SS = 5;
// If you still want to see the local anchor #3 distance on this board:
void newRange() {
float dist = DW1000Ranging.getDistantDevice()->getRange();
Serial.print("Anchor #3 local distance to tag: ");
Serial.println(dist);
}
void newDevice(DW1000Device *device) {
Serial.print("Anchor #3 new device: ");
Serial.println(device->getShortAddress(), HEX);
}
void inactiveDevice(DW1000Device *device) {
Serial.print("Anchor #3 inactive device: ");
Serial.println(device->getShortAddress(), HEX);
}
// ---------------------
// ESP-NOW Setup
// ---------------------
// A struct for receiving all 3 distances in one packet.
typedef struct {
float distAnchor83;
float distAnchor84;
float distAnchor85;
} DistancesPacket;
// We’ll store the received distances here.
float distance83 = 0.0;
float distance84 = 0.0;
float distance85 = 0.0;
// Callback: runs whenever the Tag sends the updated distances.
void onDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
DistancesPacket packet;
memcpy(&packet, incomingData, sizeof(packet));
distance83 = packet.distAnchor83;
distance84 = packet.distAnchor84;
distance85 = packet.distAnchor85;
Serial.println(“=== Received 3 distances from Tag ===”);
Serial.print("Anchor 83: ");
Serial.println(distance83);
Serial.print("Anchor 84: ");
Serial.println(distance84);
Serial.print("Anchor 85: ");
Serial.println(distance85);
// TODO: Here you can add your localization/trilateration,
// motor control, obstacle avoidance, etc.
}
void setup() {
Serial.begin(115200);
delay(1000);
Serial.println(“=== Anchor #3 (Main Controller) ===”);
// 1) Initialize DW1000 (Anchor #3)
SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI);
DW1000Ranging.initCommunication(PIN_RST, PIN_SS, PIN_IRQ);
DW1000.setAntennaDelay(Adelay);
DW1000Ranging.attachNewRange(newRange);
DW1000Ranging.attachNewDevice(newDevice);
DW1000Ranging.attachInactiveDevice(inactiveDevice);
DW1000Ranging.startAsAnchor(anchor_addr, DW1000.MODE_LONGDATA_RANGE_LOWPOWER, false);
// 2) Initialize ESP-NOW to receive distances from the Tag
WiFi.mode(WIFI_STA);
if (esp_now_init() != ESP_OK) {
Serial.println(“Error initializing ESP-NOW”);
return;
}
esp_now_register_recv_cb(onDataRecv);
}
void loop() {
// Keep measuring local distance if you want (anchor #3 → tag).
DW1000Ranging.loop();
// For demonstration, print all 3 distances once per second
static unsigned long lastPrint = 0;
if (millis() - lastPrint > 1000) {
lastPrint = millis();
Serial.print(“[Periodic] A83=”);
Serial.print(distance83);
Serial.print(" m, A84=“);
Serial.print(distance84);
Serial.print(” m, A85=“);
Serial.print(distance85);
Serial.println(” m");
}
}
Tag Code:
/**************************************************************
- TAG Code
-
- Measures distance to each of the 3 anchors (83,84,85).
-
- Collects all 3 distances in a struct.
-
- Sends them (via ESP-NOW) to Anchor #3’s MAC address.
**************************************************************/
- Sends them (via ESP-NOW) to Anchor #3’s MAC address.
#include <SPI.h>
#include “DW1000Ranging.h”
#include “DW1000.h”
#include <WiFi.h>
#include <esp_now.h>
// ---------------------
// DW1000 Setup
// ---------------------
#define SPI_SCK 18
#define SPI_MISO 19
#define SPI_MOSI 23
#define DW_CS 5
const uint8_t PIN_RST = 16;
const uint8_t PIN_IRQ = 17;
const uint8_t PIN_SS = 5;
// Tag short address
char tag_addr = “02:00:00:00:00:00:10:01”;
// Per-Anchor Moving Average Configuration
#define MAX_ANCHORS 3 // Maximum number of anchors
#define WINDOW_SIZE 15 // Adjust as needed
#define MEDIAN_WINDOW 5
float distanceBuffer[WINDOW_SIZE]; // Buffer to hold historical data
// We’ll store the distances for anchors 83, 84, 85
float distAnchor83 = 0.0;
float distAnchor84 = 0.0;
float distAnchor85 = 0.0;
typedef struct {
uint16_t shortAddress;
float buffer[WINDOW_SIZE];
int index;
bool filled;
float medianHist[MEDIAN_WINDOW];
int medIndex;
} AnchorFilter;
AnchorFilter anchorFilters[MAX_ANCHORS];
int numAnchors = 0;
// ---------------------
// ESP-NOW Setup
// ---------------------
// A struct to send all three distances in one packet.
typedef struct {
float distAnchor83;
float distAnchor84;
float distAnchor85;
} DistancesPacket;
DistancesPacket packetToSend;
// >>> REPLACE WITH ANCHOR #3’s ACTUAL MAC ADDRESS <<<
uint8_t anchor3MAC = {0xCC, 0x7B, 0x5C, 0xF1, 0xF5, 0xBC}; // Example placeholder
// Optional: Callback to see if the packet was sent successfully
void onDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
// You can uncomment for debugging:
// Serial.print("ESP-NOW Send Status: ");
// Serial.println(status == ESP_NOW_SEND_SUCCESS ? “Success” : “Fail”);
}
void setup()
{
Serial.begin(115200);
delay(1000);
// 1) Initialize DW1000 for Tag
SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI);
DW1000Ranging.initCommunication(PIN_RST, PIN_SS, PIN_IRQ);
DW1000Ranging.attachNewRange(newRange);
DW1000Ranging.attachNewDevice(newDevice);
DW1000Ranging.attachInactiveDevice(inactiveDevice);
// Start as tag
DW1000Ranging.startAsTag(tag_addr, DW1000.MODE_LONGDATA_RANGE_LOWPOWER, false);
DW1000.setChannel(2);
// DW1000.setPulseFrequency(DW1000.PULSE_FREQ_16MHZ);
// Initialize buffer with zeros
for (int i = 0; i < WINDOW_SIZE; i++) {
distanceBuffer[i] = 0.0;
}
// 2) Initialize ESP-NOW for sending data to Anchor #3
WiFi.mode(WIFI_STA); // Station mode
if (esp_now_init() != ESP_OK) {
Serial.println(“Error initializing ESP-NOW”);
return;
}
esp_now_register_send_cb(onDataSent);
// Add Anchor #3 as a peer
esp_now_peer_info_t peerInfo;
memcpy(peerInfo.peer_addr, anchor3MAC, 6);
peerInfo.channel = 0;
peerInfo.encrypt = false;
if (esp_now_add_peer(&peerInfo) != ESP_OK) {
Serial.println(“Failed to add peer (Anchor #3)”);
return;
}
}
float getMedian(float* arr, int size) {
float temp[size];
memcpy(temp, arr, sizeof(temp));
std::sort(temp, temp + size);
return temp[size/2];
}
void loop()
{
// Continuously process DW1000 ranging
DW1000Ranging.loop();
}
// Called whenever a new distance measurement is ready
void newRange()
{
// Identify which anchor this measurement is from
DW1000Device* dev = DW1000Ranging.getDistantDevice();
uint16_t shortAddr = dev->getShortAddress();
float rawDistance = dev->getRange();
// // Print for debugging
// Serial.print(shortAddr, HEX);
// Serial.print(", ");
// Serial.println(distance);
// Median filtering
static float medianHistory[MAX_ANCHORS][MEDIAN_WINDOW];
static int medIndex[MAX_ANCHORS] = {0};
int anchorIdx = -1;
for(int i=0; i<numAnchors; i++) {
if(anchorFilters[i].shortAddress == shortAddr) {
anchorIdx = i;
break;
}
}
if(anchorIdx != -1) {
medianHistory[anchorIdx][medIndex[anchorIdx]] = rawDistance;
medIndex[anchorIdx] = (medIndex[anchorIdx] + 1) % MEDIAN_WINDOW;
rawDistance = getMedian(medianHistory[anchorIdx], MEDIAN_WINDOW);
}
// Find or create filter for this anchor
int filterIndex = -1;
for(int i=0; i<numAnchors; i++) {
if(anchorFilters[i].shortAddress == shortAddr) {
filterIndex = i;
break;
}
}
if(filterIndex == -1 && numAnchors < MAX_ANCHORS) {
filterIndex = numAnchors++;
anchorFilters[filterIndex].shortAddress = shortAddr;
memset(anchorFilters[filterIndex].buffer, 0, sizeof(float)*WINDOW_SIZE);
}
if(filterIndex != -1) {
// Update buffer for this anchor
AnchorFilter* filter = &anchorFilters[filterIndex];
filter->buffer[filter->index] = rawDistance;
filter->index = (filter->index + 1) % WINDOW_SIZE;
// Calculate moving average
float avg = 0.0;
int count = filter->filled ? WINDOW_SIZE : filter->index;
for(int i=0; i<count; i++) {
avg += filter->buffer[i];
}
avg /= count;
if(!filter->filled && filter->index == 0) {
filter->filled = true;
}
// Print results
Serial.print(shortAddr, HEX);
Serial.print(",");
Serial.print(avg);
Serial.println(" m");
if (shortAddr == 0x83) {
distAnchor83 = avg;
} else if (shortAddr == 0x84) {
distAnchor84 = avg;
} else if (shortAddr == 0x85) {
distAnchor85 = avg;
}
}
// Store this distance based on anchor ID
// (In your case, anchor short addresses are: 0x835B, 0x845B, 0x855B, etc.
// But let’s do a partial match for the last two digits or a known pattern.)
// The short address you see might be truncated from your “83:00:5B:D5:…” etc.
// Adjust the if-conditions to match your actual short addresses.
// Prepare the struct with all three distances
packetToSend.distAnchor83 = distAnchor83;
packetToSend.distAnchor84 = distAnchor84;
packetToSend.distAnchor85 = distAnchor85;
// Send the entire struct to Anchor #3
esp_now_send(anchor3MAC, (uint8_t*)&packetToSend, sizeof(packetToSend));
}
void newDevice(DW1000Device *device)
{
Serial.print("Tag sees new device: ");
Serial.println(device->getShortAddress(), HEX);
}
void inactiveDevice(DW1000Device *device)
{
Serial.print("Tag sees inactive device: ");
Serial.println(device->getShortAddress(), HEX);
}