c2-utopia/src/sprejemnik-olimex.cpp

#include <Arduino.h>
#include <esp_now.h>
#include <esp_wifi.h>
#include <WiFi.h>

// WIFI paket
#include "sensor_msg.h"

#include <OSCBundle.h>
#include <OSCBoards.h>

#include <Adafruit_NeoPixel.h>

//#define DEBUG

// RGB LED
#define NUMPIXELS 1
#define PIXELPIN 18
Adafruit_NeoPixel pixels(NUMPIXELS, PIXELPIN, NEO_GRB + NEO_KHZ800);

#include "SLIPEncodedSerial.h"
SLIPEncodedUSBSerial SLIPSerial(Serial);
 
// Set your new MAC Address
// MAC naslov sprejemnika: 08:3A:F2:50:EF:6C
uint8_t newMACAddress[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C};

// Maksimalno stevilo
#define ST_SPREJEMNIKOV 10

int odcitekId;
sensor_msg  odcitki[ST_SPREJEMNIKOV];
bool poslji[ST_SPREJEMNIKOV];

void prejemPodatkov(const uint8_t * mac_addr, const uint8_t * noviPodatki, int len) {
  
  char macNaslov[18];
  snprintf(macNaslov, sizeof(macNaslov), "%02x:%02x:%02x:%02x:%02x:%02x",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  
  #ifdef DEBUG
  Serial.print("Prejel podatke od ");
  Serial.println(macNaslov);
  #endif

  // ID odcitka je na prvem mestu!
  memcpy(&odcitekId, noviPodatki, sizeof(uint8_t));
  
  #ifdef DEBUG
  Serial.print("RAZBIRAM ID ");
  Serial.println(odcitekId);
  #endif
  
  memcpy(&odcitki[odcitekId], noviPodatki, sizeof(sensor_msg));

  #ifdef DEBUG
  Serial.printf("aX: %i \n", odcitki[odcitekId].aX);
  Serial.printf("aY: %i \n", odcitki[odcitekId].aY);
  Serial.printf("aZ: %i \n", odcitki[odcitekId].aZ);
  Serial.printf("qX: %f \n", odcitki[odcitekId].qX);
  Serial.printf("qY: %f \n", odcitki[odcitekId].qY);
  Serial.printf("qZ: %f \n", odcitki[odcitekId].qZ);
  Serial.printf("qW: %f \n", odcitki[odcitekId].qW);
  Serial.println();
  #endif  

  poslji[odcitekId] = true;
}

void setup() {
  // Nizja CPU frekvenca
  //setCpuFrequencyMhz(80);
  SLIPSerial.begin(115200);

  // Ne posiljaj preden se podatki napolnijo
  for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
    poslji[i] = false;
  }

  // Init - 3 one second blinks
  pixels.begin();
  pixels.clear();
  pixels.setPixelColor(0, pixels.Color(0, 255, 0));
  pixels.setBrightness(100);
  for (int i = 0; i < 3; i++) {

    #ifdef DEBUG
    Serial.println(i + 1);
    #endif

    pixels.clear();
    delay(500);
    pixels.show();
    delay(500);
  }

  // vklopi LED!
  pixels.setPixelColor(0, pixels.Color(255, 255, 255));
  pixels.show();

  Serial.println("Inicializiram WIFI...");
  WiFi.mode(WIFI_STA);

  esp_wifi_set_mac(WIFI_IF_STA, &newMACAddress[0]);
  
  esp_err_t result = esp_now_init();
  if (result != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    Serial.println(result);
    pixels.setPixelColor(0, pixels.Color(255, 0, 0));
    return;
  }
  Serial.print("MAC naslov: ");
  Serial.println(WiFi.macAddress());
  esp_now_register_recv_cb(prejemPodatkov);
}

void loop() {
  /* OSC MSG channels */
  OSCBundle bundle;
  char glava[32];

  for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
    if (poslji[i]) {
      // Accel in quaternion v bundlu skupaj z ostalim sta velika 396 bytov!
      sprintf(glava, "/ww/%d/acc", i); 
      /*
      Serial.print("Posiljam ");
      Serial.println(glava);
      */
      bundle.add(glava)
        .add(odcitki[i].aX)
        .add(odcitki[i].aY)
        .add(odcitki[i].aZ);

      sprintf(glava, "/ww/%d/quat", i);
      /*
      Serial.print("Posiljam ");
      Serial.println(glava);
      */
      bundle.add(glava)
        .add(odcitki[i].qW)
        .add(odcitki[i].qX)
        .add(odcitki[i].qY)
        .add(odcitki[i].qZ);

      /*
      Serial.printf("XaX: %i \n", odcitki[i].aX);
      Serial.printf("XaY: %i \n", odcitki[i].aY);
      Serial.printf("XaZ: %i \n", odcitki[i].aZ);
      Serial.printf("XqX: %f \n", odcitki[i].qX);
      Serial.printf("XqY: %f \n", odcitki[i].qY);
      Serial.printf("XqZ: %f \n", odcitki[i].qZ);
      Serial.printf("XqW: %f \n", odcitki[i].qW);
      Serial.println();
      */

      sprintf(glava, "/ww/%d/bat", i);
      bundle.add(glava)
        .add(odcitki[i].bat);

      SLIPSerial.beginPacket();
      bundle.send(SLIPSerial);
      SLIPSerial.endPacket();
      
      bundle.empty();
      poslji[i] = false;
    }
  }
  //delay(10);
}