/*
	PIDによるサーボモータ制御用ソフトウェア

	対象ハードウェア：秋月電子製サーボモータ学習キット
	ＤＣモータ：タミヤ製ハイパワーギヤボックス HE(72003)
	CPU：PIC18F1320
	Cコンパイラ：Microchip C18

	作成：鳴門教育大学　技術・工業・情報コース
	　　　伊藤研究室

	作成履歴：
		2009年 7月 8日　新規 バージョン 1.0
		2009年 8月 2日　多重割込を使わないバージョン 2.0
		2009年 8月 8日　異常動作をできるだけしないようなバージョン 2.3
		　　　　　　　　ウォッチドッグタイマによる異常動作の検出
		　　　　　　　　メモリが異常になったときリセット
		　　　　　　　　モータに過度の回転制御がなされたとき停止
*/
#define SVPID_VER "svPID-v2.3\x0D"	// 本プログラムの名称，電源ＯＮ時に出力

/* --------------------------------------------------------------------------
【取扱説明書】
　PID制御により基板上のポテンショメータまたは，コンピュータのシリアル通信用ソフトウェアを使って，ＤＣモータをサーボ制御する。

【コマンド】
h           簡単なヘルプの出力
b         * 基板上のポテンショメータによる指示モードを設定
c         * コンピュータからの通信による指示モードを設定
p           PID制御用パラメータ(Kp, Ti, Td)をそれぞれBIAS倍し，整数値として出力
r         * PID制御用パラメータをEEPROMから読み込み設定し，出力
w           現在設定されているPID制御用パラメータをEEPROMに書き込み，出力
v           ポテンショメータの指示値，モータのポテンショメータの値，制御量×BIAS，制御量の変化分×BIAS
z           ログの出力を開始（ログの出力個数設定が０のとき，無限）
x           ログの出力を停止
l<n>[CR]    ログの出力個数を設定 （<n>は0〜10000,省略すると0）
k<n>[CR]  * PID制御用パラメータKpの値をBIAS倍した整数値で設定（<n>は省略すると0）
i<n>[CR]  * PID制御用パラメータTi[秒]の値をBIAS倍した整数値で設定 (0のときは，無限大を示す)（<n>は省略すると0）
d<n>[CR]  * PID制御用パラメータTd[秒]の値をBIAS倍した整数値で設定 (0のときは，無限大を示す)（<n>は省略すると0）
a<n>[CR]    ポテンショメータの指示値をnに設定 （<n>は0〜1023,省略すると0）
A<n>[CR]    ポテンショメータの指示値をnに設定し，ログを出力（ログの出力個数設定が０のとき，無限） （<n>は0〜1023,省略すると0）

*は，コマンドが正常に動作した後，PIDの制御量をリセットする。
[CR]は，エンターキー(0x0D)を示す。
<n>の設定をキャンセルする場合,DELまたはBSキーを送信する

ログは，モータのポテンショメータの値と指示値を","で区切って出力

BIASは1000とする。

主な仕様：
　　制御間隔　　　 ：2ミリ秒
　　PWM信号の周波数：2.4KHz，PWMの分解能:10bit(0〜1023)
　　ポテンショメータの分解能：10bit(0〜1023)
主な機能：
　JP4のジャンパをショートして電源ＯＮにすると，PID制御用パラメータをデフォルト値にリセットしEEPROMに書き込む。
　JP4のジャンパがショートのとき，PID制御は無効となり，PIDの制御量はリセットされる。

　モータ制御のPWMのデューティ比の最大値が設定された状態が約2秒以上継続すると，PID制御が不能と判断し，PIDの制御量をリセットする。

モータ・ノイズによる異常動作について
　PICマイコン系とモータ系の電源が絶縁されていない場合，モータからのノイズによりPICマイコンがリセットされることがある。
　さらに，通信回路にもノイズが入り通信データが異常になることがある。
　このようなノイズによる異常動作を防止するためには，PICマイコン系とモータ系の電源をフォトカプラによって絶縁する必要がある。（回路の修正が必要）
*/

// --------------------------------------------------------------------------
// コンパイルのオプション設定
// PWM信号のフォトカプラによる絶縁回路用の場合，以下の記号定数を定義する。
#define ISOLATED_CIRCUIT

// USART (RS-232C)のボーレートの設定
// コンピュータ側RS-232Cの設定：115200bps, 8ビット, パリティなし，1 ストップビット
// フロー制御していないため，コンピュータ側からデータを送信する場合，１バイト毎に10ミリ秒のウェイトを入れること。
// 115200bpsよりボーレートを低くすると，ログの出力が間に合わなくなる。
// シリアル通信用ソフトウェアが改行コード(16進数0D)を受信したとき，コンピュータのスクリーンにおいても改行するように設定する。
#define USART_BAUD 115200		// デフォルト
// #define USART_BAUD 57600
// #define USART_BAUD  9600

// ポートBのRB0をデバッグ用出力信号としてに使う場合，以下の記号定数を定義する。
// #define SV_DEBUG

