使用mpu6050 PID問題
以下是我的程式碼#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "PID_v1.h"
#include"Servo.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gry_offset_X 45//陀螺仪X轴的静态飘移。
#define Gry_offset_Y 271
#define Gyr_Gain -0.00763//由陀螺仪X轴读数转换为角速度值，=1/131，向前方向与X轴为反方向，故乘以-1
#define ACC_Gain 0.000061//由读数转换为加速度值，=1/16384
#define pi 3.1415926
#define K_x 0.715//一阶互补滤波权重取值，不知由来，可更改。
#define k_Y 1.3
unsigned long preTime = 0;
Servo myservo;
float angle_X = 0;
float angle_Y = 0;
float Kp=5; //Initial Proportional Gain
float Ki=0; //Initial Integral Gain
float Kd=0; //Initial Differential Gain
floatInput, Output;
floatSetpoint=45;
intDirection;
PID myPID(&Input, &Output, &Setpoint,Kp,Ki,Kd, DIRECT);
void setup()
{
Wire.begin();
Serial.begin(9600);
accelgyro.initialize();//MPU6050初始化。
myservo.attach(6,1000,2200);
delay(2500);
myservo.writeMicroseconds(1000);
delay(2000);
myPID.SetMode(AUTOMATIC);
}
void loop()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
MPU6050_Angle();
Input = angle_X;
myPID.Compute();
myservo.write(Output);
//delay(10);
Serial.print(angle_X); Serial.print("\t");
Serial.print(angle_Y); Serial.print("\t");
Serial.println(Output);
}
voidMPU6050_Angle()
{
unsigned long now = millis();//当前时间(ms)
double dt = (now - preTime);//微分时间(s)
preTime = now;//上一次采样时间(ms)
double Ay = ay * ACC_Gain;//转换为向前的加速度(g)，为负值
double Az = az * ACC_Gain;//转换为向下的加速度(g)
double Ax = ax * ACC_Gain;
double angleA_X= atan(Ay / Az)* 180/ pi; //加速度仪，反正切获得弧度值，乘以180度/pi
double angleA_Y= atan(Ax / Az)*180/pi; //获得角度值，乘以-1得正
double gx_revised = gx + Gry_offset_X;//陀螺仪x轴静态时修正后的角速度读数，向前为负值
double gy_revised = gy + Gry_offset_Y;
double omega_X= Gyr_Gain* gx_revised;//陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
double omega_Y= Gyr_Gain* gy_revised;
double angle_dt_X = omega_X * dt;//角度的单次积分值
double angle_dt_Y = omega_Y * dt;
angle_X+= (Gyr_Gain * (gx + Gry_offset_X)) * dt;
angle_Y+= (Gyr_Gain * (gy + Gry_offset_Y)) * dt;
double A_X= K_x/ (K_x+ dt);
//陀螺仪的权值
double A_Y= k_Y/(k_Y+dt);
angle_X= A_X* (angle_X+ omega_X* dt)+ (1-A_X)* angleA_X;//一阶互补滤波后的输出角度(o)
angle_Y= A_Y*(angle_Y+omega_Y*dt)+(1-A_Y)*angleA_Y;
}
我使用的arduino mega 2560
執行一段時間arduino都會停住
請問是程式碼有問題還是硬體部分呢? 对于我们不懂算法的有很大帮助，谢啦
页:
[1]