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(); } } }