// --------------------------------------------------------------------------
#include <p18f1320.h>
#include <stdlib.h>
#include <reset.h>
#include <portb.h>
#include <timers.h>
#include <adc.h>
#include <usart.h>
#include <pwm.h>
#include <delays.h>		// １命令の実行時間 = 1 / (Fosc / 4)
						//                  = 1 / (40M  / 4) = 0.1μ秒
// コンフィグレーション・ビットの設定開始
#pragma config OSC   = HSPLL	// クロック発振モード：PLLモード(内部で４倍になる。)
								// 内部動作クロック(Fosc) =外部振動子(セラロック)の発振周波数10MHz x 4 = 40MHz
#pragma config WDT   = ON, WDTPS = 64	// ウォッチドッグタイマ：使用する
										// タイムアップまでの時間：125/31000 * 64 = 0.258秒

// すべてのメモリプロテクトをしない
#pragma config CP0   = OFF, CP1   = OFF, CPB   = OFF, CPD   = OFF
#pragma config WRT0  = OFF, WRT1  = OFF, WRTB  = OFF, WRTC  = OFF, WRTD  = OFF
#pragma config EBTR0 = OFF, EBTR1 = OFF, EBTRB = OFF

#pragma config LVP   = OFF	// 低電圧ICSP制御：使用しない
#pragma config BOR   = OFF	// ブラウンアウトリセット：使用しない
#pragma config MCLRE = OFF	// MCLRピン：使用しない　→　RA5が利用可

#pragma config PWRT  = OFF	// パワーアップタイマ：使用しない
#pragma config FSCM  = OFF	// フェールセーフ・クロック監視：禁止
#pragma config IESO  = OFF	// システムクロックの内外切り替え：禁止
// コンフィグレーション・ビットの設定終了

unsigned char mem_chk_0 = 0xAA;			// メモリ異常検査用

#define LEN_RQUE 4						// USART用受信バッファのバイト数
unsigned char head_a = 0;				// 受信バッファのインデックスはメモリ異常時検査をかねて二重構成とする
unsigned char head_b = 0;
unsigned char tail_a = 0;
unsigned char tail_b = 0;
unsigned char rque [LEN_RQUE];		 	// USART用受信バッファ

struct {
	unsigned update_flag:1;			// タイマ０のオーバーフロー割り込みの発生フラグ
	unsigned logout_flag:1;			// ログを出力するフラグ
	unsigned input_num_sign:1;		// 入力パラメータ数値の符号(0:正,1:負)
} FlagBits;

#define NUM_LOG_COUNT 500			// コンピュータ制御でログを出力する個数のデフォルト値
#define MAX_LOGLEN  10000			// ログの出力個数の最大値
unsigned int log_count_num = NUM_LOG_COUNT;
unsigned int log_count;				// ログを出力する個数（０のときは，無制限）

// リレーを動作させるための出力ポート
#define RELAY_A  PORTBbits.RB2	// 0:スイッチング側　1:Vcc側
#define RELAY_B  PORTBbits.RB7	// 0:スイッチング側　1:Vcc側

// JP4の入力ポート
#define JP4 PORTBbits.RB5		// オープン(1)のときPID制御を無効，ショート(0)の時有効
								// 電源ＯＮ時，ショートのとき，PIDパラメータをデフォルト値にしてEEPROMに書き込む。

#define MAX_CTL_COUNT_LIMIT 100	// モータに過度の回転制御がなされた場合を検出するための定数と変数
#define DUTY_LIMIT   512		// 制限デューティ比
int max_ctr_count = 0;			// 制限デューティ比の状態が続くときカウントアップ

unsigned char ctl_mode;		// PID制御のモード
#define NO_CONTROL    0		// モータが制御不能で，PID制御を無効にする
#define VOL_CONTROL   1		// 基板上のポテンショメータ(V1)を使って指示する
#define COM_CONTROL   2		// コンピュータ通信によって指示する

unsigned char cmd_mode;		// 入力コマンドのモード
long input_num;
#define NO_PARM       0		// パラメータのないコマンド
#define KP_PARM       1		// Kpの値を設定するコマンド
#define TI_PARM       2		// Tiの値を設定するコマンド
#define TD_PARM       3		// Tdの値を設定するコマンド
#define POS_PARM      4		// ポテンショメータの設定値を設定するコマンド
#define POS_PARM_LOG  5		// ポテンショメータの設定値を設定し，ログを出力するコマンド
#define LOGLEN_PARM   6		// ログの出力個数を設定するコマンド

// PIDのパラメータ
#define DELTA_T 0.002000				// 割り込み間隔：2000μ秒
#define TMR0_INIT_VAL (0x10000 - 20000)	// 必要なカウント数 = 割り込み間隔 / ((1/Fosc)×4)
										//                  = 2000 / ((1/40) × 4)
										//                  = 20000

