在处理过程中创建一个简单的界面，以显示9个自由度IMU的输出。

硬件组件

STM32 NucleoSTM32F072 Nucleo Board  ×        1      

AdafruitHMC5883L板  ×    1      

Adafruit MPU6050板  ×      1               

面包板（通用）  ×     1      

介绍

     我带着另一个小项目来到这里。在这篇文章中，我想强调使用处理作为传感器输出的可视界面，以及使用陀螺仪，加速度计和磁力计计算平台的偏航，俯仰和滚动的一些基础知识。

     在这个项目中，我特别使用了STM32板，IAR工作台IDE和标准外设库，但您可以使用Arduino Uno或几乎任何Arduino用于此目的。

     我做了这个项目，希望学习如何使用上面列出的所有内容并制作一个自平衡机器人，让我们看看结果如何。加工界面的目的是向通常访问机器人俱乐部和大学的年轻学生展示。

加工（V3）

     如果您使用Arduino，您可能会喜欢处理。

     Arduino不仅基于处理，而且还有大量的示例和库，而且编程非常简单。从我读过的内容来看，它是为了帮助艺术家用简单的编程知识将他们的想法变为现实而创建的 - 所以快速制作可视化界面（以及在你的项目中指挥你的项目）是非常好的，尽管对于时间图我还是现在更喜欢Matlab。

如果您使用Arduin / Energia，这个IDE似乎很熟悉

首先是使用传感器数据

在我的项目中，我使用了MPU6050，其中包括陀螺仪，加速度计和HMC5883L。我不会解释如何使用这些特定的传感器，只是如何使用通用数据。

如果您对此不感兴趣并前来处理，请跳过。

偏航使用磁力计 - 简易模式

     从3轴磁力计获得偏航很容易 - 正确的偏航是很困难的。

     磁力计检测任何磁场并考虑到完美的环境，只有现有的地磁场才是这个目的的理想选择。通常它也被认为是与地面平行的场以进行计算，然后进行位校正以获得真北（相对于磁北） - 这些校正取决于年份和定位。

第一种情况 - Z轴垂直于地面

      如果您有这样的传感器，Z将被视为无关紧要。通过这种方式，您可以轻松地从X和Y获得X轴与磁北的角度。还记得三角学吗？  只要做arctan(y/x) 。      你如何在编程中这样做？那么你需要：

float magnetic_heading = atan2(y,x); //returns heading in radians

    为什么atan2 而不是atan ？因为我们想要所有4个象限（查看定义atan2 和atan 更多信息）。测试一下，转换成度数：

int Degrees = magnetic_heading * 180 / PI;

     容易吗？但这是考虑到无干扰环境而且没有倾斜（Z始终垂直于地面！）。

     我确实实现了一些倾斜补偿和一些硬金属和软金属干扰（看起来，已经有一些概念需要注意）但是它不能很好地工作，90度转弯我们更像80º转110º。所以对于这个项目，我会把它留在这里只是为了在界面上显示出一点偏航。有关更多信息，我认为ST的“ 使用LSM303DLH进行倾斜补偿电子罗盘”非常好。

     陀螺仪可用于帮助以类似的方式显示俯仰和滚动，但我的转折点从1º到359º有问题。

俯仰和滚动

俯仰和滚动我们计算起来更可靠。为此，您可以使用加速度计和陀螺仪来补偿彼此的问题。

首先是加速度计。

      它测量加速度。静止不动（或只是让它以恒定速度运动）并且只检测重力 - 它总是指向同一方向：向下。为了获得角度，我非常喜欢ST的应用笔记方法。将X和Y作为加速度计轴的值：

Pitch = asin(-X);Roll = asin( Y / cos(Pitch)  );

     如果音高模块是90º，请小心。余弦将为0（除以0不是一个好主意），因此如果是这种情况你应该避免进行Roll数学运算并将Roll设置为0º。这个问题是，快速移动会使计算失效，因为除了重力之外会有加速度。这就是陀螺仪有用的地方。

      好的，现在单独使用陀螺仪。陀螺仪测量角速度。很好，如果你还记得微积分，只需积分角速度来获得时间间隔内的角度。好吧，我们无法整合，但我们可以做总结，这非常接近吗？

      如果你有一个循环进行这个数学计算，测量之间的时间（可以通过传感器测量速率定义）乘以角速度将给你测量之间的角位移，对吗？继续添加，你将得到积分的近似值（当然，误差越小，间隔越小越好，但例如MPU6050可以超过8KHz）。

