查看: 101|回复: 2

[项目] 【自制无人机】“辛”路历程(十)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);
      
}  [/mw_shl_code]
【演示】
测试视频中,将MPU6050从架子上拆下,在手里调整测试,卡死现象减少。


【修正】
其次,PWM频率是可以修改的,在Servo.h文件中,修改下面这行可以把PWM频率升到400Hz:

#define REFRESH_INTERVAL 2500
不过这只能在编译之前改,不能在程序运行的过程中动态修改。
【备注】
micros()函数返回Arduino板开始运行当前程序时的微秒数。该数字在大约70分钟后溢出,即回到零。
millis()函数可以用来获取Arduino运行程序的时间长度,该时间长度单位是毫秒,Arduino最长可记录50天。如果超出记录时间上限,记录将从0重新开始。



云天  初级技匠
 楼主|

发表于 2020-8-8 20:41:48

发重了,可删除
回复

使用道具 举报

rzyzzxw  版主

发表于 2020-8-9 08:59:43

云天 发表于 2020-8-8 20:41
发重了,可删除

可以自己删除,也可以修改成新内容哈。
回复

使用道具 举报

高级模式
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2020 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail