
//=================================================================================
//ʱ //: 12MHz
unit SYS_Servo
{
	link unit MCU {} = #.COM_MCU;
	
	uint8 TickNumber;
	
	//---------------------------------------------------
	//: 16MHz
	//һڹ̶20ms = 20000us, 8Ҫ20000usɨ, ÿҪ2500us
	
	//ÿΪ100, жΪ 2500us / 100 = 25us, ƵΪ40000, t = 16M / 40000 = 400;
	//ÿΪ200, жΪ 2500us / 200 = 12.5us, ƵΪ80000, t = 16M / 80000 = 200;
	//ÿΪ250, жΪ 2500us / 250 = 10us, ƵΪ100000, t = 16M / 100000 = 160;
	//ÿΪ300, жΪ 2500us / 300 = 8.3333us, ƵΪ100000, t = 16M / 120000 = 133;
	
	//---------------------------------------------------
	//ʼ
	//[i] function_void OS_init;
	public void OS_init()
	{
		MCU.TCCR2A = 0B0000_0000;
		MCU.TCCR2B = 0B0000_1001;
		
		uint32 f = $freq$;
		MCU.OCR2A = (uint8)(uint16)(f / 100000);
		
		MCU.TIMSK2 = 0B0000_0010;
		n = 0;
		tick = 0;
		out_put = 0B0000_0001;
		
		PWM_A = 60;
		TickNumber = 250;
		
		//ʼǶΪмֵ
		//Ҫ, Ϊģл
		//for( int8 i = 0; i < 8; i + 1 ) {
		//	PWM[i] = 128;
		//}
	}
	//---------------------------------------------------
	//ýǶ
	//[i] function_void SetValue int32;
	public void SetValue( int32 d )
	{
		PWM[0] = (uint8)(uint16)(uint)d;
	}
	
	//---------------------------------------------------
	//жϷ
	interrupt [MCU.WATCH.TIMER2_COMPA] fast
	void timer2_compA()
	{
		#asm "push r25"
		#asm "push r26"
		#asm "push r27"
		#asm "push r30"
		#asm "push r31"
		#asm "in r31,0x3f"
		#asm "push r31"
		
		//if( tick != TickNumber ) {
		#asm "lds r30,&tick"
		#asm "lds r31,&TickNumber"
		#asm "cp r31,r30"
		#asm "breq @if0_end"
			
			//if( tick == PWM_A ) PORTA_OUT = 0x00;
			//#asm "clr r31"
			#asm "lds r30,&tick"
			#asm "lds r26,&PWM_A"
			
			#asm "cp r30,r26"
			#asm "brne @if0_a"
			//#asm "out &PORTA_OUT-0x20,r31"
			
			
			#asm "call &#.SYS_Servo_Clear"
			//#..driver.D.D0_OUT = 0;
			
			
			#asm "@if0_a:"
			
			//, ԰ tick + 1 ŵ if( tick != TickNumber ) { 
			//tick + 1;
			#asm "lds r31,&tick"
			#asm "inc r31"
			#asm "sts &tick,r31"
			
			#asm "jmp @end"
			//return;
		//}
		#asm "@if0_end:"
		
//		//HeadTick = 0;
//		#asm "clr r31"
//		#asm "sts &HeadTick,r31"
		
		//tick = 0;
		#asm "clr r31"
		#asm "sts &tick,r31"
		
		//out_put << 1;
		#asm "lds r31,&out_put"
		#asm "lsl r31"
		#asm "sts &out_put,r31"
		
		//n + 1;
		#asm "lds r31,&n"
		#asm "inc r31"
		#asm "sts &n,r31"
		
		//if( n == 8 ) {
		#asm "lds r30,&n"
		#asm "cpi r30,8"
		#asm "brne @if1_end"
		
			//n = 0;
			#asm "clr r31"
			#asm "sts &n,r31"
			
			//out_put = 0B0000_0001;
			#asm "ldi r31,0B00000001"
			#asm "sts &out_put,r31"
		//}
		#asm "@if1_end:"
		
		//PWM_A = PWM[n];
		#asm "clr r25"
		#asm "ldi r30,&PWM%256"
		#asm "ldi r31,&PWM/256"
		#asm "lds r27,&n"
		#asm "add r30,r27"
		#asm "adc r31,r25"
		
		#asm "ld r26,Z"
		#asm "sts &PWM_A,r26"
		
		//PORTA_OUT = out_put;
		#asm "lds r31,&out_put"
		//#asm "out &PORTA_OUT-0x20,r31"
		
		
		//#asm "sbrc r31,0"
		//#..driver.D.D0_OUT = 1;
		//#asm "sbrs r31,0"
		//#..driver.D.D0_OUT = 0;
		#asm "call &#.SYS_Servo_SetPin"
		
		
		#asm "@end:"
		#asm "pop r31"
		#asm "out 0x3f,r31"
		#asm "pop r31"
		#asm "pop r30"
		#asm "pop r27"
		#asm "pop r26"
		#asm "pop r25"
	}
	//---------------------------------------------------
	public [uint8*8] PWM;
	uint8 n;
	uint8 out_put;
	uint8 tick;
	
	uint8 PWM_A;
}