/*---------------------------------------------------------------------
ATtiny2313 @ 14.74MHz　朱天鳳仙花、4024駆動用サブボードプログラム

WinAVR20070525 with AVR Studio 4.13 (build 557)
最適化オプション無し


　4024に限らず、ICS2.0のコマンドを受信して、PWM出力に変換する。

　…にしても、日本語訳されたデータシートやら『AVRマイコンリファレンスブック』やら、
　ベストテクノロジーさんのサンプルやらを参考に継ぎはぎしていったら、
　作法もへったくれもない読み難いコードになっちゃいました（＾＾；

---------------------------------------------------------------------*/


#include <avr/io.h>
#include <avr/interrupt.h>


#define FOSC 14745600		// 14.745600MHz
#define BAUD 115200			// 115.2kbps
#define MYUBRR FOSC/16/BAUD-1	// UART分周率 標準動作

#define ICS100	184  // キャラクタリスティック変更用　setting 1
#define ICS150	276  // キャラクタリスティック変更用　setting 2
#define ICS200	389  // キャラクタリスティック変更用　setting 3
#define PWM0700 1291 // 8分周で700us  KONDOサーボのパルスリミット（下）
#define PWM0900 1660 // 8分周で900us  JRサーボのパルスリミット（下）
#define PWM1500 2764 // 8分周で1500us サーボニュートラル
#define PWM2100 3870 // 8分周で2100us JRサーボのパルスリミット（上）
#define PWM2300 4239 // 8分周で2300us KONDOサーボのパルスリミット（上）
#define PWM2500	4608 // 8分周で2500us 20msの8分割

#define COM_SV_MOVE	0x80
#define COM_SV_STAT	0xA0

short				 	currPWM[8];
short				 	tempPWM[8];

short 				ICSVAL;
unsigned char buf[3];
unsigned char svChn;

unsigned char svHChn;

unsigned char tempCh;



void initUART(unsigned int baud) {

	UBRRH = (unsigned char)( baud>>8 );		// ボーレート上位
	UBRRL = (unsigned char)baud;					// ボーレート下位
	UCSRB = ( 1<<RXEN ) | ( 1<<TXEN );		// 送受信許可
	UCSRC = ( 2<<UPM0 ) | ( 3<<UCSZ0 );  	// パリティEVEN　データ8bits

}

char getUART() {
  loop_until_bit_is_set(UCSRA, RXC);// 受信バッファ有無
	return UDR;
}


ISR(SIG_TIMER1_COMPA) {
	TCNT1 = 0;

	svChn ++;
	if(svChn > 8)svChn = 0;
	PORTB = svChn;
	PORTD |= _BV(PD5);
	OCR1B = currPWM[(short)svChn];			// パルス幅を指定するだけで直線補完すらしていない…（＾＾；

	if( tempCh == 1 ) {									// キャラクタリスティックチェンジをしている場合の措置。
		currPWM[(short)svChn] = tempPWM[(short)svChn];		// 保存しておいたパルス値を元に戻す。
		tempCh = 0;
	}
}

ISR(SIG_TIMER1_COMPB) {

	PORTD &= ~_BV(PD5);

}

void initPWM() {
    
	TCCR1B= (0<<ICNC1) | (0<<ICES1) | (0<<WGM13) |(0<<WGM12) | (0<<CS12) |(1<<CS11) | (0<<CS10) ;
	TIMSK = (0<<TOIE1) | (1<<OCIE1A) | (1<<OCIE1B) | (0<<ICIE1) | (1<<OCIE0B) | (0<<TOIE0) | (1<<OCIE0A);

	char i = 0;
	for( i = 0; i < 8; i++) currPWM[(unsigned short)i] = PWM1500;

	svChn = 0x00;
	PORTB = svChn;
	PORTD |= _BV(PD5);

	OCR1A = PWM2500;
	OCR1B = currPWM[(short)svChn];

}

void setDuty( unsigned char ch ) {
	int VH, VL, Val;

	VH = 0;
	VL = 0;

	VH = (short)buf[1];
	VH = VH << 7;
	VL = (short)buf[2];
	ICSVAL = VH | VL;   												// 7BIT＋7BIT で数字を解釈
	Val = (ICSVAL - 7500) / 27 * 10 + PWM1500;	// ICS2の指示値からパルス幅を作成。
	if( Val >= PWM2300 ) Val = PWM2300;					// パルス幅の上限を超えない為の安全措置。メーカー毎に変える。
	if( Val <= PWM0700 ) Val = PWM0700;					// パルス幅の下限を超えない為の安全措置。メーカー毎に変える。
	currPWM[(short)ch] = Val;										// 指定チャンネルのサーボの目標位置を設定。
}

void setSCom( unsigned char ch ) {

	char temp;

	tempCh = 1;

	temp = buf[2] / 3;																// スピードやストレッチの変更は、サーボのセッティングを変更するだけ。
	tempPWM[(short)ch] = currPWM[(short)ch];					// よってICS2.0のサブコマンドは無視。
																										// サブコマンド直後に送信される数字が、
	if( temp >= 40 ) currPWM[(short)ch] = ICS200;     // 120以上の時、セッティング3
	else if( temp > 20 ) currPWM[(short)ch] = ICS150; // 120未満で60より大きい時、セッティング2
	else currPWM[(short)ch] = ICS100;									// それ以外の時セッティング1
}

void initIO() {

	DDRB = 0xe7;				// PB0→マルチプレクサのA　PB1→マルチプレクサのB　PB2→マルチプレクサのC
	DDRD = 0x75;				// PD5からパルス出力。PWM出力機能は使わないで割り込みを使ってソフトでON-OFF
	PORTB = 0x00;
}

char chkSW() {
	char sw;
	sw = PINB & 0xfc;  // PB4とPB3にDipスイッチを接続。4.7kΩでプルアップ。
	sw = sw >> 3;      // ONの時0、OFFの時1 になるように接続する。
	return sw;
}

int main(void) {

	char temp;

	initIO();									// IO定義
	initUART(MYUBRR);					// USART設定
	initPWM();								// PWM設定

	temp = chkSW();						// Dipスイッチの状態により、制御下にあるサーボのIDを決める。

	switch (temp) {
		case 0:	svHChn = 0;			// Dipスイッチが両方ともONの時、サーボIDは0〜7とする。
						break;
		case 1:	svHChn = 8;			// PB4に接続されている側のみONの時、サーボIDは8〜15とする。
						break;
		case 2:	svHChn = 16;		// PB3に接続されている側のみONの時、サーボIDは16〜23とする。
						break;
		default: svHChn = 24;		// Dipスイッチが両方ともOFFの時、サーボIDは24〜31とする。
						break;
	}

	sei();

	while(1) {

		buf[0] = getUART();
		buf[1] = getUART();
		buf[2] = getUART();

		svChn = ( buf[0] & 0x1f ) - svHChn;
		if( svChn < 8 ) {
			if( buf[0] & COM_SV_MOVE ) setDuty( svChn );
			else if( buf[0] & COM_SV_STAT ) setSCom( svChn ); 
		}
	}
}