int pitch =0;while(1){G_Y = Get_Y_measurement(); //returns in º/spitch += 0,1 * G_Y;(100); //wait 100ms, let's imagine this is the time between measurements}

      现在请记住，对于滚动，你使用陀螺仪的X轴。

      有了这个，你就可以在陀螺仪上滚动和俯仰。您可以使用Z轴以类似的方式获得偏航。

      但这够了吗？比加速度计好吗？嗯，这很精确......有一段时间了。大约一分钟后，您可能会看到相当大的错误。还记得数学吗？我们想要一个积分，我们近似。这意味着存在错误累积。

怎么解决这个问题

     您使用过滤器。例如卡尔曼滤波器（相当复杂）和互补滤波器 - 我使用的滤波器。

     互补滤波器对来自加速度计和具有不同权重的陀螺仪的计算进行求和。陀螺仪非常适合短暂的快速移动，最大的重量，如98％，而加速度计只有2％（这可以/应该调整）。

像这样的东西：

Final_Roll = G_Roll * 0,98 + A_Roll * 0,02

当G_Roll 从陀螺仪计算出的卷筒，并A_Roll 从加速度计。

      它将如何运作？那么，当静止不动时，陀螺仪会给出接近0的读数 - 这意味着总贡献将非常小 - 而Acc将处于它擅长的情况：静止不动 - 使Roll达到非常准确的值，消除了累积误差。陀螺仪。

     当移动陀螺仪将超过额外加速度的Acc误差时，当然这对于短旋转效果最佳，如果旋转一分钟陀螺仪误差将在最终滚动时开始明显。

做一些与Pitch类似的事情。当然，所有这些都不能补偿传感器的个体误差，如偏移值。

      所以有时间进行一些编程处理。代码将在下面，因为我认为，补充评论，最好显示它是如何完成的。将其复制到处理IDE +执行CTRL + T索引是一个好主意，使其更清晰。

/*

Rotation visual interface

To use you require a microcontroler sendingyaw, pich and roll in this order:

   yaw least significant byte

   yaw most significant byte

   Pitch (1 byte)

   Roll (1 byte)

This in a cycle. Be carefull to not send ittoo fast or it will start lagging.

Made by: Luís Afonso

*/

import processing.serial.*;

Serial myPort;  // Create object from Serial class

PFont f;

int Yaw_H;      // Data received from the serial port

int Yaw_L = 0;

int Pitch_H;      // Data received from the serial port

int Pitch_L = 0;

int Roll_H;      // Data received from the serial port

int Roll_L = 0;

float r;

// Angle andangular velocity, accleration

float F_Yaw;

float F_Pitch;

float F_Roll;

int xi = 0, yi=0;

void setup()

{

//size(1300, 720);

fullScreen();

// Initialize allvalues

r = height * 0.45;

// I know that thefirst port in the serial list on my mac

// is always my  FTDI adaptor, so I open Serial.list()[0].

// On Windowsmachines, this generally opens COM1.

// Open whatever portis the one you're using.

String portName = Serial.list()[0];

myPort = new Serial(this, portName, 115200);

/*

     Change the text font to make it bigger

     You need to go to "Tools->Createfont" to be able to use this. Just comment these 2 lines out

   if you don't feel like it.

*/

f = createFont("SourceCodePro-Regular.vlw", 20);

textFont(f);

}

void draw()

{

/*

   Wait for at least 4 values to be availableto receive.

   It should be in order:

   yaw least significant byte

   yaw most significant byte

   Pitch

   Roll

   Pitch and Roll can be just 1 byte since theygo in a 180º range, while yaw is 360º (360 doesn't fit in 8bit number)

*/

if (myPort.available() >= 4) {  // If data is available,

   Yaw_L = myPort.read();        // read it and store it in val

   Yaw_H = myPort.read();        // read it and store it in val

   Pitch_L = myPort.read();        // read it and store it in val

   //Pitch_H =myPort.read();         // read it andstore it in val

   Roll_L = myPort.read();        // read it and store it in val

   //Roll_H =myPort.read();         // read it andstore it in val

   /* Just fordebug purposes, prints in text console */

   print(Yaw_L);

   print(" ");

   print(Yaw_H);

   print(" ");

   print(Pitch_L);

   print(" ");

   print(Roll_L);

   println(" ");

}

//Set background toblack (will erase previous drawing that was there in each cycle)

background(0);

// Translate theorigin point to the center of the screen

translate(width/2, height/2);

int Yaw;

Yaw = (Yaw_H < int Pitch = 1*(Pitch_L-90); int Roll = 1*(Roll_L-90); F_Yaw = ((Yaw+90)*PI/180.0); // Convert polar to cartesian float x = -r * cos(F_Yaw); float y = r *sin(F_Yaw); // Draw the ellipse at the cartesian coordinateellipseMode(CENTER); noStroke(); fill(0, 255, 255); ellipse(x, y, 32, 32);fill(200); ellipse(0, 0, 10, 10); float F_Roll = Roll * PI /180; y =sin(F_Roll) * 300; x = cos(F_Roll)*300; stroke(0, 255, 0); line(xi, yi, x+xi,y+yi); line(xi, yi, xi-x, yi-y); float F_Pitch = Pitch * PI /180; float  y_P= yi + sin(F_Pitch)*100; stroke(255, 0, 0); line(0, y_P, 300, y_P);line(0, y_P, -300, y_P); textAlign(RIGHT); fill(0, 255, 0);text("Roll:", 300, 200); text(Roll, 350, 200); fill(255, 0, 0);text("Pitch:", 300, 240); text(Pitch, 350, 240); fill(0, 100, 255);text("Yaw:", 300, 280); text(Yaw, 350, 280);}

代码

/*

Rotation visual interface

  Touse you require a microcontroler sending yaw, pich and roll in this order:

   yaw least significant byte

   yaw most significant byte

   Pitch (1 byte)

   Roll (1 byte)

   

This in a cycle. Be carefull to not send it too fast or it will startlagging.

  

Made by: Lus Afonso

*/

import processing.serial.*;

Serial myPort;  // Createobject from Serial class

PFont f;

int Yaw_H;      // Datareceived from the serial port

int Yaw_L = 0;

int Pitch_H;      // Datareceived from the serial port

int Pitch_L = 0;

int Roll_H;      // Datareceived from the serial port

int Roll_L = 0;

float r;

// Angle and angular velocity, accleration

float F_Yaw;

float F_Pitch;

float F_Roll;

int xi = 0, yi=0;

void setup()

{

  //size(1300, 720);

fullScreen();

  // Initialize all values

  r =height * 0.45;

  // I know that the first port in the serial list on mymac

  // is always my FTDI adaptor, so I open Serial.list()[0].

  // On Windows machines, this generally opens COM1.

  // Open whatever port is the one you're using.

String portName = Serial.list()[0];

myPort = new Serial(this, portName, 115200);

  /*

     Change the text font to make it bigger

     You need to go to "Tools->Create font" to be able to usethis. Just comment these 2 lines out

   if you don't feel like it.

  */

  f =createFont("SourceCodePro-Regular.vlw", 20);

textFont(f);

}

void draw()

{

  /*

   Wait for at least 4 values to be available to receive.

   It should be in order:

   yaw least significant byte

   yaw most significant byte

   Pitch

   Roll

   

   Pitch and Roll can be just 1 byte since they go in a 180 range, whileyaw is 360 (360 doesn't fit in 8bit number)

  */

  if (myPort.available() >= 4) {  // If data isavailable,

   Yaw_L = myPort.read();         // read it and store it in val

   Yaw_H = myPort.read();         // read it and store it in val

   Pitch_L = myPort.read();         // read it and store it in val

    //Pitch_H = myPort.read();         // read it and store it in val

   Roll_L = myPort.read();         // read it and store it in val

    //Roll_H = myPort.read();         // read it and store it in val

    /* Just for debug purposes, prints in text console */

    print(Yaw_L);

   print(" ");

   print(Yaw_H);

   print(" ");

   print(Pitch_L);

   print(" ");

   print(Roll_L);

   println(" ");

  }

  //Set background to black (will erase previous drawingthat was there in each cycle)

background(0);

  // Translate the origin point to the center of the screen

translate(width/2, height/2);

  intYaw;

  Yaw= (Yaw_H <<8)|Yaw_L;

  intPitch = 1*(Pitch_L-90);

  intRoll = 1*(Roll_L-90);

F_Yaw = ((Yaw+90)*PI/180.0);

  // Convert polar to cartesian

float x = -r * cos(F_Yaw);

float y = r * sin(F_Yaw);

  // Draw the ellipse at the cartesian coordinate

ellipseMode(CENTER);

noStroke();

fill(0, 255, 255);

ellipse(x, y, 32, 32);

fill(200);

ellipse(0, 0, 10, 10);

float F_Roll = Roll * PI /180;

  y =sin(F_Roll) * 300;

  x =cos(F_Roll)*300;

stroke(0, 255, 0);

line(xi, yi, x+xi, y+yi);

line(xi, yi, xi-x, yi-y);

float F_Pitch = Pitch * PI /180;

float  y_P=  yi + sin(F_Pitch)*100;

stroke(255, 0, 0);

line(0, y_P, 300, y_P);

line(0, y_P, -300, y_P);

  

  

textAlign(RIGHT);

fill(0, 255, 0);

text("Roll:", 300, 200);

text(Roll, 350, 200);

fill(255, 0, 0);

text("Pitch:", 300, 240);

text(Pitch, 350, 240);

fill(0, 100, 255);

text("Yaw:", 300, 280);

text(Yaw, 350, 280);

}