///////////////////////////////////////////////////////////////////
//
//	複数サーボ駆動回路用プログラム
//	ATmega328P 使用
//
//　・タイマ０とタイマ２の割り込みを使用して
//	　合計１６個までのサーボ信号を出力
//　・タイマのプリスケーラを０，２共通の(clk/64)にしたので
//	　サーボ信号の分解能は低い (clk = 1MHz)
//　　分解能をあげるには外部クロックを使用
//	・2.5msごとに割り込みをかけて　そのたびに出力するピンを切り替える
//	  	  _
//		_| |__________________
//			_
//		___| |________________
//            _
//		_____| |______________
//		
//	  つまり2.5ms×８＝20msで一つのピンにとってはサーボ信号用信号になる
//
//
//	・Ｉ２Ｃによる通信でサーボの値を指定する
//	・Ｉ２Ｃ用のアドレスは２ビットのスイッチによって指定する
//
//
//										ver1.0 (2011.12.1)
///////////////////////////////////////////////////////////////////

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

#define PWM_SEL data[0]
#define PWM_NUM	data[1]
#define PWM_POS data[2]

	unsigned char i = 0 , j = 0;
	unsigned char PWM0[8] = {15,15,15,23,23,23,31,31};
	unsigned char PWM1[8] = {31,31,31,23,23,23,15,15};

///////////////////////////////////////////////////////////////
//
//	タイマ０、２のコンペアマッチＡ割り込みで
//  出力するピンを切り替えて、同時にコンペアマッチＢ用の
//	値をセットしておく　
//	コンペアマッチＢ割り込みが発生するまでは
//　選択したピンをHにしておく。
//
//	タイマ０はポートＤの８ビット
//	タイマ２はポートＢとＣの下位４ビットづつなので
//　割り込みプログラムの記述が違う
//
////////////////////////////////////////////////////////////////

ISR ( TIMER0_COMPA_vect){
	unsigned char puls = 0x01;

	OCR0B = PWM0[i];
	puls = puls << i;
	PORTD = puls;
	i++;
	if(i == 8) i = 0;
}


ISR ( TIMER2_COMPA_vect){

	OCR2B = PWM1[j];
	switch(j){
		case 0: PORTB = 0x01; break;
		case 1: PORTB = 0x02; break;
		case 2: PORTB = 0x04; break;
		case 3: PORTB = 0x08; break;
		case 4: PORTC = 0x01; break;
		case 5: PORTC = 0x02; break;
		case 6: PORTC = 0x04; break;
		case 7: PORTC = 0x08; break; 
	}
	j++;
	if(j == 8) j = 0;
}

///////////////////////////////////////////////////
//
//  タイマ０、２　それぞれのコンペアマッチＢで
//  出力をクリアする
//
///////////////////////////////////////////////////

ISR ( TIMER0_COMPB_vect){
	PORTD = 0;
}

ISR ( TIMER2_COMPB_vect){
	PORTC = PORTC & 0xF0;
	PORTB = PORTB & 0xF0;
}


int main(void){	
 	unsigned char sel = 0;
	unsigned char data[3];

	////////////////////////////////////////////////
	//   ポートＢ　０〜３：サーボ出力１下位４ビット
	//				４,５：スイッチ入力 アドレス用
	//				６,７：未使用（外部発振）
	//
	//　 ポートＣ  ０〜４：サーボ出力１上位４ビット
	//				４,５：Ｉ２Ｃ
	//				６,７：未使用
	//
	//	 ポートＤ　０〜７：サーボ出力0　８ビット
	////////////////////////////////////////////////

	DDRB  = 0b11001111;		
	DDRC  = 0b11111111;		
	DDRD  = 0b11111111;		//ポートD全て出力
	
	PORTB = 0b00000000;     //ポートB初期化
	PORTC = 0b00000000;		//ポートC初期化
	PORTD = 0b00000000;		//ポートD初期化
	
	//********** Ｉ２Ｃ初期設定　*******************//

	TWBR = 0xFF;			//クロック　約２kHz
	TWAR = 0b10000000;		//アドレス
							//一斉呼び出し不可


	//********** サーボ出力０用割り込み設定 ********//
	//タイマ０使用

	TCCR0A = 0b00000010;	//CTCモード	
	TCCR0B = 0b00000011;	//プリスケーラ　clk/64
	OCR0A = 39;				//2.5ms周期で割り込み
	OCR0B = 23;				//Hの時間初期値 1.5ms
	TIMSK0 = 0b00000110;	//ABコンペアマッチ割り込み有効
	
	//********** サーボ出力１用割り込み設定 ********//
	//タイマ２使用
	TCCR2A = 0b00000010;	//CTCモード
	TCCR2B = 0b00000100;	//プリスケーラ　clk/64
	OCR2A  = 39;			//2.5ms周期で割り込み
	OCR2B  = 23;			//Hの時間初期値 1.5ms
	TIMSK2 = 0b00000110;	//ABコンペアマッチ割り込み有効
		
	sei();					//全体割り込み許可
	

	//********** メインループ ******************//
	while(1){
		sel = 0;
		TWCR = 0b11000100;	//アドレス待ち	
		while(!(TWCR & 0x80));//受信待ち

		if((TWSR & 0xF8) == 0x60){	//該当アドレスなら
			while(1){
				TWCR = 0b11000100;	//データ待ち
				while(!(TWCR & 0x80));//受信待ち

				if((TWSR & 0xF8) == 0xA0) break;
									//停止条件ならループを抜ける
				data[sel] = TWDR;
				sel++;
				if(sel == 3) sel = 0;
			}

			//******** サーボの値の更新 ********//
			if(PWM_SEL == 0){
				PWM0[PWM_NUM] = PWM_POS;
			}
			else if(PWM_SEL == 1){
				PWM1[PWM_NUM] = PWM_POS;
			}	
		}
	}
	return 0;		
}