//=================================================================================
//ʱ //: 16MHz
unit Servo
{
	link unit MCU {} = #.MEGA328;
	
	link uint8 TIMSK2 = MCU.TIMSK2;
	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;
	
	//ʼ
	public void OS_init()
	{
		PORTA_DIR = 0xFF;
		PORTA_OUT = 0x00;
		
		//CTC OCRA
		MCU.TCCR2A = 0B0000_0010;
		MCU.TCCR2B = 0B0000_0001;
		
		MCU.OCR2A = 160;
		
		MCU.TIMSK2 = 0B0000_0010;
		
		TickNumber = 250;
		n = 0;
		tick = 0;
		out_put = 0B0000_0001;
		PWM_A = 60;
		
		//uint8 d = 50;
		
		PWM[0] = 90;
		PWM[1] = 90;
		PWM[2] = 90;
		PWM[3] = 90;
		PWM[4] = 90;
		PWM[5] = 90;
		PWM[6] = 90;
		PWM[7] = 90;
	}
	//---------------------------------------------------
	//жϷ
	
	uint8 d;
	
	interrupt [MCU.WATCH.TIMER2_COMPA] fast
	void timer1_compA()
	{
		#asm "push r26"
		#asm "push r27"
		#asm "push r30"
		#asm "push r31"
		#asm "in r31,0x3f"
		#asm "push r31"
		
		//MCU.TIMSK = 0B0000_0000;
		//...#asm "clr r31"
		//...#asm "out &TIMSK1-0x20,r31"
		
		//...#asm "sei"
		
		//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 "@if0_a:"
			
			//, ԰ tick + 1 ŵ if( tick != TickNumber ) { 
			//tick + 1;
			#asm "lds r31,&tick"
			#asm "inc r31"
			#asm "sts &tick,r31"
			
			//...#asm "cli"
			
			//MCU.TIMSK = 0B0000_0010;
			//...#asm "ldi r31,0B00000010"
			//...#asm "out &TIMSK1-0x20,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 "ldi r30,&PWM%256"
		#asm "ldi r31,&PWM/256"
		#asm "lds r26,&n%256"
		#asm "lds r27,&n/256"
		#asm "add r30,r26"
		#asm "adc r31,r27"
		#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 "cli"
		//MCU.TIMSK | 0B0000_0010;
		//...#asm "ldi r31,0B00000010"
		//...#asm "out &TIMSK1-0x20,r31"
		
		#asm "@end:"
		#asm "pop r31"
		#asm "out 0x3f,r31"
		#asm "pop r31"
		#asm "pop r30"
		#asm "pop r27"
		#asm "pop r26"
	}
	//---------------------------------------------------
	public [uint8*16] PWM;
	uint8 n;
	uint8 out_put;
	uint8 tick;
	
	uint8 PWM_A;
	
	link uint8 PORTA_OUT = MCU.PORTD;
	link uint8 PORTA_DIR = MCU.DDRD;
}