Stay There

// P5.js Code

let eyes = [];

function setup() {
  createCanvas(windowWidth, windowHeight);
  background(0);
  for (let i = 0; i < 50; i++) {
    let size = random(20, 100);
    let x = random(size, width - size);
    let y = random(size, height - size);
    let eye = new Eye(x, y, size);
    for (let j = 0; j < eyes.length; j++) {
      let d = dist(x, y, eyes[j].x, eyes[j].y);
      if (d < size + eyes[j].size) {
        x = random(size, width - size);
        y = random(size, height - size);
        eye.x = x;
        eye.y = y;
        j = -1;
      }
    }
    eyes.push(eye);
  }
  serial = new p5.SerialPort();
  serial.open("/dev/ttyUSB0");
  serial.on("data", serialEvent);
}

function serialEvent() {
  let data = serial.readStringUntil("\n");
  if (data) {
    let location = split(data, ",");
    if (location.length == 2) {
      let x = map(location[0], 0, 1023, 0, width);
      let y = map(location[1], 0, 1023, 0, height);
      for (let i = 0; i < eyes.length; i++) {
        eyes[i].update(x, y);
      }
    }
  }
}

function draw() {
  for (let i = 0; i < eyes.length; i++) {
    eyes[i].display();
  }
}

class Eye {
  constructor(x, y, size) {
    this.x = x;
    this.y = y;
    this.size = size;
    this.pupilSize = this.size / 3;
    this.pupilX = this.x;
    this.pupilY = this.y;
    this.angle = 0;
  }

  update(x, y) {
    let d = dist(x, y, this.x, this.y);
    let maxDist = this.size / 2 - this.pupilSize / 2;
    let mappedDist = map(d, 0, maxDist, 0, 1);
    let angle = atan2(y - this.y, x - this.x);
    angle = constrain(angle, -PI / 4, PI / 4);
    this.angle = lerp(this.angle, angle, 0.1);
    this.pupilX = this.x + cos(this.angle) * mappedDist * maxDist;
    this.pupilY = this.y + sin(this.angle) * mappedDist * maxDist;
  }

  display() {
    noStroke();
    fill(255);
    ellipse(this.x, this.y, this.size);
    fill(0);
    ellipse(this.pupilX, this.pupilY, this.pupilSize);
  }
}

//Nano BLE - UWB Sender Code

#include <ArduinoBLE.h>

 // BLE POS Service
BLEService POSService("11a953ef-0809-4d89-9a56-25ab22841ecd");

// BLE Orientation Characteristic
BLEStringCharacteristic POSSensorData("e624a94b-4a14-41f3-ae53-ec99c6025855", BLERead | BLENotify, 512);

unsigned long previousTime=0;           // end time of the last period in microseconds
unsigned long currentTime=0;
unsigned long deltaTime=0;
const unsigned long period=1000;       // time period for pulse counting in microseconds

String c; //some temporary string variable

char s[60]; //snprintf buffer
String pos;

void setup() 
{
  delay(10000);
  Serial.begin(115200);
  Serial1.begin(115200); //dwm1001 require 115200 baud
  Serial1.write(0x0D); //send the first "Enter"
  Serial1.write(0x0D); //send the second "Enter" to get into Shell Mode
  c = Serial1.readString(); //read the response from dwm1001
  Serial1.write(0x6C); //send "l"
  Serial1.write(0x65); //send "e"
  //Serial1.write(0x63); //send "c" lec shows distance to all anchors & position
  Serial1.write(0x70); //send "p" lep shows only position
  Serial1.write(0x0D); //send "Enter"

  // initialize the built-in LED pin to indicate when a central is connected
  pinMode(LED_BUILTIN, OUTPUT); 

  // begin BLE initialization
  if (!BLE.begin()) {
    Serial.println("starting BLE failed!");
    while (1);
  }

  /* Set a local name for the BLE device
     This name will appear in advertising packets
     and can be used by remote devices to identify this BLE device
     The name can be changed but maybe be truncated based on space left in advertisement packet
  */
  BLE.setLocalName("POSSensor");
  BLE.setAdvertisedService(POSService); // add the service UUID
  POSService.addCharacteristic(POSSensorData); // add the POS characteristic
  BLE.addService(POSService); // Add the POS service
  POSSensorData.writeValue("hello"); // set initial value for this characteristic

  /* Start advertising BLE.  It will start continuously transmitting BLE
     advertising packets and will be visible to remote BLE central devices
     until it receives a new connection */

  // start advertising
  BLE.advertise();

  Serial.println("Bluetooth device active, waiting for connections...");  
  String address = BLE.address();
  Serial.print("Local address is: ");
  Serial.println(address);  
  Serial.flush();  
} 

