【自制无人机】“辛”路历程（十）PID库控制普通电机

【自制无人机】“辛”路历程（十）PID库控制普通电机

【前言】

现在PID在Arduino有PID库，使用方便。下面使用PID库控制普通电机 ，但会出现因电机转动产生的颤动，使MPU6050数据产生波动，也会因此让杜邦线口接触不良。因在测试中，出现程序卡死现象，网上有人说是因为普通电机产生的电流脉冲，干扰I2C信号通信；也有说将杜邦线换成短线，并用热熔胶固定；不能用金属件固定MPU6050等等。
【代码】
#include "Wire.h"  
#include "I2Cdev.h"  
#include "MPU6050.h"  
  
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

#define Gry_offset 296//陀螺仪X轴的静态飘移。
#define Gyr_Gain -0.00763//由陀螺仪X轴读数转换为角速度值，=1/131，向前方向与X轴为反方向，故乘以-1

#define ACC_Gain 0.000061//由读数转换为加速度值，=1/16384
#define pi 3.1415926
#define K 0.715//一阶互补滤波权重取值，不知由来，可更改。

unsigned long preTime = 0;
float angleG = 0;

  
int input1 = 5; // pin 5 向 input1 输出
int input2 = 6; // pin 6 向 input2 输出
int enA = 10; //pin 10 向 输出A使能端输出
double Input, Output, Setpoint;  
PID myPID(&Input, &Output, &Setpoint,2, 0.01, 0.1, DIRECT);


void setup()  
{  
    Wire.begin();  
    Serial.begin(115200);  
  
    accelgyro.initialize();                 //初始化  
    if (!accelgyro.testConnection()) { while (1); }
    delay(5000);
    Serial.println("OK");
    pinMode(input1,OUTPUT);
    pinMode(input2,OUTPUT);
    pinMode(enA,OUTPUT);
    digitalWrite(input1,HIGH); //给高电平
    digitalWrite(input2,LOW);  //给低电平
    Setpoint=0;
    myPID.SetMode(AUTOMATIC);
    myPID.SetOutputLimits(-150,150);
}  
void loop()  
{  
  unsigned long now = millis();//当前时间(ms)
  float dt = (now - preTime) / 1000.0;//微分时间(s)
  preTime = now;//上一次采样时间(ms)

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
  
  float Y_Accelerometer = ay * ACC_Gain;//转换为向前的加速度(g)，为负值
  float Z_Accelerometer = az * ACC_Gain;//转换为向下的加速度(g)
  float angleA= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪，反正切获得弧度值，乘以180度/pi
                                                                   //获得角度值，乘以-1得正值
                                                                  
  float gx_revised = gx + Gry_offset;//陀螺仪x轴静态时修正后的角速度读数，向前为负值
  float omega= Gyr_Gain* gx_revised;//陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
  float angle_dt = omega * dt;//角度的单次积分值
  //angleG+= angle_dt;//陀螺仪，积分获得角度值
  
  angleG+= (Gyr_Gain * (gx + Gry_offset)) * dt;


  float A= K/ (K+ dt);//陀螺仪的权值
  angleG= A* (angleG+ omega* dt)+ (1-A)* angleA;//一阶互补滤波后的输出角度(o)
  
  
    /* kalman end */
    if(angleG<40 || angleG>-40)
    {
    Input=angleG;
    myPID.Compute();
    }
    else
    {
      Output=0;
    }
    analogWrite(enA,90-Output);
     Serial.print(angleG);
     Serial.print("   ");
    Serial.println(Output);
    delay(150);
      
}  [/mw_shl_code]
【演示】
测试视频中，将MPU6050从架子上拆下，在手里调整测试，卡死现象减少。


【修正】

其次，PWM频率是可以修改的，在Servo.h文件中，修改下面这行可以把PWM频率升到400Hz：

#define REFRESH_INTERVAL 2500

不过这只能在编译之前改，不能在程序运行的过程中动态修改。

【备注】

micros()函数返回Arduino板开始运行当前程序时的微秒数。该数字在大约70分钟后溢出,即回到零。

millis（）函数可以用来获取Arduino运行程序的时间长度，该时间长度单位是毫秒,Arduino最长可记录50天。如果超出记录时间上限，记录将从0重新开始。