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

Standradising the new final sketch

master
Jurij Podgoršek 2022-03-05 00:49:55 +01:00
parent 2427962b9c
commit 018887a13d
5 changed files with 465 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
osc32bt/osc32bt.ino
View File

@ -62,13 +62,12 @@ unsigned long timeOn = 0;
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
Quaternion pq; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 gy; // [x, y, z] gyro sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
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
// Sem dobimo vrednosti
@ -204,27 +203,6 @@ void loop() {
#endif
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
GyX = euler[0];
GyY = euler[1];
GyZ = euler[2];
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
GyX = ypr[0];
GyY = ypr[1];
GyZ = ypr[2];
#endif
#ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity
mpu.dmpGetQuaternion(&q, fifoBuffer);

181
osc32final/SLIPEncodedBluetoothSerial.cpp 100644
View File

@ -0,0 +1,181 @@
#include "SLIPEncodedBluetoothSerial.h"
#include "BluetoothSerial.h"
/*
CONSTRUCTOR
*/
//instantiate with the tranmission layer
//use BluetoothSerial
SLIPEncodedBluetoothSerial::SLIPEncodedBluetoothSerial(BluetoothSerial &s){
serial = &s;
rstate = CHAR;
}
static const uint8_t eot = 0300;
static const uint8_t slipesc = 0333;
static const uint8_t slipescend = 0334;
static const uint8_t slipescesc = 0335;
/*
SERIAL METHODS
*/
bool SLIPEncodedBluetoothSerial::endofPacket()
{
if(rstate == SECONDEOT)
{
rstate = CHAR;
return true;
}
if (rstate==FIRSTEOT)
{
if(serial->available())
{
uint8_t c =serial->peek();
if(c==eot)
{
serial->read(); // throw it on the floor
}
}
rstate = CHAR;
return true;
}
return false;
}
int SLIPEncodedBluetoothSerial::available(){
back:
int cnt = serial->available();
if(cnt==0)
return 0;
if(rstate==CHAR)
{
uint8_t c =serial->peek();
if(c==slipesc)
{
rstate = SLIPESC;
serial->read(); // throw it on the floor
goto back;
}
else if( c==eot)
{
rstate = FIRSTEOT;
serial->read(); // throw it on the floor
goto back;
}
return 1; // we may have more but this is the only sure bet
}
else if(rstate==SLIPESC)
return 1;
else if(rstate==FIRSTEOT)
{
if(serial->peek()==eot)
{
rstate = SECONDEOT;
serial->read(); // throw it on the floor
return 0;
}
rstate = CHAR;
}else if (rstate==SECONDEOT) {
rstate = CHAR;
}
return 0;
}
//reads a byte from the buffer
int SLIPEncodedBluetoothSerial::read(){
back:
uint8_t c = serial->read();
if(rstate==CHAR)
{
if(c==slipesc)
{
rstate=SLIPESC;
goto back;
}
else if(c==eot){
return -1; // xxx this is an error
}
return c;
}
else
if(rstate==SLIPESC)
{
rstate=CHAR;
if(c==slipescend)
return eot;
else if(c==slipescesc)
return slipesc;
else {
// insert some error code here
return -1;
}
}
else
return -1;
}
// as close as we can get to correct behavior
int SLIPEncodedBluetoothSerial::peek(){
uint8_t c = serial->peek();
if(rstate==SLIPESC)
{
if(c==slipescend)
return eot;
else if(c==slipescesc)
return slipesc;
}
return c;
}
//the arduino and wiring libraries have different return types for the write function
#if defined(WIRING) || defined(BOARD_DEFS_H)
//encode SLIP
void SLIPEncodedBluetoothSerial::write(uint8_t b){
if(b == eot){
serial->write(slipesc);
return serial->write(slipescend);
} else if(b==slipesc) {
serial->write(slipesc);
return serial->write(slipescesc);
} else {
return serial->write(b);
}
}
void SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) { while(size--) write(*buffer++); }
#else
//encode SLIP
size_t SLIPEncodedBluetoothSerial::write(uint8_t b){
if(b == eot){
serial->write(slipesc);
return serial->write(slipescend);
} else if(b==slipesc) {
serial->write(slipesc);
return serial->write(slipescesc);
} else {
return serial->write(b);
}
}
size_t SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) { size_t result=0; while(size--) result = write(*buffer++); return result; }
#endif
void SLIPEncodedBluetoothSerial::begin(String name){
serial->begin(name);
}
//SLIP specific method which begins a transmitted packet
void SLIPEncodedBluetoothSerial::beginPacket() { serial->write(eot); }
//signify the end of the packet with an EOT
void SLIPEncodedBluetoothSerial::endPacket(){
serial->write(eot);
}
void SLIPEncodedBluetoothSerial::flush(){
serial->flush();
}

62
osc32final/SLIPEncodedBluetoothSerial.h 100644
View File

