#include "Wprogram.h"
int _ServoPin;
void servoAttach(int pin){
	if(pin!=12 && pin!=13){	_ServoPin=0;	return;	}
	pinMode((_ServoPin=pin), OUTPUT);
#define ICR1 (F_CPU/8/50)
	ICR1H=(ICR1>>8);	ICR1L=ICR1&0xff;
	TCCR1A=(pin==12?0x82:0x22);	// if(TCNT1==OCR1A/B) OC1A/B=0
	TCCR1B=0x1a;			// Fast PWM, TOP=ICR1, CK/8
}
void servoWrite(int deg){
	int OCR1;
        OCR1=(F_CPU/8000)+(F_CPU/80000/18)*deg;
	if(_ServoPin==12){
		OCR1AH=(OCR1>>8);	OCR1AL=OCR1&0xff;
	}else if(_ServoPin==13){
		OCR1BH=(OCR1>>8);	OCR1BL=OCR1&0xff;
	}
}