#ifndef ISOLATED_CIRCUIT				// PWM信号の非絶縁回路用（オリジナル回路）
#define DEFAULT_KP (5.0*1000.0)			// デフォルトの比例定数
#define DEFAULT_TI  0.800				// デフォルトの積分時間
#define DEFAULT_TD  0.002				// デフォルトの微分時間
#endif

#ifdef ISOLATED_CIRCUIT					// PWM信号のフォトカプラによる絶縁回路用
#define DEFAULT_KP (5.0*1000.0)			// デフォルトの比例定数
#define DEFAULT_TI  0.800				// デフォルトの積分時間
#define DEFAULT_TD  0.002				// デフォルトの微分時間
#endif

#define BIAS_INT 1000			// PID制御関係の値の表示と入力するときに掛ける値
float bias = BIAS_INT;

#define MAX_POTENSION 1023		// ポテンショメータの最大値
#define MAX_DUTY      1023		// デューティ比の最大値
#define MAX_DUTY_BIAS 1023000L	// デューティ比の最大値のBIAS倍
#define POTENSION_LIMIT 100		// 指示位置の制限 POTENSION_LIMIT〜(MAX_POTENSION- POTENSION_LIMIT)に制限する。
								// コンピュータによる指示の場合，この指示位置の制限はしない。

// PIDパラメータ
unsigned char mem_chk_1 = 0x55;		// メモリ異常検査用
float Kp;		// 比例定数 ( * bias)

float Ti;		// 積分時間
#define TOO_SMALL_TI 0.000001
float Ki;		//   Ki = Kp * DELTA_T / Ti (Ti < TOO_SMALL_TI のとき，Ki = 0.0とする。)
				//        * bias
float Td;		// 微分時間
float Kd;		//   Kd = Kp * Td / DELTA_T
				//		  * bias

int set_pos;	// 基板上のポテンショメータの値
				// サーボモータへの指示値

int mot_pos;	// モータのポテンショメータの値

int e0 = 0;		// 今回の偏差
int e1 = 0;		// 前回の偏差

#define MAX_ABS_VN 10000000L	// Vnの絶対値の最大値
long Vn = 0;		// 今回の制御量
long delta_Vn;		// Vnの更新量

// EEPROMの割り当て
#define EEPROM_MAGIC_ADR0 0		// マジックナンバー 'YI'
#define EEPROM_MAGIC_ADR1 1
#define MAGIC0 'Y'
#define MAGIC1 'I'

#define EEPROM_KP  (EEPROM_MAGIC_ADR1 + 1)		// Kpをfloat型で記録
#define EEPROM_TI  (EEPROM_KP + sizeof(float))	// Tiをfloat型で記録
#define EEPROM_TD  (EEPROM_TI + sizeof(float))	// Tdをfloat型で記録

// EEPROMの初期化用データ
#pragma romdata config=0xf0000e
unsigned char rom rom_msg[] =
#define EEPROM_ID_MSG    0x0e
SVPID_VER					// プログラムのバージョン
"\x00"

#define EEPROM_HELP_MSG  (0x0e + sizeof(SVPID_VER))
"b,c:Vol/Computer control\x0D"	// ヘルプ・メッセージ
"p:Disp PID parm\x0D"
"r,w:Read/Write PID parm from/to EEPROM\x0D"
"v:Disp pos\x0D"
"z,x:Start/Stop log\x0D"

"\x0Dn<-1000*int\x0D"
"k<n>:Kp\x0D"
"i<n>:Ti\x0D"
"d<n>:Td\x0D"
"\x0Dn<-[0,1023]\x0D"
"a<n>:Set pos\x0D"
"A<n>:Set pos, Disp log\x0D"
"\x0Dn<-[0,10000]\x0D"
"l<n>:Set log len\x0D"
;		// 最後に\x00が付く


// 関数プロトタイプ宣言
void InterruptHandler (void);

void contol_servo (void);
void set_Ki (void);
void set_Kd (void);
void reset_pid_err(void);

void usart_cr (void);

void Delay_1M_TCYx(unsigned char t);

unsigned char read_onchip_eeprom (
	unsigned char address);

void write_onchip_eeprom (
	unsigned char address,
	unsigned char data);

float read_float_eeprom (
	unsigned char address);

void write_float_eeprom (
	unsigned char address,
	float x);

void read_PID_parm(void);
void write_PID_parm(void);

void disp_eeprom_str(
	unsigned char adr);

// 割り込みハンドラの開始
#pragma code InterruptVectorHigh = 0x08
void InterruptVector (void)
{
	_asm
	goto InterruptHandler
	_endasm
}
#pragma code

