Ciscenje, delujoc acc in quat, WIP euler konverzija, WIP olimex sketch
parent
b3e37f57a7
commit
d20c552ee7
|
|
@ -5,11 +5,27 @@ AHRSensor {
|
|||
<>euler,
|
||||
<>accel,
|
||||
|
||||
<battery,
|
||||
<>battery,
|
||||
<>eps,
|
||||
|
||||
// GUI elementi
|
||||
// Quaternion
|
||||
<gQw,
|
||||
<gQx,
|
||||
<gQy,
|
||||
<gQz,
|
||||
// Eulerjevi koti
|
||||
<gEx,
|
||||
<gEy,
|
||||
<gEz;
|
||||
<gEz,
|
||||
// Pospeskomer
|
||||
<gAx,
|
||||
<gAy,
|
||||
<gAz,
|
||||
// Baterija
|
||||
<gB,
|
||||
// Dogodkov na sekundo
|
||||
<gEps;
|
||||
|
||||
*new { |id|
|
||||
^super.newCopyArgs(id).init;
|
||||
|
|
@ -34,20 +50,19 @@ AHRSensor {
|
|||
var m21 = matrix[1], m22 = matrix[5], m23 = matrix[9];
|
||||
var m31 = matrix[2], m32 = matrix[6], m33 = matrix[10];
|
||||
|
||||
var euler = [0, 0, 0];
|
||||
var e = [0, 0, 0];
|
||||
|
||||
euler[1] = asin(m13.clip(- 1, 1));
|
||||
e[1] = asin(m13.clip(- 1, 1));
|
||||
|
||||
if ((m13.abs < 0.9999999), {
|
||||
euler[0] = atan2(m23.neg, m33);
|
||||
euler[2] = atan2(m12.neg, m11);
|
||||
e[0] = atan2(m23.neg, m33);
|
||||
e[2] = atan2(m12.neg, m11);
|
||||
}, {
|
||||
euler[0] = atan2(m32, m22);
|
||||
euler[2] = 0;
|
||||
e[0] = atan2(m32, m22);
|
||||
e[2] = 0;
|
||||
});
|
||||
|
||||
euler.postln;
|
||||
euler;
|
||||
^e;
|
||||
}
|
||||
|
||||
init {
|
||||
|
|
@ -56,6 +71,7 @@ AHRSensor {
|
|||
euler = [0, 0, 0];
|
||||
accel = [0, 0, 0];
|
||||
battery = 0;
|
||||
eps = 0;
|
||||
|
||||
this.guiInit;
|
||||
}
|
||||
|
|
@ -74,31 +90,117 @@ AHRSensor {
|
|||
gEx = StaticText().string_(0).stringColor_(cRed);
|
||||
gEy = StaticText().string_(0).stringColor_(cGreen);
|
||||
gEz = StaticText().string_(0).stringColor_(cBlue);
|
||||
|
||||
//gAx = StaticText().string_(0).stringColor_(cRed);
|
||||
//gAy = StaticText().string_(0).stringColor_(cGreen);
|
||||
//gAz = StaticText().string_(0).stringColor_(cBlue);
|
||||
gAx = LevelIndicator();
|
||||
gAy = LevelIndicator();
|
||||
gAz = LevelIndicator();
|
||||
|
||||
gQw = StaticText().string_(0).stringColor_(cPurple);
|
||||
gQx = StaticText().string_(0).stringColor_(cRed);
|
||||
gQy = StaticText().string_(0).stringColor_(cGreen);
|
||||
gQz = StaticText().string_(0).stringColor_(cBlue);
|
||||
|
||||
gB = StaticText().string_(0).stringColor_(cRed);
|
||||
gEps = StaticText().string_(0);
|
||||
}
|
||||
|
||||
getGui {
|
||||
^VLayout(
|
||||
StaticText().string_("Sensor " ++ id),
|
||||
HLayout(
|
||||
^[
|
||||
[
|
||||
StaticText().font_(Font("OpenSans", 12, true)).string_("Sensor " ++ id),
|
||||
nil,
|
||||
nil,
|
||||
[StaticText().string_("bat: "), align: \right],
|
||||
gB,
|
||||
[StaticText().string_("events/s: "), align: \right],
|
||||
gEps
|
||||
],
|
||||
[
|
||||
StaticText().string_("quaternion: "),
|
||||
[StaticText().string_("w: "), align: \right],
|
||||
gQw,
|
||||
[StaticText().string_("x: "), align: \right],
|
||||
gQx,
|
||||
[StaticText().string_("y: "), align: \right],
|
||||
gQy,
|
||||
[StaticText().string_("z: "), align: \right],
|
||||
gQz,
|
||||
],
|
||||
[
|
||||
StaticText().string_("euler: "),
|
||||
[StaticText().string_("x: "), align: \left],
|
||||
[gEx, align: \left],
|
||||
StaticText().string_("y: "),
|
||||
[StaticText().string_("x: "), align: \right],
|
||||
gEx,
|
||||
[StaticText().string_("y: "), align: \right],
|
||||
gEy,
|
||||
StaticText().string_("z: "),
|
||||
[StaticText().string_("z: "), align: \right],
|
||||
gEz,
|
||||
)
|
||||
);
|
||||
],
|
||||
[
|
||||
StaticText().string_("accel: "),
|
||||
[StaticText().string_("x: "), align: \right],
|
||||
gAx,
|
||||
[StaticText().string_("y: "), align: \right],
|
||||
gAy,
|
||||
[StaticText().string_("z: "), align: \right],
|
||||
gAz,
|
||||
],
|
||||
[]
|
||||
];
|
||||
}
|
||||
|
||||
updateEuler { |newQuat|
|
||||
var quatDiff = newQuat / quat;
|
||||
euler += quat2euler(quatDiff);
|
||||
euler = euler + this.quat2euler(quatDiff);
|
||||
}
|
||||
|
||||
refreshGuiQuat {
|
||||
// Stevilo decimalk
|
||||
var prec = 3;
|
||||
gQw.string_(quat.a.asStringPrec(prec));
|
||||
gQx.string_(quat.b.asStringPrec(prec));
|
||||
gQy.string_(quat.c.asStringPrec(prec));
|
||||
gQz.string_(quat.d.asStringPrec(prec));
|
||||
}
|
||||
|
||||
refreshGuiEuler {
|
||||
// Stevilo decimalk
|
||||
var prec = 3;
|
||||
gEx.string_(euler[0].asStringPrec(prec));
|
||||
gEy.string_(euler[1].asStringPrec(prec));
|
||||
gEz.string_(euler[2].asStringPrec(prec));
|
||||
}
|
||||
|
||||
refreshGuiAccel {
|
||||
// Stevilo decimalk
|
||||
//var prec = 2;
|
||||
//gAx.string_(accel[0].asStringPrec(prec));
|
||||
//gAy.string_(accel[1].asStringPrec(prec));
|
||||
//gAz.string_(accel[2].asStringPrec(prec));
|
||||
var from = -50, to = 50;
|
||||
[gAx, gAy, gAz].do({|el, i|
|
||||
el.value_(accel[i].linlin(from, to, 0, 1));
|
||||
});
|
||||
}
|
||||
|
||||
refreshGuiBat {
|
||||
// Stevilo decimalk
|
||||
var prec = 2;
|
||||
gB.string_(battery.asStringPrec(prec));
|
||||
}
|
||||
|
||||
refreshGuiEps {
|
||||
// Stevilo decimalk
|
||||
var prec = 2;
|
||||
gEps.string_(eps);
|
||||
}
|
||||
|
||||
refreshGui {
|
||||
gEx.string_(euler[0]);
|
||||
gEy.string_(euler[1]);
|
||||
gEz.string_(euler[2]);
|
||||
this.refreshGuiQuat;
|
||||
this.refreshGuiEuler;
|
||||
this.refreshGuiAccel;
|
||||
this.refreshGuiBat;
|
||||
}
|
||||
}
|
||||
|
|
@ -1,180 +0,0 @@
|
|||
#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();
|
||||
}
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
/*
|
||||
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
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
# SPDX-License-Identifier: GPL-2.0-or-later
|
||||
#
|
||||
# Example OpenOCD configuration file for ESP32-WROVER-KIT board.
|
||||
#
|
||||
# For example, OpenOCD can be started for ESP32 debugging on
|
||||
#
|
||||
# openocd -f board/esp32-wrover-kit-3.3v.cfg
|
||||
#
|
||||
|
||||
# Source the JTAG interface configuration file
|
||||
source [find interface/ftdi/esp32_devkitj_v1.cfg]
|
||||
set ESP32_FLASH_VOLTAGE 3.3
|
||||
# Source the ESP32 configuration file
|
||||
source [find target/esp32.cfg]
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,19 +0,0 @@
|
|||
{
|
||||
"name":"Arduino on ESP32",
|
||||
"toolchainPrefix":"xtensa-esp32-elf",
|
||||
"svdFile":"esp32.svd",
|
||||
"request":"attach",
|
||||
"postAttachCommands":[
|
||||
"set remote hardware-watchpoint-limit 2",
|
||||
"monitor reset halt",
|
||||
"monitor gdb_sync",
|
||||
"thb setup",
|
||||
"c"
|
||||
],
|
||||
"overrideRestartCommands":[
|
||||
"monitor reset halt",
|
||||
"monitor gdb_sync",
|
||||
"thb setup",
|
||||
"c"
|
||||
]
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -22,15 +22,24 @@ build_src_filter =
|
|||
+<main.cpp>
|
||||
|
||||
;; Olimex prototype sketch
|
||||
[env:ada]
|
||||
board = esp32-s2-saola-1
|
||||
[env:main-olimex]
|
||||
build_src_filter =
|
||||
+<ada.cpp>
|
||||
+<main-olimex.cpp>
|
||||
board = esp32-s2-saola-1
|
||||
upload_speed = 460800
|
||||
build_flags =
|
||||
|
||||
[env:sprejemnik]
|
||||
build_src_filter =
|
||||
+<sprejemnik.cpp>
|
||||
|
||||
[env:sprejemnik-olimex]
|
||||
build_src_filter =
|
||||
+<sprejemnik-olimex.cpp>
|
||||
board = esp32-s2-saola-1
|
||||
upload_speed = 460800
|
||||
build_flags =
|
||||
|
||||
[env:scanner]
|
||||
build_src_filter =
|
||||
+<scanner.cpp>
|
||||
|
|
|
|||
349
server.js
349
server.js
|
|
@ -1,349 +0,0 @@
|
|||
// Which port do I send OSC messages to? (SupperCollider, ...)
|
||||
const OSCPORT = 57120;
|
||||
|
||||
// Which port do I listen to (for visuals, calibration)
|
||||
const PORT = 6676
|
||||
|
||||
// Do we have a problem, shall we debug?
|
||||
const DEBUG = {
|
||||
osc: true,
|
||||
udp: false,
|
||||
midi: true
|
||||
}
|
||||
|
||||
// MIDI out
|
||||
const MIDI = true
|
||||
|
||||
// Serial baud rate
|
||||
const BAUDRATE = 115200
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
require('serialport')
|
||||
const express = require('express')
|
||||
const http = require('http')
|
||||
const WebSocket = require('ws')
|
||||
const osc = require('osc')
|
||||
const readline = require('readline')
|
||||
const fs = require('fs')
|
||||
const midi = require('midi')
|
||||
|
||||
|
||||
// Seznam povezanih tty naprav
|
||||
const tty = process.argv.splice(2)
|
||||
|
||||
let eulerRotation = [0, 0, 0]
|
||||
|
||||
const include_files = [
|
||||
'/anim.js',
|
||||
'/control.js',
|
||||
'/osctl.js',
|
||||
'/test.js',
|
||||
'/node_modules/three/build/three.min.js',
|
||||
'/node_modules/nouislider/distribute/nouislider.min.js',
|
||||
'/node_modules/nouislider/distribute/nouislider.min.css',
|
||||
'/node_modules/osc/dist/osc-browser.js'
|
||||
];
|
||||
|
||||
function isFloat(value) {
|
||||
if (
|
||||
typeof value === 'number' &&
|
||||
!Number.isNaN(value) &&
|
||||
!Number.isInteger(value)
|
||||
) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
const app = express();
|
||||
const server = http.Server(app);
|
||||
|
||||
// Odprti serijski OSC linki
|
||||
let serijskePovezave = {}
|
||||
|
||||
let mo = null
|
||||
let mi = null
|
||||
if (MIDI) {
|
||||
// Midi port
|
||||
mo = new midi.Output()
|
||||
mi = new midi.Input()
|
||||
//mo.getPortCount()
|
||||
//mo.getPortName(0)
|
||||
//mo.openPort(0)
|
||||
mo.openVirtualPort("kegel")
|
||||
mi.openVirtualPort("ww-midi-in")
|
||||
}
|
||||
|
||||
function openSerial(ttyAddr) {
|
||||
console.log('opening ', ttyAddr, BAUDRATE)
|
||||
|
||||
serijskePovezave[ttyAddr] = new osc.SerialPort({
|
||||
devicePath: ttyAddr,
|
||||
bitrate: BAUDRATE,
|
||||
autoOpen: true,
|
||||
useSLIP: true
|
||||
})
|
||||
|
||||
const scon = serijskePovezave[ttyAddr]
|
||||
|
||||
scon.on('open', e => {
|
||||
console.log(`serial connection ${ttyAddr} opened`)
|
||||
//console.log(scon)
|
||||
})
|
||||
scon.on('error', e => {
|
||||
console.error(`tty ${ttyAddr} error`, e)
|
||||
//scon.close()
|
||||
})
|
||||
scon.on('close', e => {
|
||||
console.warn(`serial connection ${ttyAddr} closed, restarting in 1 second`)
|
||||
setTimeout(() => { openSerial(ttyAddr) }, 1000)
|
||||
})
|
||||
|
||||
// Arduino OSC gre v web
|
||||
scon.on('message', msg => {
|
||||
const index = Object.keys(serijskePovezave).indexOf(ttyAddr)
|
||||
const prepend = `/ww/${index}`
|
||||
|
||||
// Debug incoming serial osc
|
||||
if (DEBUG.osc) {
|
||||
console.log('osc SERIAL msg', msg.address, msg.args.map((a) => isFloat(a) ? a.toFixed(3) : a), ttyAddr, prepend)
|
||||
}
|
||||
sendAll(msg, null, null, osclients, prepend)
|
||||
})
|
||||
|
||||
scon.open()
|
||||
|
||||
if (scon._closeCode) {
|
||||
scon = null
|
||||
console.log('restarting serial connection ', ttyAddr)
|
||||
setTimeout(openSerial, 1000)
|
||||
}
|
||||
}
|
||||
|
||||
if (tty.length > 0) {
|
||||
tty.forEach((ttyAddr) => {
|
||||
openSerial(ttyAddr);
|
||||
});
|
||||
}
|
||||
|
||||
app.get('/', (req, res) => {
|
||||
res.sendFile(__dirname + '/index.html');
|
||||
});
|
||||
|
||||
app.get('/ctl', (req, res) => {
|
||||
res.sendFile(__dirname + '/control.html');
|
||||
});
|
||||
|
||||
app.get('/test', (req, res) => {
|
||||
res.sendFile(__dirname + '/test.html');
|
||||
});
|
||||
|
||||
// Hydra inclusion
|
||||
app.get('/hydra', function(req, res) {
|
||||
res.sendFile(__dirname + '/hydra-osc-main/index.html');
|
||||
});
|
||||
app.get('/lib/osc.min.js', function(req, res) {
|
||||
res.sendFile(__dirname + '/hydra-osc-main/lib/osc.min.js');
|
||||
});
|
||||
|
||||
let settings = {};
|
||||
app.get('/settings', function(req, res) {
|
||||
res.send(settings);
|
||||
});
|
||||
|
||||
include_files.map(function(file) {
|
||||
app.get(file, function(req, res){
|
||||
res.sendFile(__dirname + file);
|
||||
});
|
||||
});
|
||||
|
||||
server.listen(PORT, () => console.log('listening on *:' + PORT))
|
||||
|
||||
// Websocket init
|
||||
const wss = new WebSocket.Server({ server })
|
||||
|
||||
|
||||
// Relay multicast to websockets
|
||||
// @TODO still sends to supercollider? Do we need two sockets?
|
||||
/*
|
||||
var dgram = require('dgram');
|
||||
var sss = dgram.createSocket('udp4');
|
||||
sss.on('listening', () => {
|
||||
sss.addMembership('224.0.1.9');
|
||||
})
|
||||
sss.bind(6696, '224.0.1.9');
|
||||
*/
|
||||
|
||||
const scudp = new osc.UDPPort({
|
||||
remotePort: OSCPORT
|
||||
//socket: sss
|
||||
})
|
||||
|
||||
scudp.on('open', () => {
|
||||
console.log("UDP to OSC open")
|
||||
})
|
||||
|
||||
scudp.on('message', (msg) => {
|
||||
if (DEBUG.udp) {
|
||||
console.log('got UDP msg', msg);
|
||||
}
|
||||
|
||||
osclients.forEach( client => {
|
||||
if (client) {
|
||||
//console.log("sending", msg, info)
|
||||
client.send(msg)
|
||||
}
|
||||
})
|
||||
})
|
||||
scudp.on('error', (e) => {
|
||||
console.log('UDP OSC error!', e)
|
||||
})
|
||||
scudp.open()
|
||||
|
||||
if (MIDI) {
|
||||
mi.on('message', (deltaTime, message) => {
|
||||
// The message is an array of numbers corresponding to the MIDI bytes:
|
||||
// [status, data1, data2]
|
||||
// https://www.cs.cf.ac.uk/Dave/Multimedia/node158.html has some helpful
|
||||
// information interpreting the messages.
|
||||
console.log(`midi in: ${message} d: ${deltaTime}`);
|
||||
mo.send(message)
|
||||
|
||||
sendAll({
|
||||
address: '/midi-in',
|
||||
args: message
|
||||
}, null, null, osclients)
|
||||
});
|
||||
}
|
||||
|
||||
function eulerFromQuaternion(quaternion) {
|
||||
// Quaternion to matrix.
|
||||
const w = quaternion[0], x = quaternion[1], y = quaternion[2], z = quaternion[3];
|
||||
const x2 = x + x, y2 = y + y, z2 = z + z;
|
||||
const xx = x * x2, xy = x * y2, xz = x * z2;
|
||||
const yy = y * y2, yz = y * z2, zz = z * z2;
|
||||
const wx = w * x2, wy = w * y2, wz = w * z2;
|
||||
const matrix = [
|
||||
1 - ( yy + zz ), xy + wz, xz - wy, 0,
|
||||
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
|
||||
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
|
||||
0, 0, 0, 1
|
||||
];
|
||||
// Matrix to euler
|
||||
function clamp( value, min, max ) {
|
||||
return Math.max( min, Math.min( max, value ) );
|
||||
}
|
||||
const m11 = matrix[ 0 ], m12 = matrix[ 4 ], m13 = matrix[ 8 ];
|
||||
const m21 = matrix[ 1 ], m22 = matrix[ 5 ], m23 = matrix[ 9 ];
|
||||
const m31 = matrix[ 2 ], m32 = matrix[ 6 ], m33 = matrix[ 10 ];
|
||||
var euler = [ 0, 0, 0 ];
|
||||
|
||||
euler[1] = Math.asin( clamp( m13, - 1, 1 ) );
|
||||
if ( Math.abs( m13 ) < 0.9999999 ) {
|
||||
euler[0] = Math.atan2( - m23, m33 );
|
||||
euler[2] = Math.atan2( - m12, m11 );
|
||||
} else {
|
||||
euler[0] = Math.atan2( m32, m22 );
|
||||
euler[2] = 0;
|
||||
}
|
||||
return euler;
|
||||
}
|
||||
|
||||
const sendAll = (msg, info, oscWS, osclients, prepend = '') => {
|
||||
// Reset euler rotation to 0
|
||||
if (msg.address == '/keys') {
|
||||
if (msg.args[0] && msg.args[1] && msg.args[2] && msg.args[3]) {
|
||||
eulerRotation = [0, 0, 0]
|
||||
}
|
||||
}
|
||||
|
||||
// Convert quaternion diff to euler angle diff
|
||||
if (msg.address == '/quaternionDiff') {
|
||||
const euler = eulerFromQuaternion(msg.args, 'XYZ');
|
||||
sendAll({
|
||||
address: '/eulerDiff',
|
||||
args: euler
|
||||
}, info, oscWS, osclients, prepend)
|
||||
|
||||
eulerRotation[0] += euler[0]
|
||||
eulerRotation[1] += euler[1]
|
||||
eulerRotation[2] += euler[2]
|
||||
|
||||
sendAll({
|
||||
address: '/euler',
|
||||
args: eulerRotation
|
||||
}, info, oscWS, osclients, prepend)
|
||||
}
|
||||
|
||||
// Add prepend
|
||||
let sendMsg = {
|
||||
address: `${prepend}${msg.address}`,
|
||||
args: msg.args
|
||||
}
|
||||
|
||||
// OSC relay
|
||||
osclients.forEach( client => {
|
||||
if (client && oscWS != client) {
|
||||
// console.log("OSC RELAY", sendMsg, info, prepend)
|
||||
client.send(sendMsg)
|
||||
}
|
||||
})
|
||||
|
||||
if (scudp) {
|
||||
if (DEBUG.udp) {
|
||||
console.log("UDP SEND", msg)
|
||||
}
|
||||
scudp.send(sendMsg)
|
||||
}
|
||||
}
|
||||
|
||||
let osclients = []
|
||||
|
||||
wss.on('connection', function (ws) {
|
||||
console.log('new client connection', ws._socket.remoteAddress)
|
||||
let oscWS = new osc.WebSocketPort({
|
||||
socket: ws
|
||||
});
|
||||
|
||||
// Vsi OSC sem grejo naprej na kliente OSC
|
||||
oscWS.on('message', ({ address, args}, info) => {
|
||||
console.log('fasal sem', address, args)
|
||||
if (MIDI) {
|
||||
if (address == '/midi') {
|
||||
if (DEBUG.midi) {
|
||||
console.log('midi SEND', args)
|
||||
}
|
||||
mo.send(args)
|
||||
}
|
||||
}
|
||||
|
||||
sendAll({ address, args}, info, oscWS, osclients)
|
||||
})
|
||||
|
||||
oscWS.on('error', error => {
|
||||
console.warn('Ignoring invalid OSC')
|
||||
console.warn(error)
|
||||
oscWS.close()
|
||||
osclients = osclients.filter(ws => ws !== oscWS)
|
||||
})
|
||||
osclients.push(oscWS)
|
||||
|
||||
oscWS.on('close', () => {
|
||||
console.log('closing socket', oscWS.socket.remoteAddress)
|
||||
osclients = osclients.filter(ws => ws !== oscWS)
|
||||
})
|
||||
})
|
||||
|
||||
// Zapri midi
|
||||
/*
|
||||
if (MIDI && mo) {
|
||||
mo.closePort()
|
||||
}
|
||||
*/
|
||||
|
|
@ -1,23 +1,30 @@
|
|||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
#include <Adafruit_NeoPixel.h>
|
||||
#include "esp_adc_cal.h"
|
||||
|
||||
// ID kegla mora bit unikaten za vsakega! (se poslje poleg parametrov)
|
||||
#define SENSOR_ID 1
|
||||
|
||||
// I²C
|
||||
#define SDApin 2
|
||||
#define SCLpin 3
|
||||
// ID senzorja mora bit unikaten! (se poslje poleg parametrov)
|
||||
#define SENSOR_ID 9
|
||||
|
||||
// Stanje baterije
|
||||
#define BATTERYPIN 8
|
||||
#define BATTERY_PIN 8
|
||||
|
||||
// RGB LED
|
||||
#define NUMPIXELS 1
|
||||
#define PIXELPIN 18
|
||||
Adafruit_NeoPixel pixels(NUMPIXELS, PIXELPIN, NEO_GRB + NEO_KHZ800);
|
||||
|
||||
// debagiranje!
|
||||
#define DEBUG
|
||||
|
||||
// I²C pins
|
||||
#define SDA_PIN 2
|
||||
#define SCL_PIN 3
|
||||
|
||||
// IMU library
|
||||
#include <SPI.h>
|
||||
#include <Adafruit_BNO055.h>
|
||||
|
||||
/* Set the delay between fresh samples */
|
||||
#define BNO055_SAMPLERATE_DELAY_MS (10)
|
||||
|
||||
// ESPNOW WIFI package structure
|
||||
#include "sensor_msg.h"
|
||||
|
||||
|
|
@ -26,7 +33,9 @@
|
|||
#include <WiFi.h>
|
||||
|
||||
// MAC naslov sprejemnika
|
||||
uint8_t sprejemnikMac[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };
|
||||
//uint8_t sprejemnikMac[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };
|
||||
// PC!
|
||||
uint8_t sprejemnikMac[] = {0x9c, 0xb6, 0xd0, 0xc4, 0xe8, 0xb9};
|
||||
sensor_msg odcitek;
|
||||
esp_now_peer_info_t peerInfo;
|
||||
|
||||
|
|
@ -39,58 +48,95 @@ imu::Vector<3> linearAccel;
|
|||
|
||||
int cas = 0;
|
||||
|
||||
void error_blink() {
|
||||
pixels.setPixelColor(0, pixels.Color(255, 0, 0));
|
||||
while(1) {
|
||||
pixels.clear();
|
||||
delay(200);
|
||||
pixels.show();
|
||||
delay(200);
|
||||
};
|
||||
}
|
||||
|
||||
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...");
|
||||
|
||||
Wire.begin(SDApin, SCLpin);
|
||||
Wire.begin(SDA_PIN, SCL_PIN);
|
||||
// Fast mode
|
||||
Wire.setClock(400000);
|
||||
bno = Adafruit_BNO055(55, 0x28, &Wire);
|
||||
|
||||
// Init - 3 one second blinks
|
||||
pixels.begin();
|
||||
pixels.clear();
|
||||
pixels.setPixelColor(0, pixels.Color(0, 255, 0));
|
||||
pixels.setBrightness(100);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
|
||||
#ifdef DEBUG
|
||||
Serial.println(i + 1);
|
||||
#endif
|
||||
|
||||
pixels.clear();
|
||||
delay(500);
|
||||
pixels.show();
|
||||
delay(500);
|
||||
}
|
||||
|
||||
bno = Adafruit_BNO055(55, 0x28, &Wire);
|
||||
/* Initialise the sensor */
|
||||
if(!bno.begin(OPERATION_MODE_NDOF)) {
|
||||
//if(!bno.begin(OPERATION_MODE_AMG)) {
|
||||
/* There was a problem detecting the BNO055 ... check your connections */
|
||||
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
|
||||
while(1);
|
||||
Serial.println("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
|
||||
error_blink();
|
||||
}
|
||||
|
||||
delay(1000);
|
||||
|
||||
/* Use external crystal for better accuracy? */
|
||||
bno.setExtCrystalUse(false);
|
||||
|
||||
bno.setExtCrystalUse(true);
|
||||
|
||||
// WIFI init
|
||||
WiFi.mode(WIFI_STA);
|
||||
if (esp_now_init() != ESP_OK) {
|
||||
Serial.println("Error initializing ESP-NOW");
|
||||
return;
|
||||
error_blink();
|
||||
}
|
||||
|
||||
memcpy(peerInfo.peer_addr, sprejemnikMac, 6);
|
||||
peerInfo.channel = 0;
|
||||
peerInfo.channel = 1;
|
||||
peerInfo.encrypt = false;
|
||||
|
||||
if (esp_now_add_peer(&peerInfo) != ESP_OK){
|
||||
Serial.println("WIFI registracija ni uspela");
|
||||
return;
|
||||
error_blink();
|
||||
}
|
||||
|
||||
// Running - led on!
|
||||
pixels.setPixelColor(0, pixels.Color(255, 255, 255));
|
||||
pixels.show();
|
||||
|
||||
// Nastavi ID senzorja
|
||||
odcitek.id = SENSOR_ID;
|
||||
|
||||
pinMode(BATTERY_PIN, INPUT);
|
||||
|
||||
// Initial time
|
||||
cas = millis();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
/* Get a new sensor event */
|
||||
//sensors_event_t event;
|
||||
//bno.getEvent(&event);
|
||||
// Read battery (from example T7 sketch)
|
||||
uint32_t readADC_Cal(int ADC_Raw)
|
||||
{
|
||||
esp_adc_cal_characteristics_t adc_chars;
|
||||
|
||||
esp_adc_cal_characterize(ADC_UNIT_1, ADC_ATTEN_DB_11, ADC_WIDTH_BIT_13, 1100, &adc_chars);
|
||||
return (esp_adc_cal_raw_to_voltage(ADC_Raw, &adc_chars));
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// Get quat (fusion mode); 4 * 2bytes
|
||||
quat = bno.getQuat();
|
||||
odcitek.qX = quat.x();
|
||||
|
|
@ -100,13 +146,13 @@ void loop() {
|
|||
|
||||
#ifdef DEBUG
|
||||
Serial.print(F("Quat: "));
|
||||
Serial.print((float)odcitek.qX);
|
||||
Serial.print(quat.x());
|
||||
Serial.print(F(" "));
|
||||
Serial.print((float)odcitek.qY);
|
||||
Serial.print(quat.y());
|
||||
Serial.print(F(" "));
|
||||
Serial.print((float)odcitek.qZ);
|
||||
Serial.print(quat.z());
|
||||
Serial.print(F(" "));
|
||||
Serial.print((float)odcitek.qW);
|
||||
Serial.print(quat.w());
|
||||
Serial.println("");
|
||||
#endif
|
||||
|
||||
|
|
@ -116,7 +162,15 @@ void loop() {
|
|||
// @TODO use something more precise, like https://github.com/rlogiacco/BatterySense ?
|
||||
if (millis() - cas > 1000) {
|
||||
cas = millis();
|
||||
odcitek.bat = analogRead(BATTERYPIN) * (1.1 / 8192);
|
||||
//odcitek.bat = (float) analogRead(BATTERY_PIN) * (1.1 / 8192);
|
||||
//odcitek.bat = (float) (readADC_Cal(analogRead(BATTERY_PIN)) * 2) / 1000;
|
||||
odcitek.bat = (float) (analogReadMilliVolts(BATTERY_PIN) * 2) / 1000;
|
||||
|
||||
#ifdef DEBUG
|
||||
Serial.print(F("Battery: "));
|
||||
Serial.print((float)odcitek.bat);
|
||||
Serial.println("");
|
||||
#endif
|
||||
}
|
||||
|
||||
// Get linear acceleration (3 * 2bytes)
|
||||
20
src/main.cpp
20
src/main.cpp
|
|
@ -1,8 +1,9 @@
|
|||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
#include "esp_adc_cal.h"
|
||||
|
||||
// ID senzorja mora bit unikaten! (se poslje poleg parametrov)
|
||||
#define SENSOR_ID 1
|
||||
#define SENSOR_ID 5
|
||||
|
||||
// Stanje baterije
|
||||
#define BATTERY_PIN 2
|
||||
|
|
@ -15,7 +16,7 @@
|
|||
#define LED_PIN 17
|
||||
|
||||
// debagiranje!
|
||||
#define DEBUG
|
||||
//#define DEBUG
|
||||
|
||||
// I²C pins
|
||||
#define SDA_PIN 8
|
||||
|
|
@ -69,9 +70,9 @@ void setup() {
|
|||
for (int i = 0; i < 3; i++) {
|
||||
Serial.println(i + 1);
|
||||
digitalWrite(LED_PIN, LOW);
|
||||
delay(1000);
|
||||
delay(500);
|
||||
digitalWrite(LED_PIN, HIGH);
|
||||
delay(1000);
|
||||
delay(500);
|
||||
}
|
||||
|
||||
bno = Adafruit_BNO055(55, 0x28, &Wire);
|
||||
|
|
@ -114,6 +115,15 @@ void setup() {
|
|||
cas = millis();
|
||||
}
|
||||
|
||||
// Read battery (from example T7 sketch)
|
||||
uint32_t readADC_Cal(int ADC_Raw)
|
||||
{
|
||||
esp_adc_cal_characteristics_t adc_chars;
|
||||
|
||||
esp_adc_cal_characterize(ADC_UNIT_1, ADC_ATTEN_DB_11, ADC_WIDTH_BIT_12, 1100, &adc_chars);
|
||||
return (esp_adc_cal_raw_to_voltage(ADC_Raw, &adc_chars));
|
||||
}
|
||||
|
||||
void loop() {
|
||||
/* Get a new sensor event */
|
||||
//sensors_event_t event;
|
||||
|
|
@ -144,7 +154,7 @@ void loop() {
|
|||
// @TODO use something more precise, like https://github.com/rlogiacco/BatterySense ?
|
||||
if (millis() - cas > 1000) {
|
||||
cas = millis();
|
||||
odcitek.bat = analogRead(BATTERY_PIN) * (1.1 / 8192);
|
||||
odcitek.bat = (float) (readADC_Cal(analogRead(BATTERY_PIN)) * 2) / 1000;
|
||||
}
|
||||
|
||||
// Get linear acceleration (3 * 2bytes)
|
||||
|
|
|
|||
|
|
@ -1,43 +1,36 @@
|
|||
#include <Arduino.h>
|
||||
#include <esp_now.h>
|
||||
#include <esp_wifi.h>
|
||||
#include <WiFi.h>
|
||||
|
||||
// WIFI paket
|
||||
#include "sensor_msg.h"
|
||||
|
||||
#include <OSCBundle.h>
|
||||
#include <OSCBoards.h>
|
||||
|
||||
#include <Adafruit_NeoPixel.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 don’t have Serial
|
||||
#endif
|
||||
// RGB LED
|
||||
#define NUMPIXELS 1
|
||||
#define PIXELPIN 18
|
||||
Adafruit_NeoPixel pixels(NUMPIXELS, PIXELPIN, NEO_GRB + NEO_KHZ800);
|
||||
|
||||
#include "SLIPEncodedSerial.h"
|
||||
SLIPEncodedUSBSerial SLIPSerial(Serial);
|
||||
|
||||
// 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;
|
||||
float aX;
|
||||
float aY;
|
||||
float aZ;
|
||||
float qX;
|
||||
float qY;
|
||||
float qZ;
|
||||
float qW;
|
||||
} sensor_msg;
|
||||
|
||||
//sensor_msg odcitek;
|
||||
|
||||
// Maksimalno stevilo
|
||||
#define ST_KEGLOV 10
|
||||
#define ST_SPREJEMNIKOV 10
|
||||
|
||||
int odcitekId;
|
||||
sensor_msg odcitki[ST_KEGLOV];
|
||||
bool poslji[ST_KEGLOV];
|
||||
sensor_msg odcitki[ST_SPREJEMNIKOV];
|
||||
bool poslji[ST_SPREJEMNIKOV];
|
||||
|
||||
void prejemPodatkov(const uint8_t * mac_addr, const uint8_t * noviPodatki, int len) {
|
||||
|
||||
|
|
@ -75,13 +68,36 @@ void prejemPodatkov(const uint8_t * mac_addr, const uint8_t * noviPodatki, int l
|
|||
}
|
||||
|
||||
void setup() {
|
||||
// Nizja CPU frekvenca
|
||||
//setCpuFrequencyMhz(80);
|
||||
SLIPSerial.begin(115200);
|
||||
|
||||
// Ne posiljaj preden se podatki napolnijo
|
||||
for (int i = 0; i < ST_KEGLOV; i++) {
|
||||
poslji[0] = false;
|
||||
for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
|
||||
poslji[i] = false;
|
||||
}
|
||||
|
||||
// Init - 3 one second blinks
|
||||
pixels.begin();
|
||||
pixels.clear();
|
||||
pixels.setPixelColor(0, pixels.Color(0, 255, 0));
|
||||
pixels.setBrightness(100);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
|
||||
#ifdef DEBUG
|
||||
Serial.println(i + 1);
|
||||
#endif
|
||||
|
||||
pixels.clear();
|
||||
delay(500);
|
||||
pixels.show();
|
||||
delay(500);
|
||||
}
|
||||
|
||||
// vklopi LED!
|
||||
pixels.setPixelColor(0, pixels.Color(255, 255, 255));
|
||||
pixels.show();
|
||||
|
||||
Serial.println("Inicializiram WIFI...");
|
||||
WiFi.mode(WIFI_STA);
|
||||
|
||||
|
|
@ -91,6 +107,7 @@ void setup() {
|
|||
if (result != ESP_OK) {
|
||||
Serial.println("Error initializing ESP-NOW");
|
||||
Serial.println(result);
|
||||
pixels.setPixelColor(0, pixels.Color(255, 0, 0));
|
||||
return;
|
||||
}
|
||||
Serial.print("MAC naslov: ");
|
||||
|
|
@ -103,9 +120,10 @@ void loop() {
|
|||
OSCBundle bundle;
|
||||
char glava[32];
|
||||
|
||||
for (int i = 0; i < ST_KEGLOV; i++) {
|
||||
for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
|
||||
if (poslji[i]) {
|
||||
sprintf(glava, "/ww/%d/accel", i);
|
||||
// Accel in quaternion v bundlu skupaj z ostalim sta velika 396 bytov!
|
||||
sprintf(glava, "/ww/%d/acc", i);
|
||||
/*
|
||||
Serial.print("Posiljam ");
|
||||
Serial.println(glava);
|
||||
|
|
@ -115,7 +133,7 @@ void loop() {
|
|||
.add(odcitki[i].aY)
|
||||
.add(odcitki[i].aZ);
|
||||
|
||||
sprintf(glava, "/ww/%d/quaternion", i);
|
||||
sprintf(glava, "/ww/%d/quat", i);
|
||||
/*
|
||||
Serial.print("Posiljam ");
|
||||
Serial.println(glava);
|
||||
|
|
@ -137,6 +155,10 @@ void loop() {
|
|||
Serial.println();
|
||||
*/
|
||||
|
||||
sprintf(glava, "/ww/%d/bat", i);
|
||||
bundle.add(glava)
|
||||
.add(odcitki[i].bat);
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
bundle.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
|
@ -79,9 +79,9 @@ void setup() {
|
|||
for (int i = 0; i < 3; i++) {
|
||||
Serial.println(i + 1);
|
||||
digitalWrite(LED_PIN, LOW);
|
||||
delay(1000);
|
||||
delay(500);
|
||||
digitalWrite(LED_PIN, HIGH);
|
||||
delay(1000);
|
||||
delay(500);
|
||||
}
|
||||
|
||||
// ESP32S3 - vklopi LED!
|
||||
|
|
@ -111,7 +111,7 @@ void loop() {
|
|||
for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
|
||||
if (poslji[i]) {
|
||||
// Accel in quaternion v bundlu skupaj z ostalim sta velika 396 bytov!
|
||||
sprintf(glava, "/ww/%d/accel", i);
|
||||
sprintf(glava, "/ww/%d/acc", i);
|
||||
/*
|
||||
Serial.print("Posiljam ");
|
||||
Serial.println(glava);
|
||||
|
|
@ -121,7 +121,7 @@ void loop() {
|
|||
.add(odcitki[i].aY)
|
||||
.add(odcitki[i].aZ);
|
||||
|
||||
sprintf(glava, "/ww/%d/quaternion", i);
|
||||
sprintf(glava, "/ww/%d/quat", i);
|
||||
/*
|
||||
Serial.print("Posiljam ");
|
||||
Serial.println(glava);
|
||||
|
|
@ -143,6 +143,10 @@ void loop() {
|
|||
Serial.println();
|
||||
*/
|
||||
|
||||
sprintf(glava, "/ww/%d/bat", i);
|
||||
bundle.add(glava)
|
||||
.add(odcitki[i].bat);
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
bundle.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
|
|
|||
117
utopia.scd
117
utopia.scd
|
|
@ -5,38 +5,46 @@
|
|||
(
|
||||
|
||||
// Initialize the the receiver via SLIP decoder
|
||||
~receiverPath = "/dev/ttyACM0";
|
||||
~receiverPath = "/dev/ttyUSB0";
|
||||
~baudRate = 115200;
|
||||
|
||||
~decoder = SLIPDecoder.new(~receiverPath, ~baudRate);
|
||||
OSCFunc.trace(false); // debug osc
|
||||
~decoder.trace(true); // debug slip decoder
|
||||
OSCFunc.trace(true); // debug osc
|
||||
OSCFunc.trace(false);
|
||||
|
||||
/*******
|
||||
* GUI *
|
||||
******/
|
||||
~senzorji = [];
|
||||
~numSensors = 3;
|
||||
~numSensors.do({ |n|
|
||||
~senzorji.add(AHRSensor.new(n + 1));
|
||||
|
||||
// Indeksirani so z IDjem
|
||||
~senzorji = Dictionary.new;
|
||||
|
||||
// Dodaj senzorje
|
||||
((1..5)).do({ |id|
|
||||
~senzorji.put(id, AHRSensor.new(id));
|
||||
});
|
||||
|
||||
~w = Window.new("Utopia || C²", Rect(300, 300, 640, ~numSensors * 20),true);
|
||||
// Olimex test
|
||||
~senzorji.put(9, AHRSensor.new(9));
|
||||
|
||||
~elementi = ~senzorji.collect({|s| s.getGui;});
|
||||
//~senzorji.postln;
|
||||
|
||||
~ttyInput = TextField().string_("/dev/ttyACM0");
|
||||
~decoder = SLIPDecoder.new(~receiverPath);
|
||||
~w = Window.new("Utopia || C²", Rect(300, 300, 600, ~senzorji.size * 20),true);
|
||||
~ttyInput = TextField().string_(~receiverPath);
|
||||
|
||||
~elementi = ~senzorji.values.sort({ |a, b| a.id < b.id;}).collect({|s| s.getGui;});
|
||||
|
||||
~w.layout_(
|
||||
VLayout(
|
||||
HLayout(
|
||||
// HEADER; serial path, buttons
|
||||
StaticText().string_("Serial path: "),
|
||||
~ttyInput,
|
||||
Button().string_("Start").action_({ | butt |
|
||||
Button().string_("Start").action_({ |butt|
|
||||
if ((~decoder.running.not), {
|
||||
// If not running, start decoder
|
||||
~decoder.deviceName = "";
|
||||
~decoder = SLIPDecoder.new(~receiverPath, ~baudRate);
|
||||
~decoder.trace(true); // debug slip decoder
|
||||
~decoder = SLIPDecoder.new(~ttyInput.string, ~baudRate);
|
||||
~decoder.start;
|
||||
butt.string_("Stop")
|
||||
}, {
|
||||
|
|
@ -46,23 +54,76 @@ OSCFunc.trace(false); // debug osc
|
|||
});
|
||||
})
|
||||
),
|
||||
// Elementi senzorjev
|
||||
VLayout(*~elementi)
|
||||
[],
|
||||
// Sensor rows
|
||||
GridLayout.rows(
|
||||
*~elementi.flatten
|
||||
)
|
||||
)
|
||||
);
|
||||
|
||||
// On window close stop the decoder
|
||||
~w.onClose_({ |w|
|
||||
~decoder.stop;
|
||||
});
|
||||
~w.front;
|
||||
|
||||
q = OSCFunc({ |msg, time, addr, recvPort|
|
||||
// OSC listeners for sensors
|
||||
~senzorji.collect({ |s|
|
||||
var oscHeader = "/ww/" ++ s.id,
|
||||
senzor = ~senzorji[s.id];
|
||||
|
||||
// Quat listener
|
||||
q = OSCdef.new((\quat ++ s.id), { |msg, time, addr, recvPort|
|
||||
var q;
|
||||
q = Quaternion.new(msg[4], msg[1], msg[2], msg[3]);
|
||||
q.postln;
|
||||
//~senzorji[0].updateEuler(q);
|
||||
//~senzorji[0].euler.postln;
|
||||
//~elementi[0].refreshGui;
|
||||
}, "/ww/1/quaternion");
|
||||
|
||||
// Start the decoder!
|
||||
//~decoder.start;
|
||||
// Count quat events
|
||||
Routine {
|
||||
senzor.eps = senzor.eps + 1;
|
||||
senzor.quat = q;
|
||||
senzor.refreshGuiQuat;
|
||||
|
||||
senzor.updateEuler(q);
|
||||
senzor.refreshGuiEuler;
|
||||
senzor.euler.postln;
|
||||
|
||||
}.play(AppClock);
|
||||
|
||||
}, oscHeader ++ "/quat");
|
||||
|
||||
// Acceleration listener
|
||||
q = OSCdef.new((\acc ++ s.id), { |msg, time, addr, recvPort|
|
||||
var a;
|
||||
a = msg.at((1..3));
|
||||
Routine {
|
||||
senzor.accel = a;
|
||||
senzor.refreshGuiAccel;
|
||||
}.play(AppClock);
|
||||
}, oscHeader ++ "/acc");
|
||||
|
||||
// Battery listener
|
||||
q = OSCdef.new((\acc ++ s.id), { |msg, time, addr, recvPort|
|
||||
var b = msg[1];
|
||||
// Since we get a battery reading every second, use this as the events/s refresh
|
||||
Routine {
|
||||
senzor.battery = b;
|
||||
senzor.refreshGuiBat;
|
||||
}.play(AppClock);
|
||||
}, oscHeader ++ "/bat");
|
||||
|
||||
});
|
||||
|
||||
// Events per second counter
|
||||
AppClock.clear;
|
||||
AppClock.sched(0, {
|
||||
~senzorji.collect({ |s|
|
||||
s.refreshGuiEps;
|
||||
s.eps = 0;
|
||||
// Do this every second
|
||||
});
|
||||
1;
|
||||
});
|
||||
)
|
||||
|
||||
|
||||
|
|
@ -70,5 +131,13 @@ q = OSCFunc({ |msg, time, addr, recvPort|
|
|||
~decoder.stop;
|
||||
~decoder.start;
|
||||
|
||||
OSCFunc.freeAll;
|
||||
|
||||
~decoder.rate;
|
||||
|
||||
/**********
|
||||
* SOUND! *
|
||||
*********/
|
||||
|
||||
~e = Env([1, 0.2, 0]);
|
||||
{[SinOsc.ar(50), SinOsc.ar(52.3)] * EnvGen.kr(~e, doneAction: Done.freeSelf)}.play;
|
||||
Loading…
Reference in New Issue