void loop() {
  // wait for a BLE central
  BLEDevice central = BLE.central();

  // if a central is connected to the peripheral:
  if (central) {
    Serial.print("Connected to central: ");
    // print the central's BT address:
    Serial.println(central.address());
    // turn on the LED to indicate the connection:
    digitalWrite(LED_BUILTIN, HIGH);  

    // check the POS every period
    // while the central is connected:
    while (central.connected()) {
      currentTime = micros();
      deltaTime = currentTime-previousTime;   

      /* Check if the time in between samples equals sampling period 
         before taking a new sample */
      if (deltaTime >= period) {
        /* Display the current time in microseconds */
        Serial.print(currentTime);
        Serial.print(",");
        previousTime = currentTime;         

        // Stores each POS data type in a string 'buff' // 
        String buff;
        pos = Serial1.readStringUntil('\n');

        // Sends buff using the BLE connection
        buff += String(pos);
        Serial.println(buff);
        POSSensorData.writeValue(buff);
      }
    }
  }
        
  // when the central disconnects, turn off the LED:
  digitalWrite(LED_BUILTIN, LOW);
  Serial.print("Disconnected from central: ");
  Serial.println(central.address());  
}

//Nano BLE - UWB Receiver Code

#include <ArduinoBLE.h>
#include <Servo.h>
#include <Adafruit_NeoPixel.h>

Servo myservo;  // create servo object to control a servo

//int pos0 = 0;    // variable to store the servo position
int pos = 0;
int servo = 3;


#define LED_PIN D3
#define LED_COUNT 16

Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

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

  myservo.attach(A2);  // attaches the servo on pin 9 to the servo object  

  strip.begin();
  strip.show();
  strip.setBrightness(100);

  // begin initialization
  if (!BLE.begin()) {
    Serial.println("starting BLE failed!");

    while (1);
  }

  Serial.println("BLE Central - Receiver");
  Serial.println("Make sure to turn on the device.");

  // start scanning for peripheral
  BLE.scan();
  pinMode(LEDR, OUTPUT);
  pinMode(LEDG, OUTPUT);
  pinMode(LEDB, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);

  digitalWrite(LED_BUILTIN, LOW);         
  digitalWrite(LEDR, HIGH);     
  digitalWrite(LEDG, HIGH);     
  digitalWrite(LEDB, HIGH);       
}

void loop() {
  // check if a peripheral has been discovered
  BLEDevice peripheral = BLE.available();

  if (peripheral) {
    // discovered a peripheral, print out address, local name, and advertised service
    Serial.print("Found ");
    Serial.print(peripheral.address());
    Serial.print(" '");
    Serial.print(peripheral.localName());
    Serial.print("' ");
    Serial.print(peripheral.advertisedServiceUuid());
    Serial.println();

    setRingColor(false);
    delay(200);
    setRingColor(true);
    delay(200);
    // Check if the peripheral is a POSSensor, the local name will be:
    // "POSSensor"
    if (peripheral.localName() == "POSSensor2") {  
      // stop scanning
      BLE.stopScan();

      monitorBLEperipheral(peripheral);

      // peripheral disconnected, start scanning again
      BLE.scan();
    }
  }
}

