/* speed.c 2007.05.10  by Masaki Shinomiya */

#include <sdcc-lib.h>
#include <pic12f683.h>
#include "asm.h"

/***	PORT#	In/Out	Name	***
	GP0	AIN	VR	ボリューム入力(電圧) 0-2.5V
	GP1	DIN	CMODE	制御モード
	GP2	DOUT	GATE	FET gate への出力
	GP3	DIN	ROT	回転数検知パルス入力
	GP4	DOUT	MONITOR	デバッグ用出力
	GP5	DOUT	MONITOR2
***********************************/
#define DEBUG		// デバッグ用コード有効

#ifdef GPIO1	/* GPIO bits が使えるとき */
#define CMODE	GPIO1
#define GATE	GPIO2
#define INPULSE	GPIO3
#define MONITOR	GPIO4
#define OUT_MONITOR_0 MONITOR = 0
#define OUT_MONITOR_1 MONITOR = 1
#define MONITOR2 GPIO5
#define OUT_MONITOR2_0 MONITOR2 = 0
#define OUT_MONITOR2_1 MONITOR2 = 1
#endif
#ifndef GPIO1	/* GPIO bits が使えないとき */
#define CMODE	GPIO & 0x02
#define GATE	GPIO & 0x04
#define INPULSE	GPIO & 0x08
#define MONITOR GPIO & 0x10
#define OUT_MONITOR_0 GPIO &= ~0x10
#define OUT_MONITOR_1 GPIO |= 0x10
#define OUT_MONITOR2_0 GPIO &= ~0x20
#define OUT_MONITOR2_1 GPIO |= 0x20
#endif

#define CPUFREQ	8 // MHz
#define POWFULL	200	// x約0.5us PWM出力の周期
#define POWMIN	80	// 出力最小値 (1-POWFULL)
#define ROTFULL 100	// 回転数指令の最大値
#define ROTMIN  5	// 回転数指令の最小値
#define MAXRPM	3400	// 100%時の回転数/分
#define PPROT	2	// エンコーダパルス数/1回転
#define PMULT	4	// エンコーダパルスに掛けられる乗数
#define LOCKED	4	//x約0.22秒 ロック保護が働くまでのウェイト
#define RETRY	LOCKED*10 //x約0.22秒 ロック保護後の再開までのウェイト(1-255)
/* STEP=FULL*60*CPUFREQ*1000*1000/(8*1024*MAXRPM*PPROT*PMULT) */
#define INTERVAL 220	// x約1ms TMR1Hのカウント値 (1-255)
#define SMALL	20	// speedsumの許容偏差
#define STEP	4	// 出力デューティを1だけ動かす偏差
#define LARGE	ROTFULL + SMALL	// speedsumの最大値
#define SMOOTH	20	// x約0.1ms INPULSEの平滑度 (1-126)

static unsigned char rot, open_loop, ptime;
static unsigned char datum, rotold, vol;
static signed char rotsum;
static signed int speedsum, err, value;

/* configuration bits */
int at 0x2007 __config = _INTOSC & _WDT_ON & _PWRTE_ON & _MCLRE_OFF;

/*** 初期化 ***/
void po_reset() {
	/* CPUクロック */
	OSCCON = 0x71;	// 8MHz
	OSCTUNE = 0x00;
	/* Interupt */
	INTCON = 0x00;	// どの割り込みも使わない
	/* Timer0使わない */
	OPTION_REG = 0x00;
	TMR0 = 0;
	/* Timer1 */
	T1CON = 0x30;	// TMR1プリスケーラ 1/8
	TMR1L = 0x00;
	TMR1H = 0x00;
	TMR1IF = 0;
	TMR1ON = 1;	// TMR1スタート
	/* Timer2 PWMモード */
	T2CON = 0x00;	// TMR2プリスケーラなし
	CCP1CON = 0x0C;	//PWM mode active-high
	CCPR1L = 0;
	CCPR1H = 0;
	TMR2 = 0;
	PR2 = POWFULL;
	TMR2ON = 1;	// TMR2スタート
	/* GPIO */
	WPU = 0x37;	//  GP3を除きDIはすべてプルアップ
	CMCON0 = 0x07;	// コンパレータ使わない
	CMCON1 = 0x00;
	VRCON = 0x00;
	ANSEL = 0x51;	// Fosc/16, ANS0
	ADCON0 = 0x01;	// 変換結果上位8ビットを使う, AN0, ADON
	TRISIO = 0x0B;	// GP2,GP4,GP5は出力
	/* Interupt使わない  */
}

/*** ボリューム値の読み込み ***/
unsigned char volume() {
//    unsigned char datum;
    GO_DONE = 1;		// A/D変換開始
    while ( GO_DONE ){;}	// 変換終了を待つ
    datum = ADRESH;
    return datum;
}

/*** 回転数検知 ***/
unsigned char rotate() {
//    unsigned char rotold;
    rotold = rot;
    /* TMR2 == POWMIN に同期 */
    while ( TMR2 > POWMIN ) {;}
    while ( TMR2 < POWMIN ) {;}
    /* 回転検知パルスを積分 */
    if ( INPULSE ) {
	if ( ++rotsum >= SMOOTH ) {
		rotsum = SMOOTH;
		rot = 1;
	}
    } else {
	if ( --rotsum <= 0 ) {
		rotsum = 0;
		rot =0;
	}
    }
    return ( rotold && (rot == 0) ); // 立ち下がりエッジ
}

int main() {
//	unsigned char vol;
    /* I/Oの初期化 */
    po_reset();
    /* その他の初期化 */
    rotsum = 0;
    speedsum = 0;
    ptime = 0;
    /* 制御モードの取得 */
    open_loop = CMODE;
    CCPR1L = POWFULL;	// 最大値からスタート
    while (1) {
	ClrWdt();	// ウォッチドッグタイマーをクリア
	vol = volume();
	if ( rotate() ) {
		speedsum -= PMULT;
		if ( speedsum < -LARGE ) { speedsum = -LARGE; }
		ptime = 0;		// ロック保護タイマーのリセット
#ifdef DEBUG
		OUT_MONITOR_1;
	} else {
		OUT_MONITOR_0;
#endif
	}
	if ( TMR1H >= INTERVAL ) {
		TMR1H = 0;		//  タイマー1リセット
		datum = CCPR1L;
		if ( open_loop ) {	// オープンループ制御
			datum = vol + POWMIN;
			if ( datum > POWFULL ) { datum = POWFULL; }
		} else {
			value = vol;
			if ( value > ROTFULL ) { value = ROTFULL; }
			if ( value < ROTMIN ) { value = ROTMIN; }
			speedsum +=  value;
			if ( speedsum > LARGE ) { speedsum = LARGE; } 
			err = speedsum;
			if ( err > SMALL ) {
				err -= SMALL;
				while ( err > 0 ) {
					err -= STEP;
					++datum;	// 加速
					speedsum = 0;
				}
			} else if ( err < -SMALL) {
				err += SMALL;
				while ( err < 0 ) {
					err += STEP;
					--datum;	// 減速
					speedsum = 0;
				}
			}
			if ( datum > POWFULL ) { datum = POWFULL; }
			if ( datum < POWMIN ) { datum = POWMIN; }
			if ( ++ ptime > LOCKED ) {
				datum = 0;	// ロック保護
				if ( ptime > RETRY ) {
					while (1) { ; } // WDTタイムアップを待つ
				}
			}
		}
		CCPR1L = datum;
#ifdef DEBUG
		OUT_MONITOR2_1;
	} else {
		OUT_MONITOR2_0;
#endif
	}
    }
}