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