@ -0,0 +1,62 @@
/*
Extends the Serial class to encode SLIP over serial
*/
#ifndef SLIPEncodedBluetoothSerial_h
#define SLIPEncodedBluetoothSerial_h
#include "Arduino.h"
#include <Stream.h>
#include "BluetoothSerial.h"
class SLIPEncodedBluetoothSerial: public Stream{
private:
enum erstate {CHAR, FIRSTEOT, SECONDEOT, SLIPESC } rstate;
//the serial port used
BluetoothSerial * serial;
public:
//the serial port used
SLIPEncodedBluetoothSerial(BluetoothSerial & );
int available();
int read();
int peek();
void flush();
//same as Serial.begin
void begin(String);
//SLIP specific method which begins a transmitted packet
void beginPacket();
//SLIP specific method which ends a transmittedpacket
void endPacket();
// SLIP specific method which indicates that an EOT was received
bool endofPacket();
//the arduino and wiring libraries have different return types for the write function
#if defined(WIRING) || defined(BOARD_DEFS_H)
void write(uint8_t b);
void write(const uint8_t *buffer, size_t size);
#else
//overrides the Stream's write function to encode SLIP
size_t write(uint8_t b);
size_t write(const uint8_t *buffer, size_t size);
//using Print::write;
#endif
};
#endif

336
osc32final/osc32final.ino
View File

