master
Jurij Podgoršek 2022-03-05 02:10:27 +01:00
parent 018887a13d
commit e4e7f3b867
1 changed files with 64 additions and 62 deletions

View File

@ -8,8 +8,8 @@
#include <OSCMessage.h> #include <OSCMessage.h>
//#define SERIAL_OSC //#define SERIAL_OSC
//#define WIFI_OSC #define WIFI_OSC
#define BT_OSC //#define BT_OSC
// SERIAL // SERIAL
#ifdef BOARD_HAS_USB_SERIAL #ifdef BOARD_HAS_USB_SERIAL
@ -135,7 +135,7 @@ OSCMessage kmsg("/keys/"); // A B C D E
void setup() { void setup() {
// Basic(debug) serial init // Basic(debug) serial init
// Serial.begin(115200); // set this as high as you can reliably run on your platform // Serial.begin(115200); // set this as high as you can reliably run on your platform
Serial.println("Starting up..."); Serial.println("Starting up...");
// I2C init // I2C init
@ -207,21 +207,21 @@ void setup() {
} }
#ifdef WIFI_OSC #ifdef WIFI_OSC
// WIFI init // WIFI init
Serial.print("Attempting to connect to SSID: "); Serial.print("Attempting to connect to SSID: ");
Serial.println(ssid); Serial.println(ssid);
connectToWiFi(ssid, password); connectToWiFi(ssid, password);
// attempt to connect to Wifi network: // attempt to connect to Wifi network:
while (WiFi.status() != WL_CONNECTED) { while (WiFi.status() != WL_CONNECTED) {
Serial.print("."); Serial.print(".");
// wait 1 second for re-trying // wait 1 second for re-trying
delay(1000); delay(1000);
} }
#endif #endif
#ifdef BT_OSC #ifdef BT_OSC
SerialBT.begin("wavey wind"); SerialBT.begin("wavey wind");
#endif #endif
} }
@ -243,25 +243,26 @@ void loop() {
diff = q.getProduct(pq.getConjugate()); diff = q.getProduct(pq.getConjugate());
qmsg.add(q.w); qmsg.add(q.w);
qmsg.add(q.x);
qmsg.add(q.y); qmsg.add(q.y);
qmsg.add(q.x);
qmsg.add(q.z); qmsg.add(q.z);
qdmsg.add(diff.w); qdmsg.add(diff.w);
qdmsg.add(diff.x);
qdmsg.add(diff.y); qdmsg.add(diff.y);
qdmsg.add(diff.x);
qdmsg.add(diff.z); qdmsg.add(diff.z);
#ifdef SERIAL_OSC #ifdef SERIAL_OSC
SLIPSerial.beginPacket(); SLIPSerial.beginPacket();
qdmsg.send(SLIPSerial); qmsg.send(SLIPSerial);
SLIPSerial.endPacket(); SLIPSerial.endPacket();
SLIPSerial.beginPacket(); SLIPSerial.beginPacket();
qmsg.send(SLIPSerial); qdmsg.send(SLIPSerial);
SLIPSerial.endPacket(); SLIPSerial.endPacket();
#endif #endif
#ifdef WIFI_OSC #ifdef WIFI_OSC
udp.beginPacket(castIp, port); udp.beginPacket(castIp, port);
qmsg.send(udp); qmsg.send(udp);
@ -272,39 +273,40 @@ void loop() {
udp.endPacket(); udp.endPacket();
#endif #endif
// Some bug below, it seems
#ifdef BT_OSC #ifdef BT_OSC
SLIPBTSerial.beginPacket(); SLIPBTSerial.beginPacket();
qmsg.send(SLIPBTSerial); qmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket(); SLIPBTSerial.endPacket();
SLIPBTSerial.beginPacket(); SLIPBTSerial.beginPacket();
qdmsg.send(SLIPBTSerial); qdmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket(); SLIPBTSerial.endPacket();
#endif #endif
qmsg.empty(); qmsg.empty();
qdmsg.empty(); qdmsg.empty();
#ifdef OUTPUT_READABLE_REALACCEL #ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity // display real acceleration, adjusted to remove gravity
mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
AcX = aaReal.x; AcX = aaReal.x;
AcY = aaReal.y; AcY = aaReal.y;
AcZ = aaReal.z; AcZ = aaReal.z;
#endif #endif
#ifdef OUTPUT_READABLE_WORLDACCEL #ifdef OUTPUT_READABLE_WORLDACCEL
// display initial world-frame acceleration, adjusted to remove gravity // display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion // and rotated based on known orientation from quaternion
mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
AcX = aaWorld.x; AcX = aaWorld.x;
AcY = aaWorld.y; AcY = aaWorld.y;
AcZ = aaWorld.z; AcZ = aaWorld.z;
#endif #endif
amsg.add(AcX); amsg.add(AcX);
@ -312,21 +314,21 @@ void loop() {
amsg.add(AcZ); amsg.add(AcZ);
#ifdef SERIAL_OSC #ifdef SERIAL_OSC
SLIPSerial.beginPacket(); SLIPSerial.beginPacket();
amsg.send(SLIPSerial); amsg.send(SLIPSerial);
SLIPSerial.endPacket(); SLIPSerial.endPacket();
#endif #endif
#ifdef WIFI_OSC #ifdef WIFI_OSC
udp.beginPacket(castIp, port); udp.beginPacket(castIp, port);
amsg.send(udp); amsg.send(udp);
udp.endPacket(); udp.endPacket();
#endif #endif
#ifdef BT_OSC #ifdef BT_OSC
SLIPBTSerial.beginPacket(); SLIPBTSerial.beginPacket();
amsg.send(SLIPBTSerial); amsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket(); SLIPBTSerial.endPacket();
#endif #endif
amsg.empty(); amsg.empty();
@ -338,21 +340,21 @@ void loop() {
} }
#ifdef SERIAL_OSC #ifdef SERIAL_OSC
SLIPSerial.beginPacket(); SLIPSerial.beginPacket();
kmsg.send(SLIPSerial); kmsg.send(SLIPSerial);
SLIPSerial.endPacket(); SLIPSerial.endPacket();
#endif #endif
#ifdef WIFI_OSC #ifdef WIFI_OSC
udp.beginPacket(castIp, port); udp.beginPacket(castIp, port);
kmsg.send(udp); kmsg.send(udp);
udp.endPacket(); udp.endPacket();
#endif #endif
#ifdef BT_OSC #ifdef BT_OSC
SLIPBTSerial.beginPacket(); SLIPBTSerial.beginPacket();
kmsg.send(SLIPBTSerial); kmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket(); SLIPBTSerial.endPacket();
#endif #endif
kmsg.empty(); kmsg.empty();