// 割り込み処理
#pragma interrupt InterruptHandler
void InterruptHandler () 
{
	if (mem_chk_0 != 0xaa) Reset();		// メモリ異常があった場合，リセットする。
	if (mem_chk_1 != 0x55) Reset();

	if (PIR1bits.RCIF) {		// USART受信割り込みか？
		if (tail_a != tail_b) Reset();		// メモリ異常があった場合，リセットする。

		rque [tail_a ++] = ReadUSART();
		if (tail_a == LEN_RQUE) tail_a = 0;
		tail_b = tail_a;

		PIR1bits.RCIF = 0;

	} else if (INTCONbits.TMR0IF) {	// タイマー０のオーバーフロー割り込みか？
		FlagBits.update_flag = 1;

		TMR0H = (TMR0_INIT_VAL >> 8) & 0x00ff;	// オーバーフローまでの間隔を設定
		TMR0L =  TMR0_INIT_VAL       & 0x00ff;
		INTCONbits.TMR0IF = 0;
	}

}
// 割り込みハンドラの終了

#pragma code
void main (void)
{
		if (mem_chk_0 != 0xaa) Reset();		// メモリ異常があった場合，リセットする。
		if (mem_chk_1 != 0x55) Reset();

#ifndef SV_DEBUG
	// ポートBの設定
	//        76543210
	TRISB = 0b01110001;	// RB7: 出力　リレーB
						// RB6: 入力　未使用
						// RB5: 入力  JP4 (ピンスイッチ)
						// RB4: 入力  USART RX(受信)　　AN6: デジタル設定
						// RB3: 出力　PWM信号
						// RB2: 出力　リレーA
						// RB1: 出力　USART TX(送信)　　AN5: デジタル設定
						// RB0: 入力　未使用
	OpenPORTB(PORTB_CHANGE_INT_OFF & PORTB_PULLUPS_ON);
#endif

#ifdef SV_DEBUG
	// ポートBの設定(RB0をデバッグに使う。)
	//        76543210
	TRISB = 0b01110000;	// RB7: 出力　リレーB
						// RB6: 入力　未使用
						// RB5: 入力  JP4 (ピンスイッチ)
						// RB4: 入力  USART RX(受信)　　AN6: デジタル設定
						// RB3: 出力　PWM信号
						// RB2: 出力　リレーA
						// RB1: 出力　USART TX(送信)　　AN5: デジタル設定
						// RB0: 出力　デバッグ用
	OpenPORTB(PORTB_CHANGE_INT_OFF & PORTB_PULLUPS_ON);
	PORTBbits.RB0 = 0;		// デバッグ用
#endif

	RELAY_A = 0;			// モータを停止
	RELAY_B = 0;

	Delay1KTCYx(50);		// リレーコイルの動作時間を待ち，リレーのノイズによる通信不良を防止
							// 0.1μ秒×1000×50 = 5000μ秒だけ待つ

	// AD変換器の設定
	OpenADC(
		ADC_FOSC_64 &		// Fosc / 64
							// Tad = 1 / (40MHz / 64) = 1.6 μ秒
							// (Tadは必ず1.6 μ秒以上)
		ADC_RIGHT_JUST &	// Least significant bits
		ADC_8_TAD,			// 取得時間　8 Tad = 1.6×8 = 12.8μ秒 
							// 総取得時間 (8+11) Tad = 1.6×19 = 30.4 μ秒
							
		ADC_CH0 &			// Channel 0
		ADC_INT_OFF &		// 割り込み禁止
		ADC_VREFPLUS_VDD &	// Vref+ = AVdd
		ADC_VREFMINUS_VSS,	// Vref- = AVss
// ADCON1 x6543210
//         dd---aa
		0b01100000);		// PORのときADCON1に設定する値
	ADCON1bits.PCFG6 = 1;	// AN6/RB4/RX　USART用受信ポートをデジタルに設定
	ADCON1bits.PCFG5 = 1;	// AN5/RB1/TX　USART用送信ポートをデジタルに設定

	// USARTの初期化
#if   USART_BAUD ==  9600
	baudUSART(
		BAUD_8_BIT_RATE  &	// 8ビット　通信速度生成
		BAUD_WAKEUP_OFF  &
		BAUD_AUTO_OFF);
	
	OpenUSART (
		USART_TX_INT_OFF  &	// 送信割り込みは使用しない
		USART_RX_INT_OFF  &	// 受信割り込みは一旦禁止にしておく
		USART_ASYNCH_MODE &	// 非同期通信
		USART_EIGHT_BIT   &	// 8ビット
		USART_CONT_RX     &	// 連続受信
		USART_BRGH_LOW,
						// 通信速度：9600bpsの設定
		64);				// Fosc=10MHz×4=40MHz
						// ボーレート = Fosc/(64*(spbrg + 1))
						//            = 40000000/(64*(64 + 1)) = 9615.4 bps
						// エラー率   = (9615.4 - 9600) / 9600 = 0.0016
						//            =  0.16(%)

#elif USART_BAUD == 57600
	baudUSART(
		BAUD_16_BIT_RATE &	// 16ビット　通信速度生成
		BAUD_WAKEUP_OFF  &
		BAUD_AUTO_OFF);
	
	OpenUSART (
		USART_TX_INT_OFF  &	// 送信割り込みは使用しない
		USART_RX_INT_OFF  &	// 受信割り込みは一旦禁止にしておく
		USART_ASYNCH_MODE &	// 非同期通信
		USART_EIGHT_BIT   &	// 8ビット
		USART_CONT_RX     &	// 連続受信
		USART_BRGH_HIGH,
						// 通信速度：57600bpsの設定
		172);			// Fosc=10MHz×4=40MHz
						// ボーレート = Fosc/(4*(spbrg + 1))
						//            = 40000000/(4*(172 + 1)) = 57803.5 bps
						// エラー率   = (57803.5 - 57600) / 57600 = 0.0035
						//            =  0.35(%)

#else					// デフォルト 115200bpsの設定
	baudUSART(
		BAUD_16_BIT_RATE &	// 16ビット　通信速度生成
		BAUD_WAKEUP_OFF  &
		BAUD_AUTO_OFF);
	
	OpenUSART (
		USART_TX_INT_OFF  &	// 送信割り込みは使用しない
		USART_RX_INT_OFF  &	// 受信割り込みは一旦禁止にしておく
		USART_ASYNCH_MODE &	// 非同期通信
		USART_EIGHT_BIT   &	// 8ビット
		USART_CONT_RX     &	// 連続受信
						// 通信速度：115200bpsの設定
		USART_BRGH_HIGH,
		86);			// Fosc=10MHz×4=40MHz
						// ボーレート = Fosc/(4*(spbrg + 1))
						//            = 40000000/(4*(86 + 1)) = 114942.5 bps
						// エラー率   = (114942.5 - 115200) / 115200 = -0.0022
						//            = -0.22(%)
#endif

	// PWMの設定（PWMはタイマ２を使う）
	OpenTimer2 (
		TIMER_INT_OFF &	// タイマ２の割り込みは使わない
		T2_PS_1_16);		// プリスケーラ（１：１６）

	OpenPWM1 (255);		// Tosc = 1 / Fosc = 1 / 40MHz = 0.025μ秒
						// PWM周期 = [(period) + 1] x 4 x Tosc x (TMR2 prescaler)
						//         = (255 + 1) x 4 x 0.025 x 16
						//         = 409.6μ秒 (周波数は2.4KHz)
	SetDCPWM1 (0);		// デューティを0にする。

	// タイマ０の設定
	OpenTimer0 (
		TIMER_INT_OFF &		// 一旦割り込みを禁止しておく
		T0_16BIT &			// 16ビットのカウンタを利用
		T0_SOURCE_INT &
		T0_PS_1_1);			// プリスケーラ（１：１）

	TMR0H = (TMR0_INIT_VAL >> 8) & 0x00ff;	// オーバーフローまでの間隔を設定
	TMR0L =  TMR0_INIT_VAL       & 0x00ff;

	// PID制御パラメータの設定
	if ((PORTBbits.RB5 == 0 ||									// JP4がショートのとき，または，
		read_onchip_eeprom (EEPROM_MAGIC_ADR0) != MAGIC0 ||
		read_onchip_eeprom (EEPROM_MAGIC_ADR1) != MAGIC1)) {	// 正常なマジックナンバーがEEPROMに記録されていないとき，
																// デフォルトのPID制御パラメータを設定
		Kp = DEFAULT_KP;
		Ti = DEFAULT_TI;
		Td = DEFAULT_TD;

		write_PID_parm ();

	} else {

		read_PID_parm();		// EEPROMに書き込まれているPID制御パラメータを設定
	}

	set_Ki ();					// TiからKiを計算
	set_Kd ();					// TdからKdを計算

	ctl_mode = VOL_CONTROL;		// 基板上のポテンショメータによる指示を設定

	FlagBits.update_flag = 0;
	FlagBits.logout_flag = 0;

	cmd_mode = NO_PARM;

	// 割り込みの設定
	RCONbits.IPEN      = 0;		// 優先順位を考慮しない割り込みを使う

	INTCON2bits.TMR0IP = 1;		// タイマ０の割り込みを「高位レベル」に設定
	INTCONbits.TMR0IE  = 1;		// タイマ０の割り込みを許可

	IPR1bits.RCIP      = 1;		// USARTの受信割り込みを「高位レベル」に設定
	PIE1bits.RCIE      = 1;		// USARTの受信割り込みを許可
	PIE1bits.TXIE      = 0;		// USARTの送信割り込みを禁止

	INTCONbits.PEIE    = 1;		// 周辺割り込みの許可
	INTCONbits.GIE     = 1;		// グローバル割り込みを許可（この時点で割り込み開始）

	// 起動メッセージを出力
	disp_eeprom_str(EEPROM_ID_MSG);

#ifdef SV_DEBUG
	// ウォッチドッグタイマによるリセットの場合
	if (isWDTTO()) {
		putrsUSART((far rom char *)"WDT RESET!\x0D");
	}
#endif

// サーボ機能の処理ループ

	while (1) {
		unsigned char cmd;

		char str[10];

		ClrWdt ();				// ウォッチドッグタイマをクリア

		if (mem_chk_0 != 0xaa) Reset();		// メモリ異常があった場合，リセットする。
		if (mem_chk_1 != 0x55) Reset();

		if (FlagBits.update_flag) {

#ifdef SV_DEBUG
PORTBbits.RB0 = 1;
#endif
			contol_servo ();		// PID制御

#ifdef SV_DEBUG
PORTBbits.RB0 = 0;
#endif
			if (FlagBits.logout_flag) {		// ログの出力

				itoa(mot_pos, str);	// モータのポテンショメータの値を出力
				putsUSART(str);

	 			while (BusyUSART());
				WriteUSART(',');
				
				itoa(set_pos, str);	// ポテンショメータの指定値を出力
				putsUSART(str);

/* Vnの値を出力すると，制御間隔を超えてしまうので，無効にしておく。
	 			while (BusyUSART());
				WriteUSART(',');

				ltoa(Vn, str);		// Vnの値を出力
				putsUSART(str);
*/
				usart_cr ();

				if (log_count) {
					log_count --;
					if (log_count == 0) {
						FlagBits.logout_flag = 0;
						log_count = log_count_num;
					}
				}
			}

			FlagBits.update_flag = 0;
		}

		// 入力データ処理
		if (head_a != head_b) Reset();		// メモリ異常があった場合，リセットする。

		if ((tail_a != head_a) && (tail_b != head_b)) {

			cmd = rque [head_a ++];			// 受信バッファより入力
			if (head_a == LEN_RQUE) head_a = 0;
			head_b = head_a;

			if (cmd_mode == NO_PARM) {
				switch (cmd) {
				case 'h':		// ヘルプの表示
					usart_cr ();
					disp_eeprom_str(EEPROM_ID_MSG);
					disp_eeprom_str(EEPROM_HELP_MSG);

					break;

				case 'k':		// Kp の設定 k[bias倍した整数値]
					cmd_mode = KP_PARM;
					goto num_setting;

				case 'i':		// Ti の設定 i[bias倍した整数値]
					cmd_mode = TI_PARM;
					goto num_setting;

				case 'd':		// Td の設定 d[bias倍した整数値]
					cmd_mode = TD_PARM;
					goto num_setting;

				case 'A':		// ポテンショメータの指示値の設定  A[0〜MAX_POTENSIONの値]
								// 設定後，ログ開始
					cmd_mode = POS_PARM_LOG;
					goto num_setting;

				case 'a':		// ポテンショメータの指示値の設定  a[0〜MAX_POTENSIONの値]
								// コンピュータ制御の時のみ有効

					cmd_mode = POS_PARM;
					goto num_setting;

				case 'l':		// ログの出力個数の設定 l[0〜MAX_LOGLEN]
					cmd_mode = LOGLEN_PARM;

num_setting:
					input_num = 0;
					FlagBits.input_num_sign = 0;
					while (BusyUSART());
					WriteUSART(cmd);			// エコーバック
					break;

				case 'p':		// PIDパラメータの出力
output_PID_parm:
					putrsUSART((far rom char *)"Kp:");
					ltoa((long)(Kp), str);
					putsUSART(str);

					putrsUSART((far rom char *)" Ti:");
					ltoa((long)(Ti * bias), str);
					putsUSART(str);

					putrsUSART((far rom char *)" Td:");
					ltoa((long)(Td * bias), str);
					putsUSART(str);

/*
					putrsUSART((far rom char *)" Ki:");
					ltoa((long)(Ki), str);
					putsUSART(str);

					putrsUSART((far rom char *)" Kd:");
					ltoa((long)(Kd), str);
					putsUSART(str);
*/
					usart_cr ();

					break;

				case 'r':		// EEPROMからPIDパラメータを読み込む
					putrsUSART((far rom char *)"Rd parm\x0D");
					read_PID_parm();
					reset_pid_err ();
					goto output_PID_parm;

				case 'w':		// PIDパラメータをEEPROMに書き込む
					putrsUSART((far rom char *)"Wt parm\x0D");
					write_PID_parm();
					read_PID_parm();
					goto output_PID_parm;

				case 'v':		// 指示値，モータのポテンショメータの値を出力
/*
					     if (ctl_mode == VOL_CONTROL) putrsUSART((far rom char *)"Vol");
					else if (ctl_mode == COM_CONTROL) putrsUSART((far rom char *)"Com");
*/
					putrsUSART((far rom char *)"Set:");
					itoa(set_pos, str);
					putsUSART(str);

					putrsUSART((far rom char *)" Pos:");
					itoa(mot_pos, str);
					putsUSART(str);

					putrsUSART((far rom char *)" Vn:");
					ltoa(Vn, str);
					putsUSART(str);

					putrsUSART((far rom char *)" dVn:");
					ltoa(delta_Vn, str);
					putsUSART(str);

					usart_cr ();

					break;

				case 'z':		// ログの開始
					FlagBits.logout_flag = 1;
					log_count = log_count_num;
					break;

				case 'x':		// ログの終了
					FlagBits.logout_flag = 0;
					break;

				case 'b':		// 基板上のポテンショメータからの指示によるサーボ・モード開始
					ctl_mode = VOL_CONTROL;
					putrsUSART((far rom char *)"Vol\x0D");
					goto mode_change;


				case 'c':		// コンピュータからの指示によるサーボ・モード開始
					ctl_mode = COM_CONTROL;
					putrsUSART((far rom char *)"Com\x0D");
mode_change:
					reset_pid_err ();
					break;

				default:
//					while (BusyUSART());
//					WriteUSART(cmd);			// エコーバック
					break;
				}

			} else {

				while (BusyUSART());
				WriteUSART(cmd);			// エコーバック

				if (cmd == 0x0d) {
					if (FlagBits.input_num_sign) {
						input_num = -input_num;
					}

					switch (cmd_mode) {
					case KP_PARM:		// Kp の設定
						Kp = (float)input_num;		// bias倍になっている
						goto End_PID_parm_set;

					case TI_PARM:		// Ti の設定
						Ti = (float)input_num / bias;
						set_Ki ();		// TiからKiを計算
						goto End_PID_parm_set;

					case TD_PARM:		// Td の設定
						Td = (float)input_num / bias;
						set_Kd ();		// TdからKdを計算
End_PID_parm_set:
						reset_pid_err ();
						break;

					case POS_PARM_LOG:		// ポテンショメータの設定とログ開始
					case POS_PARM:			// ポテンショメータの設定
						if (ctl_mode == COM_CONTROL) {
							if (0 <= input_num && input_num <= MAX_POTENSION) {
								if (cmd_mode == POS_PARM_LOG) {
									FlagBits.logout_flag = 1;
									log_count = log_count_num;
								}
								set_pos = (int)input_num;
							}
						}
						break;

					case LOGLEN_PARM:		// ログの出力個数の設定
						if (0 <= input_num && input_num <= MAX_LOGLEN) {
							log_count_num = (unsigned int)input_num;
						}
						break;

					}
					cmd_mode = NO_PARM;

				} else if (cmd == '-') {
					FlagBits.input_num_sign = 1;				// 10進数文字列の符号をマイナスに設定

				} else if ('0' <= cmd && cmd <= '9') {
					input_num = input_num * 10 + (cmd - '0');	// 10進数文字列を数値に変換

				} else if (cmd == 0x08 || cmd == 0x7f) {	// BS, DELはキャンセル
					putrsUSART((far rom char *)"Cancel\x0D");
					cmd_mode = NO_PARM;
				}
			}
		}
	}

}

