代码求优化，第一次搞的单片机程序，Ctrl+C 百分之八十，自己弄了剩下的

foxwolf200

代码求优化，第一次搞的单片机程序，Ctrl+C 百分之八十，自己弄了剩下的，分割线一下是自己弄的，现在舵机倒是可以按照陀螺仪的参数运行了，就是不怎么灵敏，而且到角度极限时居然会反转，晕。。。。。。。请前辈门指点指点，在下谢谢了，这个代码只要是想弄个自稳的云台的。

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

复制代码