kegel/espnow_sprejemnik/espnow_sprejemnik.ino

150 lines
3.6 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

#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 dont 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;
int16_t aX;
int16_t aY;
int16_t 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);
}