150 lines
3.6 KiB
C++
150 lines
3.6 KiB
C++
#include <esp_now.h>
|
||
#include <esp_wifi.h>
|
||
#include <WiFi.h>
|
||
|
||
#include <OSCBundle.h>
|
||
#include <OSCBoards.h>
|
||
|
||
//#define DEBUG
|
||
|
||
#ifdef BOARD_HAS_USB_SERIAL
|
||
#include <SLIPEncodedUSBSerial.h>
|
||
SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
|
||
#else
|
||
#include <SLIPEncodedSerial.h>
|
||
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
|
||
#endif
|
||
|
||
// Set your new MAC Address
|
||
// MAC naslov sprejemnika: 08:3A:F2:50:EF:6C
|
||
uint8_t newMACAddress[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C};
|
||
|
||
typedef struct sensor_msg {
|
||
uint8_t id;
|
||
float aX;
|
||
float aY;
|
||
float aZ;
|
||
float qX;
|
||
float qY;
|
||
float qZ;
|
||
float qW;
|
||
} sensor_msg;
|
||
|
||
//sensor_msg odcitek;
|
||
|
||
// Maksimalno stevilo
|
||
#define ST_KEGLOV 10
|
||
|
||
int odcitekId;
|
||
sensor_msg odcitki[ST_KEGLOV];
|
||
bool poslji[ST_KEGLOV];
|
||
|
||
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() {
|
||
SLIPSerial.begin(115200);
|
||
|
||
// Ne posiljaj preden se podatki napolnijo
|
||
for (int i = 0; i < ST_KEGLOV; i++) {
|
||
poslji[0] = false;
|
||
}
|
||
|
||
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);
|
||
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_KEGLOV; i++) {
|
||
if (poslji[i]) {
|
||
sprintf(glava, "/ww/%d/accel", 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/quaternion", 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();
|
||
*/
|
||
|
||
SLIPSerial.beginPacket();
|
||
bundle.send(SLIPSerial);
|
||
SLIPSerial.endPacket();
|
||
|
||
bundle.empty();
|
||
poslji[i] = false;
|
||
}
|
||
}
|
||
//delay(10);
|
||
}
|