//
// PID制御関数
//
void contol_servo (void)
{
	int e2;			// 前々回の偏差
	int motor_ctl;	// モータへの制御量(PWMのデューティ)

/*
	if (PORTBbits.RB5 == 0) {	// JP4がショートのとき
		RELAY_A = 0;			// モータを停止
		RELAY_B = 0;
		read_PID_parm();
		return;
	}
*/
/*
	if (ctl_mode == NO_CONTROL) {	// モータが制御不能のとき，PID制御を無効にする
		RELAY_A = 0;				// モータを停止
		RELAY_B = 0;
		return;
	}
*/
	if (PORTBbits.RB5 == 0    || 		// JP4がショートのとき，または，制御モードが無効のとき
	    ctl_mode == NO_CONTROL  ) {		// PID制御をせずに，
		RELAY_A = 0;					// モータを停止
		RELAY_B = 0;
		return;
	}


	SetChanADC(ADC_CH1);			// モータのポテンショメータのチャンネルを選択
	Delay10TCYx(20);				// 0.1μ秒×10×20 = 20μ秒だけ待つ
	ConvertADC();
	while(BusyADC());
	mot_pos = ReadADC();			// モータのポテンショメータの値を測定

	if (ctl_mode == VOL_CONTROL) {	// 基板上のポテンショメータによる指示の場合
		SetChanADC(ADC_CH0);		// 基板上のポテンショメータのチャンネルを選択
		Delay10TCYx(20);			// 0.1μ秒×10×20 = 20μ秒だけ待つ
		ConvertADC();
		while(BusyADC());
		set_pos = ReadADC();		// 基板上のポテンショメータの値を測定

		if (set_pos <= POTENSION_LIMIT) {	// 基板上のポテンショメータに制限を加える。
			set_pos = POTENSION_LIMIT;
		} else if (set_pos >= MAX_POTENSION - POTENSION_LIMIT) {
			set_pos = MAX_POTENSION - POTENSION_LIMIT;
		}

	}

	e2 = e1;				// 偏差をシフト
	e1 = e0;
	e0 = mot_pos - set_pos;	// 今回の偏差

	delta_Vn = 
	           (long)(Kp * (float)(e0 - e1))			// Pに関連する制御量の変化分
	         + (long)(Ki * (float) e0)					// Iに関連する制御量の変化分
	         + (long)(Kd * (float)(e0 - e1 - e1 + e2));	// Dに関連する制御量の変化分
	Vn += delta_Vn;

	if (Vn >= 0) {
		RELAY_A = 0;	// モータの回転方向を設定
		RELAY_B = 1;
	
		if (Vn >=  MAX_ABS_VN) Vn = MAX_ABS_VN;
		motor_ctl =  Vn / BIAS_INT;	// モータの制御量に変換

		if (mot_pos <= POTENSION_LIMIT) {	// モータのポテンショメータが最	小の場合，それ以上回転できないので，モータを停止する。
			motor_ctl = 0;
		}

	} else {
		RELAY_A = 1;	// モータの回転方向を設定
		RELAY_B = 0;

		if (Vn <= -MAX_ABS_VN) Vn = -MAX_ABS_VN;
		motor_ctl = -Vn / BIAS_INT;	// モータの制御量に変換

		if (mot_pos >= MAX_POTENSION - POTENSION_LIMIT) {	// モータのポテンショメータが最大の場合，それ以上回転できないので，モータを停止する。
			motor_ctl = 0;
		}

	}

	// モータの制御が利かなくなった場合，強制的にリセットする
	if (motor_ctl >= DUTY_LIMIT) {
		motor_ctl = DUTY_LIMIT;

		max_ctr_count ++;
		if (max_ctr_count >= MAX_CTL_COUNT_LIMIT) {
			ctl_mode = NO_CONTROL;
			max_ctr_count = 0;
			putrsUSART((far rom char *)"No ctrl!\x0D");
			motor_ctl = 0;
			RELAY_A = 0;			// モータを停止
			RELAY_B = 0;
			reset_pid_err();

		}
	} else {
		max_ctr_count = 0;
	}

	SetDCPWM1(motor_ctl);		// PWMのデューティを設定
}

