g1smo
/
c2-utopia
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Ciscenje, delujoc acc in quat, WIP euler konverzija, WIP olimex sketch

main
Jurij Podgoršek 2024-09-06 18:19:53 +02:00
parent b3e37f57a7
commit d20c552ee7
14 changed files with 394 additions and 92922 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

156
AHRSensor/AHRSensor.sc
View File

@ -5,11 +5,27 @@ AHRSensor {
<>euler, <>euler,
<>accel, <>accel,
<battery, <>battery,
<>eps,
// GUI elementi
// Quaternion
<gQw,
<gQx,
<gQy,
<gQz,
// Eulerjevi koti
<gEx, <gEx,
<gEy, <gEy,
<gEz; <gEz,
// Pospeskomer
<gAx,
<gAy,
<gAz,
// Baterija
<gB,
// Dogodkov na sekundo
<gEps;
*new { |id| *new { |id|
^super.newCopyArgs(id).init; ^super.newCopyArgs(id).init;
@ -24,30 +40,29 @@ AHRSensor {
wx = w * x2, wy = w * y2, wz = w * z2, wx = w * x2, wy = w * y2, wz = w * z2,
matrix = [ matrix = [
1 - ( yy + zz ), xy + wz, xz - wy, 0, 1 - ( yy + zz ), xy + wz, xz - wy, 0,
xy - wz, 1 - ( xx + zz ), yz + wx, 0, xy - wz, 1 - ( xx + zz ), yz + wx, 0,
xz + wy, yz - wx, 1 - ( xx + yy ), 0, xz + wy, yz - wx, 1 - ( xx + yy ), 0,
0, 0, 0, 1 0, 0, 0, 1
]; ];
var m11 = matrix[0], m12 = matrix[4], m13 = matrix[8]; var m11 = matrix[0], m12 = matrix[4], m13 = matrix[8];
var m21 = matrix[1], m22 = matrix[5], m23 = matrix[9]; var m21 = matrix[1], m22 = matrix[5], m23 = matrix[9];
var m31 = matrix[2], m32 = matrix[6], m33 = matrix[10]; 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), { if ((m13.abs < 0.9999999), {
euler[0] = atan2(m23.neg, m33); e[0] = atan2(m23.neg, m33);
euler[2] = atan2(m12.neg, m11); e[2] = atan2(m12.neg, m11);
}, { }, {
euler[0] = atan2(m32, m22); e[0] = atan2(m32, m22);
euler[2] = 0; e[2] = 0;
}); });
euler.postln; ^e;
euler;
} }
init { init {
@ -56,6 +71,7 @@ AHRSensor {
euler = [0, 0, 0]; euler = [0, 0, 0];
accel = [0, 0, 0]; accel = [0, 0, 0];
battery = 0; battery = 0;
eps = 0;
this.guiInit; this.guiInit;
} }
@ -74,31 +90,117 @@ AHRSensor {
gEx = StaticText().string_(0).stringColor_(cRed); gEx = StaticText().string_(0).stringColor_(cRed);
gEy = StaticText().string_(0).stringColor_(cGreen); gEy = StaticText().string_(0).stringColor_(cGreen);
gEz = StaticText().string_(0).stringColor_(cBlue); 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 { 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_("euler: "),
[StaticText().string_("x: "), align: \left], [StaticText().string_("x: "), align: \right],
[gEx, align: \left], gEx,
StaticText().string_("y: "), [StaticText().string_("y: "), align: \right],
gEy, gEy,
StaticText().string_("z: "), [StaticText().string_("z: "), align: \right],
gEz, gEz,
) ],
); [
StaticText().string_("accel: "),
[StaticText().string_("x: "), align: \right],
gAx,
[StaticText().string_("y: "), align: \right],
gAy,
[StaticText().string_("z: "), align: \right],
gAz,
],
[]
];
} }
updateEuler { |newQuat| updateEuler { |newQuat|
var quatDiff = newQuat / quat; 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 { refreshGui {
gEx.string_(euler[0]); this.refreshGuiQuat;
gEy.string_(euler[1]); this.refreshGuiEuler;
gEz.string_(euler[2]); this.refreshGuiAccel;
this.refreshGuiBat;
} }
} }

