g1smo
/
pifcamp-2021
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Added position and speed tracking and messaging

master
Martin 2022-03-05 14:40:51 +01:00
parent d55edf3271
commit 2f0e7a6b7f
1 changed files with 63 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
osc32final/osc32final.ino
View File

@ -7,6 +7,12 @@
#include <OSCBoards.h>
#include <OSCMessage.h>
// POSITION CALCULATION
#include <BasicLinearAlgebra.h>
#include "math.h"
using namespace BLA;
//#define SERIAL_OSC
//#define WIFI_OSC BRIŠI
#define BT_OSC
@ -108,7 +114,8 @@ VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
uint32_t timeOn = 0; // Uptime counter for movement calculation
Matrix<3> position; // [x,y,z] tracks position of device
Matrix<3> speed; // [x,y,z] tracks speed of device
// Sem dobimo vrednosti pospeskomerja in ziroskopa
int16_t AcX,AcY,AcZ;
@ -134,6 +141,9 @@ OSCMessage amsg("/accel/"); // X Y Z
// Keys held down
OSCMessage kmsg("/keys/"); // A B C D E
// Position and speed
OSCMessage positionMessage("/position/");
OSCMessage speedMessage("/speed/");
void setup() {
// Basic(debug) serial init
@ -152,6 +162,12 @@ void setup() {
for(int i = 0; i < KEYLEN; i++) {
pinMode(keys[i], INPUT_PULLUP);
}
// Position and speed tracking
timeOn = 0;
position.Fill(0);
speed.Fill(0);
// Start MPU
mpu.initialize();
@ -308,7 +324,6 @@ void loop() {
AcY = aaWorld.y;
AcZ = aaWorld.z;
#endif
amsg.add(AcX);
amsg.add(AcY);
amsg.add(AcZ);
@ -332,6 +347,25 @@ void loop() {
#endif
amsg.empty();
// Calculate speed and position from accelerometer data
int prevTime = timeOn;
timeOn = millis();
int elapsedTime = timeOn - prevTime;
Matrix<3> speedGain = {AcX * elapsedTime, AcY * elapsedTime, AcZ * elapsedTime};
//Assume linear acceleration over measured time window, multiply time by halfpoint between last-known and current speed
position = position + (((speed + speedGain) + speed) /2 * elapsedTime);
speed += speedGain;
positionMessage.add(position(0));
positionMessage.add(position(1));
positionMessage.add(position(2));
speedMessage.add(speed(0));
speedMessage.add(speed(1));
speedMessage.add(speed(2));
// Send keys
for(int i = 0; i < KEYLEN; i++) {
@ -343,20 +377,45 @@ void loop() {
SLIPSerial.beginPacket();
kmsg.send(SLIPSerial);
SLIPSerial.endPacket();
SLIPSerial.beginPacket();
positionMessage.send(SLIPSerial);
SLIPSerial.endPacket();
SLIPSerial.beginPacket();
speedMessage.send(SLIPSerial);
SLIPSerial.endPacket();
#endif
#ifdef WIFI_OSC
udp.beginPacket(castIp, port);
kmsg.send(udp);
udp.endPacket();
udp.beginPacket(castIp, port);
positionMessage.send(udp);
udp.endPacket();
udp.beginPacket(castIp, port);
speedMessage.send(udp);
udp.endPacket();
#endif
#ifdef BT_OSC
SLIPBTSerial.beginPacket();
kmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
#endif
SLIPBTSerial.beginPacket();
positionMessage.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
SLIPBTSerial.beginPacket();
speedMessage.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
#endif
kmsg.empty();
positionMessage.empty();
speedMessage.empty();
}
}