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);
}
}
void GPS_Clear(void)
{
Gyro_Total=0;
// Gyro_Last=Gyro_Now;
Encoder_Clear(0);
Encoder_Clear(1);
GPS_Init(Gps_Zero,0);
}
void GPS_Init(Pointfp64 position,fp64 radian)
{
int i;
Encoder_Clear(0);
Encoder_Clear(1);
for(i=0;i<GPS_STATCOUNT;i++)
{
Gps_List[i].distance1=0.0;
Gps_List[i].distance2=0.0;
Gps_List[i].distancestep1=0.0;
Gps_List[i].distancestep2=0.0;
Gps_List[i].position=position;
Gps_List[i].angle=radian/PI*180.0;
Gps_List[i].radian=radian;
Gps_List[i].speed=0.0;
Gps_List[i].acceleration=0.0;
Gyro_Total=Gps_List[i].angle/0.0000001/Gyro_Convert1;
Gyro_Now.count=0;
}
}
void ENC_L_INIT_STATE(void)
{
/*
s_el 码盘正反转初始化状态时的状态标志
0--初始化还未开始
1--校准正传系数
2--校准反正系数
3--告知主控陀螺仪标定完成
*/
static uint8_t s_el = 0;
switch(s_el)
{
case 0:
s_el = 1;
Encoder_Clear(0);
Encoder_Clear(1);
break;
case 1:
s_el = 2;
Encoder_InitXY(0);
break;
case 2:
s_el = 3;
Encoder_Clear(0);
Encoder_Clear(1);
break;
case 3:
s_el = 0;
Encoder_InitXY(1);
Encoder_Init();
Data_Save();
SS_H = 0; //复位状态标志
break;
default : s_el = 1;
Encoder_Clear(0);
Encoder_Clear(1);
break;
}
}
int main(void)
{
while (1);
/* Configuration */
SystemInit();
LED_Configuration();
BUZZ_Configuration();
//ADC_Configuration();
CAN_Configuration();
/****************************µ×Å̳õʼ»¯******************************/
Elmo_Init(elmo, 3);
// PositionPID_Init();
PositionIPD_Init();
MoveLock();
Delay_ms(100);
BLUETOOTH_Configuration();
Delay_ms(100);
Encoder_Clear();
Delay_ms(1000);
MPU6500_init();
TIM1_Configuration();
TIM2_Configuration();
TIM3_Configuration();
TIM4_Configuration();
if (SysTick_Config(SystemCoreClock / 1000)) /* Setup SysTick Timer for 1 msec interrupts */
{
while(1){LED_ON(LED2);}/* Capture error */
}
// IpdAxisX.setpoint = 200;
// IpdAngle.setpoint = 90;
// PidAxisX.setpoint = 200;
// CMDVelocity.X = 10;
// CMDVelocity.Y = 0;
// CMDVelocity.A = 0;//Degree/Second
// Elmo_Write(&elmo[0],0x01,0x01,50);
while (1)
{
if(MPU_FLAG == 1)
{
MPU_FLAG = 0;
}
if(MAPAN_FLAG == 1)
{
MAPAN_FLAG = 0;
// LED_TOGGLE(LED1);
MapanTask();//ÂëÅÌ
}
if(PID_PFLAG ==1)
{
PID_PFLAG = 0;
// PositionPIDCal();
PositionIPDCal();
VelocityTransform();
// LED_TOGGLE(LED2);
}
if (LED_FLAG == 1)
{
LED_FLAG = 0;
// SquareTracking();
// CircleTracking();
// GoBack();
// LED_TOGGLE(LED3);
// SignalTracking();
// angle_print();
// PositionVelocity_print();
}
}
}