180
osc32_9255_rtimulib_espnow/SLIPEncodedBluetoothSerial.cpp
View File

@ -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();
}

62
osc32_9255_rtimulib_espnow/SLIPEncodedBluetoothSerial.h
View File

@ -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

14
osc32_espnow_sprejemnik/debug.cfg
View File

@ -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]

46087
osc32_espnow_sprejemnik/debug.svd
View File

File diff suppressed because it is too large Load Diff

19
osc32_espnow_sprejemnik/debug_custom.json
View File

@ -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"
]
}

46087
osc32_espnow_sprejemnik/esp32.svd
View File

File diff suppressed because it is too large Load Diff

15
platformio.ini
View File

@ -22,15 +22,24 @@ build_src_filter =
+<main.cpp> +<main.cpp>
;; Olimex prototype sketch ;; Olimex prototype sketch
[env:ada] [env:main-olimex]
board = esp32-s2-saola-1
build_src_filter = build_src_filter =
+<ada.cpp> +<main-olimex.cpp>
board = esp32-s2-saola-1
upload_speed = 460800
build_flags =
[env:sprejemnik] [env:sprejemnik]
build_src_filter = build_src_filter =
+<sprejemnik.cpp> +<sprejemnik.cpp>
[env:sprejemnik-olimex]
build_src_filter =
+<sprejemnik-olimex.cpp>
board = esp32-s2-saola-1
upload_speed = 460800
build_flags =
[env:scanner] [env:scanner]
build_src_filter = build_src_filter =
+<scanner.cpp> +<scanner.cpp>

349
server.js
View File

@ -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()
}
*/

114
src/ada.cpp → src/main-olimex.cpp
View File

