void GYRO_INIT_STATE(void)
{
/*
s_gi 陀螺仪初始化状态时的状态标志
0--初始化还未开始
1--校准正传系数
2--校准反正系数
3--告知主控陀螺仪标定完成
*/
static uint8_t s_gi = 0;
switch(s_gi)
{
case 0 : s_gi = 1;
Gyro_Clear();
break;
case 1 : s_gi = 2;
Gyro_Init_P();
Gyro_Clear();
S_GYRO_C1_FUNC();
break;
case 2 : s_gi = 0;
Gyro_Init_N();
S_GYRO_C2_FUNC();
Data_Save();
SS_H = 0;//复位状态标志
break;
default : s_gi = 1;
Gyro_Clear();
}
}
// void Gyro_NonLinear(void)
// {
// Gyro_Radian_Total
// Gyro_Multiplier=PPoly_Eval(fabs(1000*Gyro_Radian_Step/time_interval),GREENGYRO);
// delay(1);
// }
void Gyro_CheckFloat(void)
{
LED2_on;
Gyro_Clear();
Gyro_Float_Flag=1;
Gyro_Float_Flag1=1;
Gyro_Float_Flag2=1;
while(Gyro_Float_Flag)
delay(1);
if(Gyro_Float_Total2-Gyro_Float_Total1==0)
Float_State=CF_ERROR;
else if(Int64Abs(Gyro_Float_Total2-Gyro_Float_Total1-Gyro_Float*1024)>40960)
{
Gyro_Float=(Gyro_Float_Total2-Gyro_Float_Total1)/1024;
Float_State=CF_CHANGED;
Data_Save();
}
else
{
Float_State=CF_NORMAL;
}
Gyro_Clear();
LED2_off;
}
void ENC_R_INIT_STATE(void)
{
/*
s_er 码盘旋转半径初始化状态时的状态标志
0--初始化还未开始
1--校准正传系数
2--校准反正系数
3--告知主控陀螺仪标定完成
*/
static uint8_t s_er = 0;
switch(s_er)
{
case 0 : s_er = 1;
Encoder_Clear(0);
Encoder_Clear(1);
Gyro_Clear();
break;
case 1 : s_er = 0;
Encoder_InitR();
Data_Save();
SS_H = 0; //复位状态标志
break;
default : s_er = 1;
Encoder_Clear(0);
Encoder_Clear(1);
}
}
