代码求优化，第一次搞的单片机程序，Ctrl+C 百分之八十，自己弄了剩下的
本帖最后由 弘毅 于 2012-12-28 20:58 编辑代码求优化，第一次搞的单片机程序，Ctrl+C 百分之八十，自己弄了剩下的，分割线一下是自己弄的，现在舵机倒是可以按照陀螺仪的参数运行了，就是不怎么灵敏，而且到角度极限时居然会反转，晕。。。。。。。请前辈门指点指点，在下谢谢了，这个代码只要是想弄个自稳的云台的。
#include <Servo.h>
#include <I2Cdev.h>
#include <MPU6050.h>
#include "Wire.h"
MPU6050 accelgyro;
Servo Servol;
Servo Servol2;
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轴为反方向，故乘以-112.
#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;
float angle_X = 0;
float angle_Y = 0;
float Temp=0,Temp_o=0;
void setup()
{
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();
Servol.attach(2);
Servol.write(90);
Servol2.attach(3);
Servol2.write(90);
}//MPU6050初始化。
void loop() {
unsigned long now = millis();//当前时间(ms)
float dt = (now - preTime) / 500.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 X_Accelerometer = ax * ACC_Gain;
float angleA_X= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪，反正切获得弧度值，乘以180度/pi43.
//获得角度值，乘以-1得正
float angleA_Y= atan(X_Accelerometer/Z_Accelerometer)*(-180)/pi;
float gx_revised = gx + Gry_offset_X;//陀螺仪x轴静态时修正后的角速度读数，向前为负值
float gy_revised = gy + Gry_offset_Y;
float omega_X= Gyr_Gain* gx_revised;//陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
float omega_Y= Gyr_Gain* gy_revised;
float angle_dt_X = omega_X * dt;//角度的单次积分值
float angle_dt_Y = omega_Y * dt;
angle_X+= (Gyr_Gain * (gx + Gry_offset_X)) * dt;
angle_Y+= (Gyr_Gain * (gy + Gry_offset_Y)) * dt;
float A_X= K_x/ (K_x+ dt); //陀螺仪的权值
float 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;
//---------------------------------------------------------------------------------------------舵机开始
Temp=90;
if (angle_X>0){
if ((Temp+angle_X)<170){
Servol.write(angle_X+Temp) ;
delay(5);
}
}
else {
if ((angle_X+Temp)>1){
Servol.write(angle_X+Temp);
delay(5);
}
}
if (angle_Y>0){
if ((Temp+angle_Y)<170){
Servol2.write(angle_Y+Temp) ;
delay(5);
}
}
else {
if ((angle_Y+Temp)>1){
Servol2.write(angle_Y+Temp);
delay(5);
}
}
Serial.print(angle_X);
Serial.print(",");
Serial.print(angle_Y);
Serial.print("\n");
}
页:
[1]