261 lines
6.9 KiB
Org Mode
261 lines
6.9 KiB
Org Mode
|
#+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
|