// TiからKiを計算
void set_Ki (void)
{
	if (Ti >= TOO_SMALL_TI) {
		Ki = Kp * DELTA_T / Ti;
	} else {
		Ki = 0.0;	// 積分時間が無限大の場合
	}
}

// TdからKdを計算
void set_Kd (void)
{
	Kd = Kp * Td / DELTA_T;
}

// PID制御用偏差と積算値をリセット
void reset_pid_err(void)
{
	e1 = e0 = 0;
	Vn = 0;
}

//
// USARTに改行コードを出力する関数
//
void usart_cr (void)
{
//	while (BusyUSART());
//	WriteUSART(0x0a);
	while (BusyUSART());
	WriteUSART(0x0d);
}

//
// EEPROMからデータを読み込む関数
//
unsigned char read_onchip_eeprom (
	unsigned char address)
{
	EECON1bits.EEPGD = 0;		// Select EEPROM data memory
	EECON1bits.CFGS  = 0;		//

	EEADR            = address;	// Setup address
	EECON1bits.RD    = 1;		// Start reading data in EEPROM
								// No requirement for waiting
	return EEDATA;
}

//
// EEPROMにデータを書き込む関数
//
void write_onchip_eeprom (
	unsigned char address,
	unsigned char data)
{
	EECON1bits.EEPGD = 0;		// Select EEPROM data memory
	EECON1bits.CFGS  = 0;

	EECON1bits.WREN  = 1;		// Enable write to EEPROM
	EEADR            = address;	// Setup address
	EEDATA           = data;	// Setup data

								// Required sequence
	EECON2           = 0x55;	// #1
	EECON2           = 0xaa;	// #2
	EECON1bits.WR    = 1;		// #3 (Actual write)

	while (EECON1bits.WR);		// Wait until finished
	EECON1bits.WREN = 0;		// Disable write to EEPROM
}

