kegel/osc32_9255_rtimulib_espnow/osc32_9255_rtimulib_espnow.org

Krmilnik v keglu

• Vključene knjižnice, definicije
  • Nastavljive zadeve
  • i2c
  • Razbiranje orientacije
  • Kalibracijski podatki
  • Brezžično povezovanje
• Podatkovne strukture
  • Paket senzorskih podatkov
  • Objekti za gibalni senzor
  • Podatki o brezžični povezavi
• Vzpostavitev
• Zanka delovanja

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.

#define KEGEL_ID 1

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

uint8_t sprejemnikMac[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };

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

#define DISPLAY_INTERVAL  5                         // interval between pose displays

i2c

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

#include <Wire.h>

Razbiranje orientacije

Sledeče knjižnice so potrebne za branje senzorja, vključene iz zdaj žal ne več aktivno razvite knjižnice 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!

#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h" 
#include "CalLib.h"

Kalibracijski podatki

Kegel moramo pred uporabo kalibrirati, s to skico. S tipko s shranimo parametre kalibracije v EEPROM, od koder ga ta skica kasneje prebere.

#include <EEPROM.h>

Brezžično povezovanje

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

#include <esp_now.h>
#include <WiFi.h>

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.

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;

Objekti za gibalni senzor

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

Števci za ritem pošiljanja paketov.

unsigned long lastDisplay;
unsigned long lastRate;
int sampleCount;

Podatki o brezžični povezavi

esp_now_peer_info_t peerInfo;

Vzpostavitev

Najprej rezerviramo prostor za razbiranje napake in vzpostavimo serijsko povezavo.

void setup() {
  int errcode; // Prostor za napako

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

Vklopimo tudi EEPROM razbiranje ter i2c povezavo s senzorjem.

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

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

  // 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);
  }

Razberemo kalibracijo magnetometra.

  if (imu->getCalibrationValid())
    Serial.println("Using compass calibration");
  else
    Serial.println("No valid compass calibration data");

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

  gravity.setScalar(0);
  gravity.setX(0);
  gravity.setY(0);
  gravity.setZ(1);

Števce nastavimo na izhodiščni čas.

  lastDisplay = lastRate = millis();
  sampleCount = 0;

WIFI nastavimo na ESP-NOW.

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

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

  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;
  }
}

Zanka delovanja

TODO opis zanke!

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");
      }
    }
  }
}