void monitorBLEperipheral(BLEDevice peripheral) {
  // connect to the peripheral
  Serial.println("Connecting ...");
  if (peripheral.connect()) {
    Serial.println("Connected");
  } else {
    Serial.println("Failed to connect!");
    return;
  }

  // discover peripheral attributes
  Serial.println("Discovering service 0xffe0 ...");
  if (peripheral.discoverService("11a953ef-0809-4d89-9a56-25ab22841ttt")) {
    Serial.println("Service discovered");
  } else {
    Serial.println("Attribute discovery failed.");
    peripheral.disconnect();

    while (1);
    return;
  }

  // retrieve the POSSensorData characteristic
  BLECharacteristic POSSensorData2 = peripheral.characteristic("e624a94b-4a14-41f3-ae53-ec99c6025999");
  
  // subscribe to the simple key characteristic
  Serial.println("Subscribing to POSSensorData characteristic ...");
  if (!POSSensorData2) {
    Serial.println("no POSSensorData characteristic found!");
    peripheral.disconnect();
    return;
  } 
    else if (!POSSensorData2.canSubscribe()) {
    Serial.println("POSSensorData characteristic is not subscribable!");
    peripheral.disconnect();
    return;
  } 
    else if (!POSSensorData2.subscribe()) {
    Serial.println("subscription failed!");
    peripheral.disconnect();
    return;
  } 
    else {
    Serial.println("Subscribed to POSSensorData characteristic");
  }

  while (peripheral.connected()) {
    // while the peripheral is connected

    // check if the value of the simple key characteristic has been updated
    if (POSSensorData2.valueUpdated()) {

// THIS SECTION CONVERTS THE RECEIVED CHARACTERISTIC FROM UNSIGNED CHAR TO STRING // 
      String str;
      int length = POSSensorData2.valueLength();
      const uint8_t* val = POSSensorData2.value();
      str.reserve(length);

      for (int i = 0; i<length; i++){
        str += (char)val[i];
      }

      Serial.println(str);

      if (str.indexOf("POS") != -1) {
        int posIndex = str.indexOf("POS");
        int firstComma = str.indexOf(',', posIndex + 1);
        int secondComma = str.indexOf(',', firstComma + 1);
        int thirdComma = str.indexOf(',', secondComma + 1);

        float x = str.substring(firstComma + 1, secondComma).toFloat();
        float y = str.substring(secondComma + 1, thirdComma).toFloat();
        float z = str.substring(thirdComma + 1, str.indexOf(',', thirdComma + 1)).toFloat();        
        
        pos = 180 - x/2.85 * 180;
        myservo.write(pos);
        delay(100);

void setRingColor(bool off) {
  for (int i = 0; i < strip.numPixels(); i++) {
    if (pos/3 < pos< pos *2/3 ) {
           strip.setPixelColor(i, strip.Color(off ? 0 : 0, 0, 255));
        }
        else {
            strip.setPixelColor(i, strip.Color(off ? 0 : 0, 0, 255));
        }
      strip.show();  
  }
        

        // if (0 <= x && x <= 2.85 && 0 <= y && y <= 3.15) {
        //   digitalWrite(LED_BUILTIN, HIGH);
        //   digitalWrite(LEDR, HIGH);
        //   digitalWrite(LEDG, HIGH);
        //   digitalWrite(LEDB, HIGH);
        // }

        // if (0 <= x && x <= 2.85 && 3.15 <= y && y <= 6.3) {
        //   digitalWrite(LED_BUILTIN, LOW);
        //   digitalWrite(LEDR, LOW);
        //   digitalWrite(LEDG, HIGH);
        //   digitalWrite(LEDB, HIGH);
        // }

        // if (2.85 <= x && x <= 5.7 && 0 <= y && y <= 3.15) {
        //   digitalWrite(LED_BUILTIN, LOW);
        //   digitalWrite(LEDR, HIGH);
        //   digitalWrite(LEDG, LOW);
        //   digitalWrite(LEDB, HIGH);
        // }

        // if (2.85 <= x && x <= 5.7 && 3.15 <= y && y <= 6.3) {
        //   digitalWrite(LED_BUILTIN, LOW);
        //   digitalWrite(LEDR, HIGH);
        //   digitalWrite(LEDG, HIGH);
        //   digitalWrite(LEDB, LOW);
        // }
        
        Serial.print("x: ");
        Serial.print(x);
        Serial.print(", y: ");
        Serial.print(y);
        Serial.print(", z: ");
        Serial.println(z);
      } else {
        Serial.println("POS not found in the data string");
      }      
    }
  }
  

  Serial.println("BLE disconnected!");
}