float read_float_eeprom (
	unsigned char address)
{
	int i;
	float x;
	unsigned char * ptr = (unsigned char *)(&x);
	for (i = 0; i < sizeof(float); i ++) {
		* ptr ++ = read_onchip_eeprom (address ++);
	}
	return x;
}

void write_float_eeprom (
	unsigned char address,
	float x)
{
	int i;
	unsigned char * ptr =  (unsigned char *)(&x);

	for (i = 0; i < sizeof(float); i ++) {
		write_onchip_eeprom (address ++, * ptr ++);
	}
}

void read_PID_parm(void)
{
	Kp = read_float_eeprom (EEPROM_KP);
	Ti = read_float_eeprom (EEPROM_TI);
	Td = read_float_eeprom (EEPROM_TD);
}

void write_PID_parm(void)
{
	write_onchip_eeprom (EEPROM_MAGIC_ADR0, MAGIC0);
	write_onchip_eeprom (EEPROM_MAGIC_ADR1, MAGIC1);

	write_float_eeprom (EEPROM_KP, Kp);
	write_float_eeprom (EEPROM_TI, Ti);
	write_float_eeprom (EEPROM_TD, Td);
}

// EEPROM内の文字列を出力
void disp_eeprom_str(
	unsigned char adr)
{
	unsigned char ch;

	while ((ch = read_onchip_eeprom(adr ++))) {
		while (BusyUSART());
		WriteUSART(ch);
	}
}