【自制无人机】“辛”路历程（十）PID库控制普通电机
本帖最后由 云天 于 2020-8-8 20:40 编辑【自制无人机】“辛”路历程（十）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);
}
【演示】
测试视频中，将MPU6050从架子上拆下，在手里调整测试，卡死现象减少。
https://v.youku.com/v_show/id_XNDU5MDIwNjAwNA==.html
【修正】
其次，PWM频率是可以修改的，在Servo.h文件中，修改下面这行可以把PWM频率升到400Hz：
#define REFRESH_INTERVAL 2500
不过这只能在编译之前改，不能在程序运行的过程中动态修改。【备注】micros()函数返回Arduino板开始运行当前程序时的微秒数。该数字在大约70分钟后溢出,即回到零。millis（）函数可以用来获取Arduino运行程序的时间长度，该时间长度单位是毫秒,Arduino最长可记录50天。如果超出记录时间上限，记录将从0重新开始。
发重了，可删除 云天 发表于 2020-8-8 20:41
发重了，可删除
可以自己删除，也可以修改成新内容哈。{:5_151:}
页:
[1]