#include?“include.h“
#include??“CollectDate.h“

int?nGravityMax=2856;????????????
int?nGravityMin=900;

unsigned?int?ATD_Data_Buf[21][2]={0};?
unsigned?int?ATD_Data_mid[3]={0};
unsigned?int?GRAVITY_dataAR1_data;?
int?VOLTAGE_GRAVITY;
int?nGravityOffset;
int?VOLTAGE_GYRO;
int?g_nGyroscopeOffset;
extern?float?g_fGravityAngleg_fYGravityAngleg_fCarSpeedAngle=0;
extern?float?g_fGyroAngleIntegralg_fSpeedControlIntegralg_fGyroAngleSpeedg_fGyroAngle;?
float?fDeltaValue;

float?angleangle_dot;//互補濾波
float?bias_cf;
float?dt=0.005;
extern?float?CarAngle;

float??kai?kai1;

void?Read_AD（void）????//讀取陀螺儀和加速度計AD值
{?
??unsigned?char?i;
??for（i=0;i<=20;i++）?
??{?
????ATD_Data_Buf[i][0]=ad_once（ADC0SE8?ADC_12bit）;????//B0?陀螺儀通道??AR1?
????ATD_Data_Buf[i][1]=ad_once（ADC0SE9?ADC_12bit）;????//B1?Z軸加速度計
??}
}

void?Get_AD_mid（void）??????//中值濾波
{?
??unsigned?char?ijkmin;
??unsigned?int?t;
?
??for（j=0;j<2;j++）?
??{????
????for（i=0;i<20;i++）?
????{
??????min=i;???
??????for（k=i+1;k<21;k++）?
??????{
????????if（ATD_Data_Buf[k][j]????????{
??????????min=k;
????????}
??????}
??????if（min!=i）
??????{
????????t=ATD_Data_Buf[i][j];
????????ATD_Data_Buf[i][j]=ATD_Data_Buf[min][j];
????????ATD_Data_Buf[min][j]=t;
??????}
????}
????
????ATD_Data_mid[j]=ATD_Data_Buf[10][j];?
??}?????
}??


void?AVE（void）
{
??int?SUM_AR1=0SUM_GRAZ=0;
??u8?i;
??for（i=0;i<10;i++）
??{
????Read_AD（）;
????Get_AD_mid（）;
????SUM_AR1+=ATD_Data_mid[0];???//陀螺儀值
????SUM_GRAZ+=ATD_Data_mid[1];??//加速度值
??}????
????AR1_data=SUM_AR1/10;????//陀螺儀值
????GRAVITY_data=SUM_GRAZ/10;???????//加速度值
????//printf（“2=%d\n“GRAVITY_data）;?
}

/***********互補濾波****************/

void?complement_filter（float?angle_m_cffloat?gyro_m_cf）
{
????bias_cf*=0.001;			//陀螺儀零飄低通濾波；500次均值；0.998?
????bias_cf+=gyro_m_cf*0.09;		???//0.002
????angle_dot=gyro_m_cf-bias_cf;		???
????angle=（angle+angle_dot*dt）*0.95+angle_m_cf*0.04;	
????//加速度低通濾波；20次均值；按100次每秒計算，低通5Hz；0.90?0.05???
????CarAngle=angle;
}

/*********************Get_ATD_mid**************************/
void?AngleCalculate（void）?
{???
????VOLTAGE_GRAVITY=GRAVITY_Receive（）;
????//printf（“%d\n“VOLTAGE_GRAVITY1）;???
????//加速度傳感器角度輸出????????
????g_fGravityAngle=（float）（VOLTAGE_GRAVITY-1838）*GRAVITY_ANGLE_RATIO;
????//陀螺儀角速度
????VOLTAGE_GYRO=AR1_Receive（）;
????//VOLTAGE_GYR1=ad_ave（ADC0SE12?ADC_12bit20）;
????//printf（“g_nGyroscopeOffset=%d\n“g_nGyroscopeOffset）;
????g_fGyroAngleSpeed=（float）（（VOLTAGE_GYRO-2852）*0.45）;//0.123陀螺儀比例參數(shù)021????
????//g_fGyroAngle=g_fGyroAngleSpeed*0.005;???//求陀螺儀比例系數(shù)
????g_fGyroAngle=g_fGyroAngleIntegral;
????fDeltaValue?=?（g_fGravityAngle?-?g_fGyroAngle）/0.7;??????????????????
????//g_fGyroAngleIntegral+=（（g_fGravityAngle-g_fGyroAngleIntegral）*（1.00+Scan_Key0（））+g_fGyroAngleSpeed）*（0.21+Scan_Key0（））;
????g_fGyroAngleIntegral+=（g_fGyroAngleSpeed+fDeltaValue）*0.005;?
????//complement_filter（g_fGyroAngleIntegralg_f

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2016-03-15?22:02??寒假調(diào)直立?-?16-03-15速度閉環(huán)\
?????目錄???????????0??2016-03-15?22:02??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\
?????目錄???????????0??2016-03-15?22:02??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\
?????文件????????2030??2015-03-30?11:51??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\128KB_Pflash.icf
?????文件????????2206??2010-11-12?03:41??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\128KB_Pflash_128KB_Dflash.icf
?????文件????????2035??2012-02-28?15:33??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\128KB_Ram.icf
?????文件????????2030??2010-10-28?03:30??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\256KB_Pflash.icf
?????文件????????2214??2012-03-01?20:12??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\256KB_Pflash_256KB_Dflash.icf
?????文件????????2024??2010-10-28?03:30??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\32KB_Ram.icf
?????文件????????2041??2012-03-01?18:46??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\512KB_Pflash.icf
?????文件????????2217??2010-11-12?03:41??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\64KB_Pflash_64KB_Dflash.icf
?????文件????????2024??2010-10-28?03:30??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\64KB_Ram.icf
?????文件????????3463??2012-03-08?21:04??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\fire_RAM_K60N512.icf
?????文件????????3441??2012-03-08?21:04??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\config?files\fire_ROM_K60N512.icf
?????文件?????????669??2012-03-10?23:38??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\iar.h
?????目錄???????????0??2016-03-15?22:02??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\
?????目錄???????????0??2016-03-15?22:02??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\
?????目錄???????????0??2016-03-15?22:02??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Exe\
?????文件???????45603??2016-03-12?20:56??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Exe\.hex
?????文件???????21994??2015-03-28?10:54??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Exe\fire_gpio_demo.hex
?????文件??????284536??2016-03-12?20:56??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Exe\fire_gpio_demo.out
?????文件???????16243??2016-03-12?20:57??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Exe\fire_gpio_demo.sim
?????目錄???????????0??2016-03-15?22:02??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\List\
?????文件???????34922??2016-03-12?20:56??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\List\fire_gpio_demo.map
?????目錄???????????0??2016-03-15?22:02??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Obj\
?????文件???????27156??2015-07-20?11:56??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Obj\adc.o
?????文件???????11376??2015-07-20?11:56??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Obj\alloc.o
?????文件???????16528??2015-07-20?11:56??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Obj\arm_cm4.o
?????文件????????8728??2015-08-12?20:35??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Obj\assert.o
?????文件???????37728??2016-03-12?20:56??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Obj\CollectDate.o
?????文件???????35772??2016-03-12?20:56??寒假調(diào)直立?-?16-03-15速度閉環(huán)\build\led_demo\Debug\Obj\Control.o
............此處省略298個文件信息