task main(){ int spdCmd; //---------------------------INIT-----------------------------------// Pid_Init(); //---------------------------END INIT-------------------------------// // while(SensorValue(endPgm) == 0){ while(true){ ClearTimer(T1); hogCPU(); //Prevent other tasks from running when this one is. //writeDebugStreamLine("Foreground\n"); //Let me know when the foreground runs count=count+1; // Count the number of times the foreground runs. //--------------------------FOREGROUND------------------------------------// spdCmd=50; int mtrCnt=nMotorEncoder[rtMotor]; int mtrCmd=Pid(spdCmd, mtrCnt, mtrCntOld, integral); motor[rtMotor]=motor[ltMotor]=mtrCmd; //--------------------------END FOREGROUND--------------------------------// timeLeft=FOREGROUND_MS-time1[T1]; // Calculate the time used in the foreground releaseCPU(); // Let other tasks run now. wait1Msec(timeLeft);// The time other tasks have to run before foreground takes control. //------------Debug------------// if (count==1){ writeDebugStreamLine("SpdCmd[%i],Kp[%i],Ki[%i],Kd[%i]",spdCmd,Kp,Ki,Kd); writeDebugStreamLine("TimeLeft,Error,MtrCmd,Speed,Integral,Distance"); } writeDebugStreamLine("%2i,%3i,%3i,%3i,%3i,%5i",timeLeft,error,mtrCmd,speed,integral,mtrCnt); }// While Loop }// Foreground Main Task
int main(void) { SysTick_init(); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); uart_init(115200); Struct_Init(); Pid_Init(); MotoPwm_Init(); OLED_Init(); Key_Init(); I2cMaster_Init(); mpu_dmp_init(); GPIO_Config(); Filter_Init(); // NRF_Usart_Pin_Init();//无线初始化 // NRF_Read_Live_Init();//无线在线传输 // ReadData_Init(); Scheduler_Init(); while(1) { Scheduler_Loop(); } }