#+TITLE: Krmilnik v keglu

* Vključene knjižnice, definicije
** Nastavljive zadeve
Ker imamo več keglov, najprej povemo, za katerega gre. Za vsak kegel je treba cifro prilagoditi. Mogoče bi nekoč lahko koncipirali boljši sistem dodajanja keglov.

#+begin_src arduino
#define KEGEL_ID 1
#+end_src

Navesti moramo tudi MAC naslov sprejemnika. Mora se skladati s kodo sprejemnika!

#+begin_src arduino
uint8_t sprejemnikMac[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };
#+end_src

Interval pošiljanja odčitkov senzorjev (v milisekundah).

#+begin_src arduino
#define DISPLAY_INTERVAL  5                         // interval between pose displays
#+end_src

** i2c
Senzorske vrednosti beremo preko i2c protokola, za kar skrbi knjižnica ~Wire.h~.

#+begin_src arduino
#include <Wire.h>
#+end_src

** Razbiranje orientacije
Sledeče knjižnice so potrebne za branje senzorja, vključene iz zdaj žal ne več aktivno razvite knjižnice [[https://github.com/RTIMULib/RTIMULib-Arduino][RTIMULib-Arduino]]. Prenesemo zadnjo verzijo in vse direktorije znotraj ~libraries~ skopiramo v ~~/Arduino/libraries~. Nato odkomentiramo primerni senzor v ~~/Arduino/libraries/RTIMULib/RTIMULibDefs.h~, torej ~#define MPU9250_68~. Vsi ostali naj ostanejo odkomentirani!

#+begin_src arduino
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h" 
#include "CalLib.h"
#+end_src

** Kalibracijski podatki

Kegel moramo pred uporabo kalibrirati, s [[file:~/projekti/kegel/ArduinoMagCal/ArduinoMagCal.ino][to skico]]. S tipko ~s~ shranimo parametre kalibracije v ~EEPROM~, od koder ga ta skica kasneje prebere.

#+begin_src arduino
#include <EEPROM.h>
#+end_src

** Brezžično povezovanje

Potrebujemo pa tudi knjižnice za brezžično pošiljanje preko WiFi-ja, z ~ESP-NOW~ komunikacijskim protokolom.

#+begin_src arduino
#include <esp_now.h>
#include <WiFi.h>
#+end_src

* Podatkovne strukture
** Paket senzorskih podatkov
Vsak brezžični paket pošlje ID kegla, razbran pospešek (z odšteto gravitacijo) po X, Y in Z oseh in pa kvaternion, ki opisuje orientacijo (X Y Z W).

Rezerviramo eno spremenljivko za odčitek senzorja.

#+begin_src arduino
typedef struct sensor_msg {
  int id;
  RTFLOAT aX;
  RTFLOAT aY;
  RTFLOAT aZ;
  RTFLOAT qX;
  RTFLOAT qY;
  RTFLOAT qZ;
  RTFLOAT qW;
} sensor_msg;

sensor_msg odcitek;
#+end_src

** Objekti za gibalni senzor

#+begin_src arduino
RTIMU *imu;             // Podatki o senzorju
RTFusionRTQF fusion;    // fuzijski objekt
RTIMUSettings settings; // nastavitve senzorja
RTQuaternion gravity;   // kvaternion za smer gravitacije
#+end_src

Števci za ritem pošiljanja paketov.

#+begin_src arduino
unsigned long lastDisplay;
unsigned long lastRate;
int sampleCount;
#+end_src

** Podatki o brezžični povezavi

#+begin_src arduino
esp_now_peer_info_t peerInfo;
#+end_src

* Vzpostavitev

Najprej rezerviramo prostor za razbiranje napake in vzpostavimo serijsko povezavo.

#+begin_src arduino
void setup() {
  int errcode; // Prostor za napako

  // Serijski port na navedeni hitrosti
  Serial.begin(115200);
  // Obvestimo o začenjanju
  Serial.println("Starting up...");
#+end_src

Vklopimo tudi EEPROM razbiranje ter i2c povezavo s senzorjem.

#+begin_src arduino
  // Init EEPROM based on magnet calibration size requirement
  EEPROM.begin(512);
  
  // I2C init
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#+end_src

Nato ustvarimo objekt za gibalni senzor in obvestimo o morebitni napaki.

#+begin_src arduino
  // create the imu object
  imu = RTIMU::createIMU(&settings);

  Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName());
  if ((errcode = imu->IMUInit()) < 0) {
    Serial.print("Failed to init IMU: "); Serial.println(errcode);
  }
#+end_src

Razberemo kalibracijo magnetometra.

#+begin_src arduino
  if (imu->getCalibrationValid())
    Serial.println("Using compass calibration");
  else
    Serial.println("No valid compass calibration data");
#+end_src

In pa nastavimo izhodiščno (nevtralno) smer gravitacije.

#+begin_src arduino
  gravity.setScalar(0);
  gravity.setX(0);
  gravity.setY(0);
  gravity.setZ(1);
#+end_src

Števce nastavimo na izhodiščni čas.

#+begin_src arduino
  lastDisplay = lastRate = millis();
  sampleCount = 0;
#+end_src

WIFI nastavimo na ESP-NOW.

#+begin_src arduino
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
#+end_src

Nato nastavimo v začetku definiran MAC naslov sprejemnika, wifi kanal ter nastavimo nekriptiran način delovanja.

#+begin_src arduino
  memcpy(peerInfo.peer_addr, sprejemnikMac, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;

  if (esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("WIFI registracija ni uspela");
    return;
  }
}
#+end_src

* Zanka delovanja

TODO opis zanke!

#+begin_src arduino
void loop() {
  unsigned long now = millis();
  unsigned long delta;
  RTVector3 realAccel;
  RTQuaternion rotatedGravity;
  RTQuaternion fusedConjugate;
  RTQuaternion qTemp;
  int loopCount = 0;

  // get the latest data if ready yet
  while (imu->IMURead()) {          
    // this flushes remaining data in case we are falling behind
    if (++loopCount >= 10)
      continue;

    fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
    
    //  do gravity rotation and subtraction
    
    // create the conjugate of the pose
    fusedConjugate = fusion.getFusionQPose().conjugate();
  
    // now do the rotation - takes two steps with qTemp as the intermediate variable
    qTemp = gravity * fusion.getFusionQPose();
    rotatedGravity = fusedConjugate * qTemp;
  
    // now adjust the measured accel and change the signs to make sense
    realAccel.setX(-(imu->getAccel().x() - rotatedGravity.x()));
    realAccel.setY(-(imu->getAccel().y() - rotatedGravity.y()));
    realAccel.setZ(-(imu->getAccel().z() - rotatedGravity.z()));
  
    sampleCount++;
    if ((delta = now - lastRate) >= 1000) {
    
      //Serial.print("Sample rate: "); Serial.print(sampleCount);
      if (!imu->IMUGyroBiasValid()) {
      //  Serial.println(", calculating gyro bias");
      } else {
      //  Serial.println();
      }
      
      sampleCount = 0;
      lastRate = now;
    }

    if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
      lastDisplay = now;

      odcitek.id = KEGEL_ID;
      odcitek.aX = realAccel.x();
      odcitek.aY = realAccel.y();
      odcitek.aZ = realAccel.z();
      odcitek.qX = fusion.getFusionQPose().x();
      odcitek.qY = fusion.getFusionQPose().y();
      odcitek.qZ = fusion.getFusionQPose().z();
      odcitek.qW = fusion.getFusionQPose().scalar();

      esp_err_t result = esp_now_send(sprejemnikMac, (uint8_t *) &odcitek, sizeof(odcitek));

      if (result == ESP_OK) {
        Serial.println("Uspesno poslano");
      } else {
        Serial.println("Napaka pri posiljanju");
      }
    }
  }
}
#+end_src