@ -6,52 +6,88 @@
#include <OSCBoards.h>
#include <OSCMessage.h>
/*
Make an OSC message and send it over serial
*/
//#define SERIAL_OSC
//#define WIFI_OSC
#define BT_OSC
// SERIAL
#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
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
#endif
// WIFI
#ifdef WIFI_OSC
#include <WiFi.h>
const char* ssid = "Grajski"; // your network SSID (name of wifi network)
const char* password = "nedeladanes"; // your network password
// Multicast IP / port
const IPAddress castIp = IPAddress(224,0,1,9);
const int port = 6696;
bool connected = false;
#include <WiFiUdp.h>
WiFiUDP udp;
void connectToWiFi(const char * ssid, const char * pwd){
Serial.println("Connecting to WiFi network: " + String(ssid));
// delete old config
WiFi.disconnect(true);
//register event handler
WiFi.onEvent(WiFiEvent);
//Initiate connection
WiFi.begin(ssid, pwd);
Serial.println("Waiting for WIFI connection...");
}
//wifi event handler
void WiFiEvent(WiFiEvent_t event){
switch(event) {
case ARDUINO_EVENT_WIFI_STA_GOT_IP:
//When connected set
Serial.print("WiFi connected! IP address: ");
Serial.println(WiFi.localIP());
//initializes the UDP state
//This initializes the transfer buffer
udp.begin(WiFi.localIP(), port);
connected = true;
break;
case ARDUINO_EVENT_WIFI_STA_DISCONNECTED:
connected = false;
Serial.println("\n\n\n================\nLOST WIFI CONNECTION!\n\n\nTrying again soon...\n\n\n");
delay(1000);
connectToWiFi(ssid, password);
break;
default: break;
}
}
#endif
// Bluetooth
#ifdef BT_OSC
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
#include <SLIPEncodedSerial.h>
#include "BluetoothSerial.h"
#include "SLIPEncodedBluetoothSerial.h"
BluetoothSerial SerialBT;
SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
#endif
// Motion sensor object
MPU6050 mpu;
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)
#define OUTPUT_READABLE_QUATERNION
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define OUTPUT_READABLE_EULER
// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL
// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
// not compensated for orientation, so +X is always +X according to the
// sensor, just without the effects of gravity. If you want acceleration
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
// components with gravity removed and adjusted for the world frame of
// reference (yaw is relative to initial orientation, since no magnetometer
// is present in this case). Could be quite handy in some cases.
#define OUTPUT_READABLE_WORLDACCEL
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
@ -62,6 +98,8 @@ uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
Quaternion pq; // [w, x, y, z] previous quaternion container
Quaternion diff; // [w, x, y, z] quaternion derivate container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 gy; // [x, y, z] gyro sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
@ -70,8 +108,7 @@ 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
// Sem dobimo vrednosti
// Sem dobimo vrednosti pospeskomerja in ziroskopa
int16_t AcX,AcY,AcZ;
float GyX, GyY, GyZ;
@ -80,23 +117,44 @@ byte keys[] = {16, 17, 5, 18};
byte pressed[] = {0, 0, 0, 0};
byte KEYLEN = 4;
OSCMessage msg("/accel/");
OSCMessage gmsg("/gyro/");
OSCMessage emsg("/error/");
OSCMessage kmsg("/keys/");
OSCMessage qmsg("/quaternion/");
/* OSC MSG channels */
// Quaternion - rotacija
OSCMessage qmsg("/quaternion/"); // W X Y Z
// Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
OSCMessage qdmsg("/quaternionDiff/"); // W X Y Z
// Accelerometer
OSCMessage amsg("/accel/"); // X Y Z
// Keys held down
OSCMessage kmsg("/keys/"); // A B C D E
void setup() {
// Basic(debug) serial init
// Serial.begin(115200); // set this as high as you can reliably run on your platform
Serial.println("Starting up...");
// I2C init
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
SLIPSerial.begin(115200); // set this as high as you can reliably run on your platform
#ifdef SERIAL_OSC
SLIPSerial.begin(115200); // set this as high as you can reliably run on your platform
#endif
// Keys
for(int i = 0; i < KEYLEN; i++) {
pinMode(keys[i], INPUT_PULLUP);
}
// Start MPU
mpu.initialize();
// Set sensitivity / range
mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
@ -104,7 +162,7 @@ void setup() {
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
// !!! Run Zero IMU to get readings
// !!! Run Zero IMU to get readings (read comments for instructions)
/* First proto (right hand, black&blue)
mpu.setXGyroOffset(76);
@ -141,114 +199,162 @@ void setup() {
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial.println("Error: " + String(devStatus));
emsg.add("DMP Initialization failed (code " + String(devStatus) + ")");
SLIPSerial.beginPacket();
emsg.send(SLIPSerial);
SLIPSerial.endPacket();
emsg.empty();
Serial.println("DMP Initialization failed (code " + String(devStatus) + ")");
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
}
#ifdef WIFI_OSC
// WIFI init
Serial.print("Attempting to connect to SSID: ");
Serial.println(ssid);
connectToWiFi(ssid, password);
// attempt to connect to Wifi network:
while (WiFi.status() != WL_CONNECTED) {
Serial.print(".");
// wait 1 second for re-trying
delay(1000);
}
#endif
#ifdef BT_OSC
SerialBT.begin("wavey wind");
#endif
}
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// read a packet from FIFO
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
// Store last Q value
pq = Quaternion(q.w,q.x,q.y,q.z);
// get quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
//compute the differential rotation between the previous and new orientation
diff = q.getProduct(pq.getConjugate());
qmsg.add(q.w);
qmsg.add(q.x);
qmsg.add(q.y);
qmsg.add(q.z);
qdmsg.add(diff.w);
qdmsg.add(diff.x);
qdmsg.add(diff.y);
qdmsg.add(diff.z);
#ifdef SERIAL_OSC
SLIPSerial.beginPacket();
qdmsg.send(SLIPSerial);
SLIPSerial.endPacket();
SLIPSerial.beginPacket();
qmsg.send(SLIPSerial);
SLIPSerial.endPacket();
#endif
#ifdef WIFI_OSC
udp.beginPacket(castIp, port);
qmsg.send(udp);
udp.endPacket();
udp.beginPacket(castIp, port);
qdmsg.send(udp);
udp.endPacket();
#endif
#ifdef BT_OSC
SLIPBTSerial.beginPacket();
qmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
SLIPBTSerial.beginPacket();
qdmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
#endif
qmsg.empty();
#endif
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
GyX = euler[0];
GyY = euler[1];
GyZ = euler[2];
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
GyX = ypr[0];
GyY = ypr[1];
GyZ = ypr[2];
#endif
qdmsg.empty();
#ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
AcX = aaReal.x;
AcY = aaReal.y;
AcZ = aaReal.z;
// display real acceleration, adjusted to remove gravity
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
AcX = aaReal.x;
AcY = aaReal.y;
AcZ = aaReal.z;
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
AcX = aaWorld.x;
AcY = aaWorld.y;
AcZ = aaWorld.z;
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
AcX = aaWorld.x;
AcY = aaWorld.y;
AcZ = aaWorld.z;
#endif
// Send over serial
msg.add(AcX);
msg.add(AcY);
msg.add(AcZ);
SLIPSerial.beginPacket();
msg.send(SLIPSerial);
SLIPSerial.endPacket();
msg.empty();
amsg.add(AcX);
amsg.add(AcY);
amsg.add(AcZ);
#ifdef SERIAL_OSC
SLIPSerial.beginPacket();
amsg.send(SLIPSerial);
SLIPSerial.endPacket();
#endif
#ifdef WIFI_OSC
udp.beginPacket(castIp, port);
amsg.send(udp);
udp.endPacket();
#endif
gmsg.add(GyX);
gmsg.add(GyY);
gmsg.add(GyZ);
SLIPSerial.beginPacket();
gmsg.send(SLIPSerial);
SLIPSerial.endPacket();
gmsg.empty();
#ifdef BT_OSC
SLIPBTSerial.beginPacket();
amsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
#endif
amsg.empty();
// Send keys
for(int i = 0; i < KEYLEN; i++) {
pressed[i] = !digitalRead(keys[i]);
kmsg.add(pressed[i]);
}
SLIPSerial.beginPacket();
kmsg.send(SLIPSerial);
SLIPSerial.endPacket();
#ifdef SERIAL_OSC
SLIPSerial.beginPacket();
kmsg.send(SLIPSerial);
SLIPSerial.endPacket();
#endif
#ifdef WIFI_OSC
udp.beginPacket(castIp, port);
kmsg.send(udp);
udp.endPacket();
#endif
#ifdef BT_OSC
SLIPBTSerial.beginPacket();
kmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
#endif
kmsg.empty();
}
}

2
osc32wifi/osc32wifi.ino
View File

@ -28,8 +28,6 @@ const IPAddress castIp = IPAddress(224,0,1,9);
const int port = 6696;
bool connected = false;
//#include "AsyncUDP.h"
//AsyncUDP udp;
#include <WiFiUdp.h>
WiFiUDP udp;