【Arduino & Processing】PmodNAVのセンサーのデータを表示する。
前回は、PmodNAVとArduinoを接続して、センサーからもらった加速度などのデータをArduino IDEに付属の「シリアルモニタ」に表示するというところまで行いました。
前回の記事は、こちらです。
shizenkarasuzon.hatenablog.com
ちなみに、PmodNAVとは、
加速度、ジャイロ、方向（magnetic field）、気圧センサーが一つになっているモジュールです。
しかし、シリアルモニタでは、数字データが文字として表示されるだけなので見にくいという欠点があります、
この記事では、受信したデータ（加速度、ジャイロ、センサーの傾き）をグラフ化して表示したいと思います。
目標はこのように表示されることです。
このようなグラフなどを表示するGUI（Graphical User Interface）を作るために、Processingを使いました。
プログラム：
Arduino側
/************************************************************************** * Description: Pmod_NAV * All data (accelerometer, gyroscope, magnetometer) are displayed * * Wiring * Module<----------> Arduino * pin 6 3.3V * pin 5 GND * pin 4 A5 * pin 2 A4 ************************************************************************/ #include <Wire.h> #include <SparkFunLSM9DS1.h> // Déclaration des adresses du module #define LSM9DS1_M 0x1E #define LSM9DS1_AG 0x6B #define GYRO 1 #define ACCEL 2 #define MAG 3 #define ROTATION 4 LSM9DS1 imu; // Creation of the object imu #define PRINT_SPEED 250 static unsigned long lastPrint = 0; // The earth's magnetic field varies according to its location. // of the magnetic field using the following site // http://www.ngdc.noaa.gov/geomag-web/#declination #define DECLINATION -0.74 // déclinaison (en degrés) pour Japan. void setup(void){ Serial.begin(115200); imu.settings.device.commInterface = IMU_MODE_I2C; // initialization of the module imu.settings.device.mAddress = LSM9DS1_M; imu.settings.device.agAddress = LSM9DS1_AG; if (!imu.begin()) { Serial.println("Probleme de communication avec le LSM9DS1."); while (1); } } void loop(){ if (imu.gyroAvailable()) imu.readGyro(); // acquisition des données du gyroscope if (imu.accelAvailable()) imu.readAccel(); //Acquisition of accelerometer data if (imu.magAvailable()) imu.readMag(); // Acquisition of the magnetometer if ((lastPrint + PRINT_SPEED) > millis()){ printGyro(); // Print "G: gx, gy, gz" printAccel(); // Print "A: ax, ay, az" printMag(); // Print "M: mx, my, mz" printAttitude(imu.ax, imu.ay, imu.az,-imu.my, -imu.mx, imu.mz); lastPrint = millis(); } } void printGyro(){ Serial.print(GYRO); Serial.print(","); Serial.print(imu.calcGyro(imu.gx), 2); Serial.print(","); Serial.print(imu.calcGyro(imu.gy), 2); Serial.print(","); Serial.println(imu.calcGyro(imu.gz), 2); } void printAccel(){ Serial.print(ACCEL); Serial.print(","); Serial.print(imu.calcAccel(imu.ax), 2); Serial.print(", "); Serial.print(imu.calcAccel(imu.ay), 2); Serial.print(", "); Serial.println(imu.calcAccel(imu.az), 2); } void printMag(){ Serial.print(MAG); Serial.print(","); Serial.print(imu.calcMag(imu.mx), 2); Serial.print(", "); Serial.print(imu.calcMag(imu.my), 2); Serial.print(", "); Serial.println(imu.calcMag(imu.mz), 2); } void printAttitude(float ax, float ay, float az, float mx, float my, float mz){ float roll = atan2(ay, az); float pitch = atan2(-ax, sqrt(ay * ay + az * az)); float heading; if (my == 0){ heading = (mx < 0) ? PI : 0; }else{ heading = atan2(mx, my); heading -= DECLINATION * PI / 180; if (heading > PI){ heading -= (2 * PI); }else if (heading < -PI){ heading += (2 * PI); } else if(heading < 0){ heading += 2 * PI; } } heading *= 180.0 / PI; pitch *= 180.0 / PI; roll *= 180.0 / PI; heading = 0; Serial.print(ROTATION); Serial.print(","); Serial.print(pitch, 2); Serial.print(", "); Serial.print(roll, 2); Serial.print(", "); Serial.println(heading, 2); }
Processing側
import processing.serial.*; static final int GYRO = 1; static final int ACCEL = 2; static final int MAG = 3; static final int ROTATION = 4; Serial myPort; float gx, gy, gz; float ax, ay, az; float mx, my, mz; float rx, ry, rz; //magnitude graphMonitor GyroGraph; graphMonitor AccelGraph; graphMonitor MagGraph; graphMonitor RotateGraph; void setup(){ size(800, 700); myPort = new Serial(this, "COM4", 115200); frameRate(100); smooth(); myPort.bufferUntil('\n'); GyroGraph = new graphMonitor("gyro", 100, 20, 500, 150); AccelGraph = new graphMonitor("accel", 100, 180, 500, 150); MagGraph = new graphMonitor("mag", 100, 350, 500, 150); RotateGraph = new graphMonitor("rotation", 100, 570, 500, 150); } void draw(){ background(0); text(str(gx),100,100); GyroGraph.graphDraw(gx, gy, gz); AccelGraph.graphDraw(ax, ay, az); MagGraph.graphDraw(mx, my, mz); RotateGraph.graphDraw(rx, ry, rz); } void serialEvent(Serial myPort){ String myString = myPort.readStringUntil('\n'); if (myString != null) { myString = trim(myString); float sensors[] = float(split(myString, ',')); if (sensors.length > 2) { switch(int(sensors[0])){ case GYRO: gx = sensors[1]; gy = sensors[2]; gz = sensors[3]; myPort.write("A"); break; case ACCEL: ax = sensors[1]; ay = sensors[2]; az = sensors[3]; myPort.write("A"); break; case MAG: mx = sensors[1]; my = sensors[2]; mz = sensors[3]; myPort.write("A"); break; case ROTATION: rx = sensors[1]; ry = sensors[2]; rz = sensors[3]; myPort.write("A"); break; } } } }
class graphMonitor { String TITLE; int X_POSITION, Y_POSITION; int X_LENGTH, Y_LENGTH; float [] y1, y2, y3; float maxRange; graphMonitor(String _TITLE, int _X_POSITION, int _Y_POSITION, int _X_LENGTH, int _Y_LENGTH) { TITLE = _TITLE; X_POSITION = _X_POSITION; Y_POSITION = _Y_POSITION; X_LENGTH = _X_LENGTH; Y_LENGTH = _Y_LENGTH; y1 = new float[X_LENGTH]; y2 = new float[X_LENGTH]; y3 = new float[X_LENGTH]; for (int i = 0; i < X_LENGTH; i++) { y1[i] = 0; y2[i] = 0; y3[i] = 0; } } void graphDraw(float _y1, float _y2, float _y3) { y1[X_LENGTH - 1] = _y1; y2[X_LENGTH - 1] = _y2; y3[X_LENGTH - 1] = _y3; for (int i = 0; i < X_LENGTH - 1; i++) { y1[i] = y1[i + 1]; y2[i] = y2[i + 1]; y3[i] = y3[i + 1]; } maxRange = 1; for (int i = 0; i < X_LENGTH - 1; i++) { maxRange = (abs(y1[i]) > maxRange ? abs(y1[i]) : maxRange); maxRange = (abs(y2[i]) > maxRange ? abs(y2[i]) : maxRange); maxRange = (abs(y3[i]) > maxRange ? abs(y3[i]) : maxRange); } pushMatrix(); translate(X_POSITION, Y_POSITION); fill(240); stroke(130); strokeWeight(1); rect(0, 0, X_LENGTH, Y_LENGTH); line(0, Y_LENGTH / 2, X_LENGTH, Y_LENGTH / 2); textSize(25); fill(60); textAlign(LEFT, BOTTOM); text(TITLE, 20, -5); textSize(22); textAlign(RIGHT); text(0, -5, Y_LENGTH / 2 + 7); text(nf(maxRange, 0, 1), -5, 18); text(nf(-1 * maxRange, 0, 1), -5, Y_LENGTH); translate(0, Y_LENGTH / 2); scale(1, -1); strokeWeight(1); for (int i = 0; i < X_LENGTH - 1; i++) { stroke(255, 0, 0); line(i, y1[i] * (Y_LENGTH / 2) / maxRange, i + 1, y1[i + 1] * (Y_LENGTH / 2) / maxRange); stroke(255, 0, 255); line(i, y2[i] * (Y_LENGTH / 2) / maxRange, i + 1, y2[i + 1] * (Y_LENGTH / 2) / maxRange); stroke(0, 0, 0); line(i, y3[i] * (Y_LENGTH / 2) / maxRange, i + 1, y3[i + 1] * (Y_LENGTH / 2) / maxRange); } popMatrix(); } }
実行方法
まず、上の二つのプログラムを同じ場所に保存してください。
このようになっていればOKです。
私は、一つ目のコードを「processing.pde」として保存し、2つ目のコードを「graph.pde」として保存しています。
そして、ArduinoをUSBでPCに接続した後に、上のプログラムを起動します。
動作環境によっては、
myPort = new Serial(this, "COM4", 115200);
の部分でエラーが出ることがあります。
「COM4」とは、Arduinoが接続されているCOMポート番号をさします。
エラーが出る場合は、デバイスマネージャ等からArduinoのCOMポート番号を調べて書き換えてください。
