From f924d667f7197c1ba994252dfc11fc72372555ac Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= <jurij@kompot.si>
Date: Fri, 23 Aug 2024 18:31:48 +0200
Subject: [PATCH] Preciscena koda, popravljen reset, resetiraj na unmute

---
 danijela.js | 98 ++++++++++++++++++++++++++++++++++-------------------
 1 file changed, 63 insertions(+), 35 deletions(-)

diff --git a/danijela.js b/danijela.js
index 27b1b36..d754c76 100644
--- a/danijela.js
+++ b/danijela.js
@@ -12,6 +12,7 @@ const midiKanalSenzor = {
   3: 22
 }
 
+// Kanal za hitrost podatkov
 const senzorCCKanal = {
   1: 75,
   2: 76,
@@ -35,7 +36,8 @@ const DEBUG = {
   osc: false
 }
 
-const batMin = 0.09
+// 0.095—0.133 je razpon baterije
+const batMin = 0.095
 const batMax = 0.133
 
 const procentBaterije = baterija => Math.round((baterija - batMin) / (batMax - batMin) * 100)
@@ -46,6 +48,7 @@ const izhodisceQ = {
   2: new Quat(),
   3: new Quat()
 }
+let kalibriraj = true
 
 const prejsnjiQ = {
   1: new Quat(),
@@ -59,22 +62,38 @@ const eulerRotacija = {
   3: [0, 0, 0]
 }
 
+// Roll/pitch/yaw rotacija (sredina je 64, sinusoidno se giblje gor in dol)
+const rotacijaMidi = {
+  1: {
+    r: 0,
+    p: 0,
+    y: 0
+  },
+  2: {
+    r: 0,
+    p: 0,
+    y: 0
+  },
+  3: {
+    r: 0,
+    p: 0,
+    y: 0
+  }
+}
+// Poslji samo spremembe!
+const staraRotacijaMidi = JSON.parse(JSON.stringify(rotacijaMidi))
+
+// Vrednost hitrost podatkov (kako hitro se vrti?)
 const senzorCC = {
   1: 64,
   2: 64,
   3: 64
 }
 
-let staroVrtenjeZMidi = {
-  1: 0,
-  2: 0,
-  3: 0
-};
-
 const muteMidi = {
-  1: 0,
-  2: 0,
-  3: 0
+  1: false,
+  2: false,
+  3: false
 }
 
 // Stetje prejetih paketov (na sekundo)
@@ -89,26 +108,30 @@ mi.openVirtualPort("danijela-midi")
 mi.on('message', (deltaTime, msg) => {
   if (msg[0] === midiKanal) {
     if (Object.values(senzorCCKanal).includes(msg[1])) {
+      // Hitrost podatkov MIDI
       const s = Object.values(senzorCCKanal).indexOf(msg[1]) + 1
       if (!muteMidi[s]) {
         senzorCC[s] = msg[2]
         console.log(`senzor ${s}: ${msg[2]}`)
       }
     } else if (Object.values(senzorCCMute).includes(msg[1])) {
+      // (Un)Mute kanala MIDI
       const s = Object.values(senzorCCMute).indexOf(msg[1]) + 1
       if (msg[2]) {
         console.log(`senzor UNMUTE ${s}`)
         muteMidi[s] = false;
+        // resetiraj izhodisce quaterniona
+        izhodisceQ[s] = prejsnjiQ[s].mul(izhodisceQ[s])
+        console.log(`senzor KALIBRACIJA ${s}`)
       } else {
+        console.log(`senzor MUTE ${s} (${muteMidi[s]})`)
         muteMidi[s] = true;
-        console.log(`senzor MUTE ${s}`)
       }
     } else if (Object.values(senzorCCkalibracija).includes(msg[1])) {
+      // Kalibracija senzorja MIDI (ponastavi izhodisce)
       const s = Object.values(senzorCCkalibracija).indexOf(msg[1]) + 1
       // resetiraj izhodisce quaterniona
-      izhodisceQ[s] = prejsnjiQ[s].conjugate()
-      // nastavi CC vrednost na sredino - 64
-	      senzorCC[s] = 64;
+      izhodisceQ[s] = prejsnjiQ[s].mul(izhodisceQ[s])
       console.log(`senzor KALIBRACIJA ${s}`)
     }
   }
@@ -208,34 +231,40 @@ oscWS.on('message', ({ address, args }) => {
       //console.log('IDX', index)
       // Izracunaj spremembo rotacije
       const novQ = new Quat({w: args[0], x: args[1], y: args[2], z: args[3]})
-      const novQC = novQ.mul(izhodisceQ[index])
+
+      // Prvi podatek = izhodiscni polozaj
+      if (kalibriraj) {
+        izhodisceQ[index] = novQ
+        kalibriraj = false
+      }
+
+      const novQC = novQ.div(izhodisceQ[index])
       const qWd = novQC.div(prejsnjiQ[index])
       prejsnjiQ[index] = novQC;
 
       const eulerD = qWd.toEuler()
       //console.log(index, eulerD)
-      eulerRotacija[index][0] += eulerD.roll * (senzorCC[index] / 64);
-      eulerRotacija[index][1] += eulerD.pitch * (senzorCC[index] / 64);
-      eulerRotacija[index][2] += eulerD.yaw * (senzorCC[index] / 64);
+      //eulerRotacija[index][0] += eulerD.roll * (senzorCC[index] / 64)
+      //eulerRotacija[index][1] += eulerD.pitch * (senzorCC[index] / 64)
+      eulerRotacija[index][2] += eulerD.yaw * (senzorCC[index] / 64)
 
-      const vrtenjeX = Math.abs(Math.sin(eulerRotacija[index][0]));
-      /*
-      posljiMidi(180, 20, Math.round(vrtenjeX * 127));
-      */
+      // roll (os  vzdolz senzorja)
+      //const rotacijaR = normSin(eulerRotacija[index][0])
 
-      // Roka dol/gor
-      const vrtenjeY = Math.abs(Math.sin(eulerRotacija[index][1]));
-      //posljiMidi(180, 20, Math.round(vrtenjeY * 127));
-      //posljiMidi(181, 20, Math.round(vrtenjeY * 127));
+      // Pitch (ce je na ravno iztegnjeni roki, gor/dol)
+      //const rotacijaP = normSin(eulerRotacija[index][1])
 
+      // Yaw (ce je na ravno iztegnjeni roki, vrtenje po osi pokoncnega telesa)
+      const rotacijaY = normSin(eulerRotacija[index][2])
 
-      const vrtenjeZ = normSin(eulerRotacija[index][2])
-      const vrtenjeZmidi = Math.round(vrtenjeZ * 127)
+      // Samo yaw trenutno gledamo
+      if (!muteMidi[index]) {
+        rotacijaMidi[index].y = Math.round(rotacijaY * 127)
 
-      if (vrtenjeZmidi !== staroVrtenjeZMidi[index]
-         && !muteMidi[index]) {
-        posljiMidi(182, midiKanalSenzor[index], vrtenjeZmidi)
-        staroVrtenjeZMidi[index] = vrtenjeZmidi
+        if (rotacijaMidi[index].y !== staraRotacijaMidi[index].y) {
+          staraRotacijaMidi[index].y = rotacijaMidi[index].y
+          posljiMidi(182, midiKanalSenzor[index], rotacijaMidi[index].y)
+        }
       }
 
       // Vsako sekundo shranimo stevec
@@ -244,11 +273,10 @@ oscWS.on('message', ({ address, args }) => {
         cas = Date.now()
         stPaketovNaSekundo = stPaketov;
         stPaketov = 0;
-        // 0.086 - 0.133 je razpon baterije
-        
       }
 
-      izpisiStanje(vrtenjeZmidi, baterija, stPaketovNaSekundo)
+      // Izpisi stanje (ce je mute, zadnja vrednost)
+      izpisiStanje(rotacijaMidi[index].y, baterija, stPaketovNaSekundo)
     }
   }
 })