@ -1,23 +1,30 @@
#include <Arduino.h> #include <Arduino.h>
#include <Wire.h> #include <Wire.h>
#include <Adafruit_NeoPixel.h>
#include "esp_adc_cal.h"
// ID kegla mora bit unikaten za vsakega! (se poslje poleg parametrov) // ID senzorja mora bit unikaten! (se poslje poleg parametrov)
#define SENSOR_ID 1 #define SENSOR_ID 9
// I²C
#define SDApin 2
#define SCLpin 3
// Stanje baterije // 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 // IMU library
#include <SPI.h> #include <SPI.h>
#include <Adafruit_BNO055.h> #include <Adafruit_BNO055.h>
/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (10)
// ESPNOW WIFI package structure // ESPNOW WIFI package structure
#include "sensor_msg.h" #include "sensor_msg.h"
@ -26,7 +33,9 @@
#include <WiFi.h> #include <WiFi.h>
// MAC naslov sprejemnika // 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; sensor_msg odcitek;
esp_now_peer_info_t peerInfo; esp_now_peer_info_t peerInfo;
@ -39,58 +48,95 @@ imu::Vector<3> linearAccel;
int cas = 0; 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() { 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...");
Wire.begin(SDApin, SCLpin); Wire.begin(SDA_PIN, SCL_PIN);
// Fast mode // Fast mode
Wire.setClock(400000); Wire.setClock(400000);
bno = Adafruit_BNO055(55, 0x28, &Wire);
/* Initialise the sensor */ // 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_NDOF)) {
//if(!bno.begin(OPERATION_MODE_AMG)) { //if(!bno.begin(OPERATION_MODE_AMG)) {
/* There was a problem detecting the BNO055 ... check your connections */ /* There was a problem detecting the BNO055 ... check your connections */
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); Serial.println("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while(1); error_blink();
} }
delay(1000); delay(1000);
/* Use external crystal for better accuracy? */ /* Use external crystal for better accuracy? */
bno.setExtCrystalUse(false); bno.setExtCrystalUse(true);
// WIFI init // WIFI init
WiFi.mode(WIFI_STA); WiFi.mode(WIFI_STA);
if (esp_now_init() != ESP_OK) { if (esp_now_init() != ESP_OK) {
Serial.println("Error initializing ESP-NOW"); Serial.println("Error initializing ESP-NOW");
return; error_blink();
} }
memcpy(peerInfo.peer_addr, sprejemnikMac, 6); memcpy(peerInfo.peer_addr, sprejemnikMac, 6);
peerInfo.channel = 0; peerInfo.channel = 1;
peerInfo.encrypt = false; peerInfo.encrypt = false;
if (esp_now_add_peer(&peerInfo) != ESP_OK){ if (esp_now_add_peer(&peerInfo) != ESP_OK){
Serial.println("WIFI registracija ni uspela"); 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 // Nastavi ID senzorja
odcitek.id = SENSOR_ID; odcitek.id = SENSOR_ID;
pinMode(BATTERY_PIN, INPUT);
// Initial time // Initial time
cas = millis(); cas = millis();
} }
void loop() { // Read battery (from example T7 sketch)
/* Get a new sensor event */ uint32_t readADC_Cal(int ADC_Raw)
//sensors_event_t event; {
//bno.getEvent(&event); 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 // Get quat (fusion mode); 4 * 2bytes
quat = bno.getQuat(); quat = bno.getQuat();
odcitek.qX = quat.x(); odcitek.qX = quat.x();
@ -100,13 +146,13 @@ void loop() {
#ifdef DEBUG #ifdef DEBUG
Serial.print(F("Quat: ")); Serial.print(F("Quat: "));
Serial.print((float)odcitek.qX); Serial.print(quat.x());
Serial.print(F(" ")); Serial.print(F(" "));
Serial.print((float)odcitek.qY); Serial.print(quat.y());
Serial.print(F(" ")); Serial.print(F(" "));
Serial.print((float)odcitek.qZ); Serial.print(quat.z());
Serial.print(F(" ")); Serial.print(F(" "));
Serial.print((float)odcitek.qW); Serial.print(quat.w());
Serial.println(""); Serial.println("");
#endif #endif
@ -116,7 +162,15 @@ void loop() {
// @TODO use something more precise, like https://github.com/rlogiacco/BatterySense ? // @TODO use something more precise, like https://github.com/rlogiacco/BatterySense ?
if (millis() - cas > 1000) { if (millis() - cas > 1000) {
cas = millis(); 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) // Get linear acceleration (3 * 2bytes)

20
src/main.cpp
View File

@ -1,8 +1,9 @@
#include <Arduino.h> #include <Arduino.h>
#include <Wire.h> #include <Wire.h>
#include "esp_adc_cal.h"
// ID senzorja mora bit unikaten! (se poslje poleg parametrov) // ID senzorja mora bit unikaten! (se poslje poleg parametrov)
#define SENSOR_ID 1 #define SENSOR_ID 5
// Stanje baterije // Stanje baterije
#define BATTERY_PIN 2 #define BATTERY_PIN 2
@ -15,7 +16,7 @@
#define LED_PIN 17 #define LED_PIN 17
// debagiranje! // debagiranje!
#define DEBUG //#define DEBUG
// I²C pins // I²C pins
#define SDA_PIN 8 #define SDA_PIN 8
@ -69,9 +70,9 @@ void setup() {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
Serial.println(i + 1); Serial.println(i + 1);
digitalWrite(LED_PIN, LOW); digitalWrite(LED_PIN, LOW);
delay(1000); delay(500);
digitalWrite(LED_PIN, HIGH); digitalWrite(LED_PIN, HIGH);
delay(1000); delay(500);
} }
bno = Adafruit_BNO055(55, 0x28, &Wire); bno = Adafruit_BNO055(55, 0x28, &Wire);
@ -114,6 +115,15 @@ void setup() {
cas = millis(); 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() { void loop() {
/* Get a new sensor event */ /* Get a new sensor event */
//sensors_event_t event; //sensors_event_t event;
@ -144,7 +154,7 @@ void loop() {
// @TODO use something more precise, like https://github.com/rlogiacco/BatterySense ? // @TODO use something more precise, like https://github.com/rlogiacco/BatterySense ?
if (millis() - cas > 1000) { if (millis() - cas > 1000) {
cas = millis(); 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) // Get linear acceleration (3 * 2bytes)

78
osc32_espnow_sprejemnik/espnow_sprejemnik/espnow_sprejemnik.ino → src/sprejemnik-olimex.cpp
View File

@ -1,43 +1,36 @@
#include <Arduino.h>
#include <esp_now.h> #include <esp_now.h>
#include <esp_wifi.h> #include <esp_wifi.h>
#include <WiFi.h> #include <WiFi.h>
// WIFI paket
#include "sensor_msg.h"
#include <OSCBundle.h> #include <OSCBundle.h>
#include <OSCBoards.h> #include <OSCBoards.h>
#include <Adafruit_NeoPixel.h>
//#define DEBUG //#define DEBUG
#ifdef BOARD_HAS_USB_SERIAL // RGB LED
#include <SLIPEncodedUSBSerial.h> #define NUMPIXELS 1
SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB ); #define PIXELPIN 18
#else Adafruit_NeoPixel pixels(NUMPIXELS, PIXELPIN, NEO_GRB + NEO_KHZ800);
#include <SLIPEncodedSerial.h>
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial #include "SLIPEncodedSerial.h"
#endif SLIPEncodedUSBSerial SLIPSerial(Serial);
// Set your new MAC Address // Set your new MAC Address
// MAC naslov sprejemnika: 08:3A:F2:50:EF:6C // MAC naslov sprejemnika: 08:3A:F2:50:EF:6C
uint8_t newMACAddress[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C}; 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 // Maksimalno stevilo
#define ST_KEGLOV 10 #define ST_SPREJEMNIKOV 10
int odcitekId; int odcitekId;
sensor_msg odcitki[ST_KEGLOV]; sensor_msg odcitki[ST_SPREJEMNIKOV];
bool poslji[ST_KEGLOV]; bool poslji[ST_SPREJEMNIKOV];
void prejemPodatkov(const uint8_t * mac_addr, const uint8_t * noviPodatki, int len) { 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() { void setup() {
// Nizja CPU frekvenca
//setCpuFrequencyMhz(80);
SLIPSerial.begin(115200); SLIPSerial.begin(115200);
// Ne posiljaj preden se podatki napolnijo // Ne posiljaj preden se podatki napolnijo
for (int i = 0; i < ST_KEGLOV; i++) { for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
poslji[0] = false; 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..."); Serial.println("Inicializiram WIFI...");
WiFi.mode(WIFI_STA); WiFi.mode(WIFI_STA);
@ -91,6 +107,7 @@ void setup() {
if (result != ESP_OK) { if (result != ESP_OK) {
Serial.println("Error initializing ESP-NOW"); Serial.println("Error initializing ESP-NOW");
Serial.println(result); Serial.println(result);
pixels.setPixelColor(0, pixels.Color(255, 0, 0));
return; return;
} }
Serial.print("MAC naslov: "); Serial.print("MAC naslov: ");
@ -103,9 +120,10 @@ void loop() {
OSCBundle bundle; OSCBundle bundle;
char glava[32]; char glava[32];
for (int i = 0; i < ST_KEGLOV; i++) { for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
if (poslji[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.print("Posiljam ");
Serial.println(glava); Serial.println(glava);
@ -115,7 +133,7 @@ void loop() {
.add(odcitki[i].aY) .add(odcitki[i].aY)
.add(odcitki[i].aZ); .add(odcitki[i].aZ);
sprintf(glava, "/ww/%d/quaternion", i); sprintf(glava, "/ww/%d/quat", i);
/* /*
Serial.print("Posiljam "); Serial.print("Posiljam ");
Serial.println(glava); Serial.println(glava);
@ -137,6 +155,10 @@ void loop() {
Serial.println(); Serial.println();
*/ */
sprintf(glava, "/ww/%d/bat", i);
bundle.add(glava)
.add(odcitki[i].bat);
SLIPSerial.beginPacket(); SLIPSerial.beginPacket();
bundle.send(SLIPSerial); bundle.send(SLIPSerial);
SLIPSerial.endPacket(); SLIPSerial.endPacket();

12
src/sprejemnik.cpp
View File

@ -79,9 +79,9 @@ void setup() {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
Serial.println(i + 1); Serial.println(i + 1);
digitalWrite(LED_PIN, LOW); digitalWrite(LED_PIN, LOW);
delay(1000); delay(500);
digitalWrite(LED_PIN, HIGH); digitalWrite(LED_PIN, HIGH);
delay(1000); delay(500);
} }
// ESP32S3 - vklopi LED! // ESP32S3 - vklopi LED!
@ -111,7 +111,7 @@ void loop() {
for (int i = 0; i < ST_SPREJEMNIKOV; i++) { for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
if (poslji[i]) { if (poslji[i]) {
// Accel in quaternion v bundlu skupaj z ostalim sta velika 396 bytov! // 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.print("Posiljam ");
Serial.println(glava); Serial.println(glava);
@ -121,7 +121,7 @@ void loop() {
.add(odcitki[i].aY) .add(odcitki[i].aY)
.add(odcitki[i].aZ); .add(odcitki[i].aZ);
sprintf(glava, "/ww/%d/quaternion", i); sprintf(glava, "/ww/%d/quat", i);
/* /*
Serial.print("Posiljam "); Serial.print("Posiljam ");
Serial.println(glava); Serial.println(glava);
@ -143,6 +143,10 @@ void loop() {
Serial.println(); Serial.println();
*/ */
sprintf(glava, "/ww/%d/bat", i);
bundle.add(glava)
.add(odcitki[i].bat);
SLIPSerial.beginPacket(); SLIPSerial.beginPacket();
bundle.send(SLIPSerial); bundle.send(SLIPSerial);
SLIPSerial.endPacket(); SLIPSerial.endPacket();

123
utopia.scd
View File

@ -5,38 +5,46 @@
( (
// Initialize the the receiver via SLIP decoder // Initialize the the receiver via SLIP decoder
~receiverPath = "/dev/ttyACM0"; ~receiverPath = "/dev/ttyUSB0";
~baudRate = 115200; ~baudRate = 115200;
~decoder = SLIPDecoder.new(~receiverPath, ~baudRate); OSCFunc.trace(true); // debug osc
OSCFunc.trace(false); // debug osc OSCFunc.trace(false);
~decoder.trace(true); // debug slip decoder
/******* /*******
* GUI * * GUI *
******/ ******/
~senzorji = [];
~numSensors = 3; // Indeksirani so z IDjem
~numSensors.do({ |n| ~senzorji = Dictionary.new;
~senzorji.add(AHRSensor.new(n + 1));
// 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_( ~w.layout_(
VLayout( VLayout(
HLayout( HLayout(
// HEADER; serial path, buttons
StaticText().string_("Serial path: "), StaticText().string_("Serial path: "),
~ttyInput, ~ttyInput,
Button().string_("Start").action_({ | butt | Button().string_("Start").action_({ |butt|
if ((~decoder.running.not), { if ((~decoder.running.not), {
// If not running, start decoder // If not running, start decoder
~decoder.deviceName = ""; ~decoder.trace(true); // debug slip decoder
~decoder = SLIPDecoder.new(~receiverPath, ~baudRate); ~decoder = SLIPDecoder.new(~ttyInput.string, ~baudRate);
~decoder.start; ~decoder.start;
butt.string_("Stop") 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; ~w.front;
q = OSCFunc({ |msg, time, addr, recvPort| // OSC listeners for sensors
var q; ~senzorji.collect({ |s|
q = Quaternion.new(msg[4], msg[1], msg[2], msg[3]); var oscHeader = "/ww/" ++ s.id,
q.postln; senzor = ~senzorji[s.id];
//~senzorji[0].updateEuler(q);
//~senzorji[0].euler.postln;
//~elementi[0].refreshGui;
}, "/ww/1/quaternion");
// Start the decoder! // Quat listener
//~decoder.start; q = OSCdef.new((\quat ++ s.id), { |msg, time, addr, recvPort|
var q;
q = Quaternion.new(msg[4], msg[1], msg[2], msg[3]);
// 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.stop;
~decoder.start; ~decoder.start;
OSCFunc.freeAll;
~decoder.rate; ~decoder.rate;
/**********
* SOUND! *
*********/
~e = Env([1, 0.2, 0]);
{[SinOsc.ar(50), SinOsc.ar(52.3)] * EnvGen.kr(~e, doneAction: Done.freeSelf)}.play;