124 lines
3.3 KiB
C++
124 lines
3.3 KiB
C++
#include <esp_now.h>
|
||
#include <esp_wifi.h>
|
||
#include <WiFi.h>
|
||
|
||
#include "RTMath.h"
|
||
|
||
#include <OSCBundle.h>
|
||
#include <OSCBoards.h>
|
||
|
||
#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 {
|
||
int id;
|
||
RTFLOAT aX;
|
||
RTFLOAT aY;
|
||
RTFLOAT aZ;
|
||
RTFLOAT qX;
|
||
RTFLOAT qY;
|
||
RTFLOAT qZ;
|
||
RTFLOAT qW;
|
||
}sensor_msg;
|
||
#include <SLIPEncodedSerial.h>
|
||
|
||
sensor_msg odcitek;
|
||
|
||
sensor_msg odcitekA;
|
||
sensor_msg odcitekB;
|
||
|
||
#define ST_KEGLOV 2
|
||
sensor_msg odcitki[ST_KEGLOV] = {odcitekA, odcitekB};
|
||
bool poslji[ST_KEGLOV] = {false, false};
|
||
|
||
void prejemPodatkov(const uint8_t * mac_addr, const uint8_t * drugiPodatki, int len) {
|
||
char macNaslov[18];
|
||
//Serial.print("Prejel podatke od ");
|
||
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]);
|
||
//Serial.println(macNaslov);
|
||
|
||
memcpy(&odcitek, drugiPodatki, sizeof(odcitek));
|
||
|
||
//Serial.printf("Board ID %u: %u bytes\n", odcitek.id, len);
|
||
// Update the structures with the new incoming data
|
||
odcitki[odcitek.id-1].aX = odcitek.aX;
|
||
odcitki[odcitek.id-1].aY = odcitek.aY;
|
||
odcitki[odcitek.id-1].aZ = odcitek.aZ;
|
||
odcitki[odcitek.id-1].qX = odcitek.qX;
|
||
odcitki[odcitek.id-1].qY = odcitek.qY;
|
||
odcitki[odcitek.id-1].qZ = odcitek.qZ;
|
||
odcitki[odcitek.id-1].qW = odcitek.qW;
|
||
/*
|
||
Serial.printf("aX: %f \n", odcitki[odcitek.id-1].aX);
|
||
Serial.printf("aY: %f \n", odcitki[odcitek.id-1].aY);
|
||
Serial.printf("aZ: %f \n", odcitki[odcitek.id-1].aZ);
|
||
Serial.printf("qX: %f \n", odcitki[odcitek.id-1].qX);
|
||
Serial.printf("qY: %f \n", odcitki[odcitek.id-1].qY);
|
||
Serial.printf("qZ: %f \n", odcitki[odcitek.id-1].qZ);
|
||
Serial.printf("qW: %f \n", odcitki[odcitek.id-1].qW);
|
||
Serial.println();
|
||
*/
|
||
poslji[odcitek.id-1] = true;
|
||
}
|
||
void setup() {
|
||
// put your setup code here, to run once:
|
||
//Serial.begin(115200);
|
||
SLIPSerial.begin(115200);
|
||
|
||
Serial.println("Inicializiram WIFI...");
|
||
WiFi.mode(WIFI_STA);
|
||
|
||
esp_wifi_set_mac(WIFI_IF_STA, &newMACAddress[0]);
|
||
|
||
if (esp_now_init() != ESP_OK) {
|
||
Serial.println("Error initializing ESP-NOW");
|
||
return;
|
||
}
|
||
Serial.print("MAC naslov: ");
|
||
Serial.println(WiFi.macAddress());
|
||
esp_now_register_recv_cb(prejemPodatkov);
|
||
}
|
||
|
||
/* OSC MSG channels */
|
||
OSCBundle bundle;
|
||
char glava[32];
|
||
|
||
void loop() {
|
||
// put your main code here, to run repeatedly:
|
||
for (int i = 0; i < ST_KEGLOV; i++) {
|
||
if (poslji[i]) {
|
||
sprintf(glava, "/ww/%d/accel", i);
|
||
bundle.add(glava)
|
||
.add(odcitki[i].aX)
|
||
.add(odcitki[i].aY)
|
||
.add(odcitki[i].aZ);
|
||
|
||
sprintf(glava, "/ww/%d/quaternion", i);
|
||
bundle.add(glava)
|
||
.add(odcitki[i].qW)
|
||
.add(odcitki[i].qX)
|
||
.add(odcitki[i].qY)
|
||
.add(odcitki[i].qZ);
|
||
|
||
SLIPSerial.beginPacket();
|
||
bundle.send(SLIPSerial);
|
||
SLIPSerial.endPacket();
|
||
|
||
bundle.empty();
|
||
|
||
poslji[i] = false;
|
||
}
|
||
}
|
||
//delay(10);
|
||
}
|