//進行任務調度  這只是個輪詢的調度方法，不是很好，但是也不算太糟。
void OSSched(void) 
{  
   //  根據中斷時保存寄存器的次序入棧，模擬一次中斷后，入棧的情況 

 // __asm__ __volatile__("fisrt_last_OSSched: \n\t"); 
  __asm__ __volatile__("PUSH __zero_reg__         \n\t");  //R1 
  __asm__ __volatile__("PUSH __tmp_reg__          \n\t");  //R0  
  __asm__ __volatile__("IN   __tmp_reg__,__SREG__ \n\t");  //保存狀態寄存器SREG 
  __asm__ __volatile__("PUSH __tmp_reg__          \n\t");  //保存狀態寄存器SREG 
  __asm__ __volatile__("CLR  __zero_reg__         \n\t");  //R1重新清零 
  __asm__ __volatile__("PUSH R2                  \n\t");   
  __asm__ __volatile__("PUSH R3                  \n\t"); 
  __asm__ __volatile__("PUSH R4                  \n\t"); 
  __asm__ __volatile__("PUSH R5                  \n\t"); 
  __asm__ __volatile__("PUSH R6                  \n\t"); 
  __asm__ __volatile__("PUSH R7                  \n\t"); 
  __asm__ __volatile__("PUSH R8                  \n\t"); 
  __asm__ __volatile__("PUSH R9                  \n\t"); 
  __asm__ __volatile__("PUSH R10                  \n\t"); 
  __asm__ __volatile__("PUSH R11                  \n\t"); 
  __asm__ __volatile__("PUSH R12                  \n\t"); 
  __asm__ __volatile__("PUSH R13                  \n\t");     
  __asm__ __volatile__("PUSH R14                  \n\t"); 
  __asm__ __volatile__("PUSH R15                  \n\t"); 
  __asm__ __volatile__("PUSH R16                  \n\t"); 
  __asm__ __volatile__("PUSH R17                  \n\t");     
  __asm__ __volatile__("PUSH R18                  \n\t");  
  __asm__ __volatile__("PUSH R19                  \n\t"); 
  __asm__ __volatile__("PUSH R20                  \n\t"); 
  __asm__ __volatile__("PUSH R21                  \n\t"); 
  __asm__ __volatile__("PUSH R22                  \n\t"); 
  __asm__ __volatile__("PUSH R23                  \n\t"); 
  __asm__ __volatile__("PUSH R24                  \n\t"); 
  __asm__ __volatile__("PUSH R25                  \n\t");     
  __asm__ __volatile__("PUSH R26                  \n\t"); 
  __asm__ __volatile__("PUSH R27                  \n\t"); 
 //__asm__ __volatile__("PUSH R28                  \n\t"); 
 //__asm__ __volatile__("PUSH R29                  \n\t");     
  __asm__ __volatile__("PUSH R30                  \n\t");  
  __asm__ __volatile__("PUSH R31                  \n\t");  

  __asm__ __volatile__("PUSH R28                  \n\t");  //R28與R29用于建立在堆棧上的指針 
  __asm__ __volatile__("PUSH R29                  \n\t");  //入棧完成 
   //===========================================  
     __asm__ __volatile__("Interrupt_OSSched:              \n\t");  
 TCB[OSTaskRunningPrio].OSTaskStackTop=SP;           //將正在運行的任務的堆棧底保存 
//=============================================
    unsigned char OSNextTaskPrio,j=0;   
	OSNextTaskPrio=OSTaskRunningPrio;
	OSRdyTbl &= ~(0x01<<idle_id);//掛起os_idle task 
	//******
next_task_id:  if(++OSNextTaskPrio>=OS_TASKS) //輪流運行各個任務，沒有優先級 
                   OSNextTaskPrio=0; 
				  if(++j>OS_TASKS)  OSRdyTbl |= (0x01<<idle_id); //如果所有的任務都是掛起的，那么就進入idle
	  if(! (OSRdyTbl & (0x01<<OSNextTaskPrio)))  goto next_task_id;
//*****
 OSTaskRunningPrio=OSNextTaskPrio;
 //***********************
  cli();  //保護堆棧轉換 
  SP=TCB[OSTaskRunningPrio].OSTaskStackTop; 
  sei(); 
   //===========================================  
    //根據中斷時的出棧次序     
  __asm__ __volatile__("POP  R29                  \n\t");     
  __asm__ __volatile__("POP  R28                  \n\t");
//===============================         
  __asm__ __volatile__("POP  R31                  \n\t");     
  __asm__ __volatile__("POP  R30                  \n\t");     
  __asm__ __volatile__("POP  R27                  \n\t");     
  __asm__ __volatile__("POP  R26                  \n\t");     
  __asm__ __volatile__("POP  R25                  \n\t");     
  __asm__ __volatile__("POP  R24                  \n\t");     
  __asm__ __volatile__("POP  R23                  \n\t");     
  __asm__ __volatile__("POP  R22                  \n\t");     
  __asm__ __volatile__("POP  R21                  \n\t");     
  __asm__ __volatile__("POP  R20                  \n\t");     
  __asm__ __volatile__("POP  R19                  \n\t"); 
  __asm__ __volatile__("POP  R18                  \n\t");
  __asm__ __volatile__("POP  R17                  \n\t");   
  __asm__ __volatile__("POP  R16                  \n\t"); 
  __asm__ __volatile__("POP R15                  \n\t"); 
  __asm__ __volatile__("POP R14                  \n\t"); 
  __asm__ __volatile__("POP R13                  \n\t"); 
  __asm__ __volatile__("POP R12                  \n\t"); 
  __asm__ __volatile__("POP R11                  \n\t"); 
  __asm__ __volatile__("POP R10                  \n\t"); 
  __asm__ __volatile__("POP R9                   \n\t"); 
  __asm__ __volatile__("POP R8                   \n\t"); 
  __asm__ __volatile__("POP R7                   \n\t"); 
  __asm__ __volatile__("POP R6                   \n\t");     
  __asm__ __volatile__("POP R5                   \n\t"); 
  __asm__ __volatile__("POP R4                   \n\t"); 
  __asm__ __volatile__("POP R3                   \n\t"); 
  __asm__ __volatile__("POP R2                   \n\t");      
  __asm__ __volatile__("POP  __tmp_reg__          \n\t");      //SERG 出棧并恢復 
  __asm__ __volatile__("OUT  __SREG__,__tmp_reg__ \n\t");      // 
  __asm__ __volatile__("POP  __tmp_reg__          \n\t");      //R0 出棧 
  __asm__ __volatile__("POP  __zero_reg__         \n\t");      //R1 出棧 
  __asm__ __volatile__("RETI                      \n\t");     //返回并開中斷 
    
  //中斷時出棧完成 
} 

//開始任務調度
void OSStartTask(uint8 id)         
{ 
OSTaskRunningPrio=id; 
  SP=TCB[id].OSTaskStackTop;
__asm__ __volatile__("RJMP Interrupt_OSSched      \n\t");  //重新調度 
//調用子函數時