使用mpu6050 PID問題

comet1989

以下是我的程式碼

1. #include "Wire.h"
   
2. #include "I2Cdev.h"
   
3. #include "MPU6050.h"
   
4. #include "PID_v1.h"
   
5. #include  "Servo.h"
   
6. 
   
7. MPU6050 accelgyro;
   
8. int16_t ax, ay, az;
   
9. int16_t gx, gy, gz;
   
10. 
    
11. #define Gry_offset_X 45//陀螺仪X轴的静态飘移。
    
12. #define Gry_offset_Y 271
    
13. #define Gyr_Gain -0.00763//由陀螺仪X轴读数转换为角速度值，=1/131，向前方向与X轴为反方向，故乘以-1
    
14. 
    
15. #define ACC_Gain 0.000061//由读数转换为加速度值，=1/16384
    
16. #define pi 3.1415926
    
17. #define K_x 0.715//一阶互补滤波权重取值，不知由来，可更改。
    
18. #define k_Y 1.3
    
19. 
    
20. unsigned long preTime = 0;
    
21. Servo myservo;
    
22. float angle_X = 0;
    
23. float angle_Y = 0;
    
24. 
    
25. float Kp=5; //Initial Proportional Gain
    
26. float Ki=0; //Initial Integral Gain
    
27. float Kd=0; //Initial Differential Gain
    
28. float  Input, Output;
    
29. float  Setpoint=45;
    
30. int  Direction;
    
31. PID myPID(&Input, &Output, &Setpoint,Kp,Ki,Kd, DIRECT);
    
32. void setup()
    
33. {
    
34.   Wire.begin();
    
35.   Serial.begin(9600);
    
36.   accelgyro.initialize();//MPU6050初始化。
    
37.   myservo.attach(6,1000,2200);
    
38.   delay(2500);
    
39.   myservo.writeMicroseconds(1000);
    
40.   delay(2000);
    
41.   
    
42.   
    
43.   myPID.SetMode(AUTOMATIC);
    
44. }
    
45. 
    
46. void loop()
    
47. {
    
48.   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
    
49.   MPU6050_Angle();
    
50.   Input = angle_X;
    
51.   myPID.Compute();
    
52.   myservo.write(Output);
    
53.   
    
54.   //delay(10);
    
55. 
    
56.     Serial.print(angle_X); Serial.print("\t");
    
57.     Serial.print(angle_Y); Serial.print("\t");
    
58.     Serial.println(Output);
    
59.     }
    
60. void  MPU6050_Angle()
    
61. {
    
62.   unsigned long now = millis();//当前时间(ms)
    
63.   double dt = (now - preTime);//微分时间(s)
    
64.   preTime = now;//上一次采样时间(ms)
    
65.   double Ay = ay * ACC_Gain;//转换为向前的加速度(g)，为负值
    
66.   double Az = az * ACC_Gain;//转换为向下的加速度(g)
    
67.   double Ax = ax * ACC_Gain;
    
68.   
    
69.   double angleA_X= atan(Ay / Az)* 180/ pi;        //加速度仪，反正切获得弧度值，乘以180度/pi
    
70.   double angleA_Y= atan(Ax / Az)*180/pi;          //获得角度值，乘以-1得正
    
71. 
    
72.   double gx_revised = gx + Gry_offset_X;//陀螺仪x轴静态时修正后的角速度读数，向前为负值
    
73.   double gy_revised = gy + Gry_offset_Y;
    
74.   double omega_X= Gyr_Gain* gx_revised;//陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
    
75.   double omega_Y= Gyr_Gain* gy_revised;
    
76.   
    
77.   double angle_dt_X = omega_X * dt;//角度的单次积分值
    
78.   double angle_dt_Y = omega_Y * dt;
    
79. 
    
80.   angle_X+= (Gyr_Gain * (gx + Gry_offset_X)) * dt;
    
81.   angle_Y+= (Gyr_Gain * (gy + Gry_offset_Y)) * dt;
    
82. 
    
83. 
    
84.   double A_X= K_x/ (K_x+ dt);
    
85.   //陀螺仪的权值
    
86.   double A_Y= k_Y/(k_Y+dt);
    
87.   angle_X= A_X* (angle_X+ omega_X* dt)+ (1-A_X)* angleA_X;//一阶互补滤波后的输出角度(o)
    
88.   angle_Y= A_Y*(angle_Y+omega_Y*dt)+(1-A_Y)*angleA_Y;
    
89. }
    
90.    

复制代码

我使用的arduino mega 2560
執行一段時間arduino都會停住
請問是程式碼有問題還是硬體部分呢?

学慧放弃

对于我们不懂算法的有很大帮助，谢啦