2009062804145579d

YPVSもコントロールする為の回路図
無題

昼飯食いながら考える用エントリー


 以下思い出し用YPVSコントローラソース [コードを表示する]
/** -----------------------------------------------------------------
 YPVS12F
 build:   2012/12/23 version 1.0
          2012/12/28 version 1.1 NR
 author:  Rilassaru
 version: 1.0
------------------------------------------------------------------ */

#include <htc.h>
#include <string.h>
#define _XTAL_FREQ 8000000 // 8MHz
#define LED GP1
#define MC1 GP4
#define MC2 GP5
#define MODE GP3

#define TRUE 1
#define FALSE 0

#define TIMEOUT_COUNT 7		// この回数タイムアウトしたらエンジン停止

#define INSENSITIVE_RANGE 6 // 不感症領域
#define WIDE_RANGE_RATIO 3  // 特別な不感症領域の倍率
#define BRAKE_TIME 120      // ブレーキを掛ける時間(x 22us)
#define DRIVE_TIME 1        // 立ち上がりの最小時間(x 22us)
#define MT_STOP_TIME 90	     // TA8428Kは切り替えにSTOP時間100us必要
#define NR_DATA_NUM 10          // NoiseReduction用データ配列数(3以上必要)
#define NR_TRIGGER 80       // 電気的ノイズと判断する領域(実測で決める)

#define FULL_CLOSE_TARGET(x) (gPotTable[x][0])
#define FULL_OPEN_TARGET(x) (gPotTable[x][10])

__CONFIG(
	FOSC_INTOSCIO &	// 発振機 内臓利用
	WDTE_OFF &		// ウォッチドッグ 無効
	PWRTE_ON &		// パワーアップタイマー 有効
	BOREN_ON &		// ブラウンアウト検出 有効
	MCLRE_OFF &		// ~MCLR 無効
	CP_OFF &			// コードプロテクション 無効
	IESO_OFF &		// IEスイッチオーバー 無効
	FCMEN_OFF			// フェイルセーフクロック 無効
	);

// ポテンションメータテーブル
const unsigned int gPotTable[2][11] = {
//0    1    2    3    4    5    6    7    8    9   10(Krpm)
//-- 2013/03/23 腹黒号
190, 190, 190, 190, 190, 190, 274, 357, 446, 500, 500,
170, 170, 170, 170, 170, 180, 230, 340, 500, 500, 500
//-- 2013/02/24 ベルーガ号
//180, 180, 180, 180, 180, 190, 274, 347, 436, 490, 490,
//160, 160, 160, 160, 160, 170, 220, 330, 490, 490, 490
//-- 2013/01/02
//190, 190, 190, 190, 190, 190, 274, 357, 446, 500, 500,
//170, 170, 170, 170, 170, 180, 230, 340, 500, 500, 500
//-- 2012/12/23
//190, 190, 190, 190, 190, 190, 274, 357, 446, 500, 500,
//170, 170, 170, 170, 170, 180, 270, 360, 500, 500, 500
};


// Engine state
typedef enum{
	EG_STARTED,		// EG_STARTED
	EG_INIT_OPEN1,	// 電源オン時の最初の全開動作
	EG_INIT_CLOSE,	// その次の全閉
	EG_INIT_OPEN2,	// その次の全開
	EG_INITIALIZED,	// 全開のままエンジン起動待ち
}EG_STATE;

// Moter state
/*
IN1 IN2 OUTA OUTB  
H   H   L    L  BRAKE
L   H   L    H  逆転
H   L   H    L  正転
L   L   OFF     (ハイインピーダンス)  ストップ
*/
typedef enum{
	MT_BRAKE,
	MT_OPEN,
	MT_CLOSE,
	MT_STOP
}MT_STATE;

MT_STATE mt_state[3] = {MT_STOP};

#define MTBrake() {MC1=1;MC2=1;}
#define MTOpen()  {MC1=0;MC2=1;}
#define MTClose() {MC1=1;MC2=0;}
#define MTStop()  {MC1=0;MC2=0;}

EG_STATE gEGState = EG_STARTED;
unsigned int gRPM = 0;
unsigned int gPot = 0;

int MoterHead(unsigned int pot, unsigned int rpm, unsigned char mode);
unsigned int PotNoiseReduction(unsigned int pot);

/** -----------------------------------------------------------------
          1|VDD    PIC12F687    VSS|8
  OPEN <- 2|GP5(MC2)        GP0/AN0|7 <-Potension meter Analog IN
  CLOSE<- 3|GP4(MC1)        GP1/AN1|6 ->LED
  MODE -> 4|GP3/~MCLR  GP2/AN2/CCP1|5 <-PULSE
------------------------------------------------------------------ */

/** -----------------------------------------------------------------
 void main(void)
 メイン関数
------------------------------------------------------------------ */
void main(void)
{
	OPTION_REG = 0b10000010;	// PUしない
	WPU        = 0b00000000;	// PUしない
	OSCCON     = 0b01110000;	// 8MHz
	TRISIO     = 0b00001101;	// IN: GP0/GP2/GP3/ OUT:GP1/GP4/GP5
	ANSEL      = 0b01010001;	// AN0選択、Tad(Fosc/16)=2.0us
	CMCON0     = 0x7;			// コンパレータ無効
	ADCON0     = 0b10000001;	// 右詰、VDD、AN0利用

	int ii;
	gEGState = EG_INIT_OPEN1;

	MTStop();	// モータ初期値
	// スタートアップデモ
	for( ii = 0; ii < 10; ii++ ) {
		__delay_ms(100);
		LED = !LED;
	}
	LED = 0;

	// Input capture初期化(*1)
	CCP1CON = 0;	// 一度リセットする必要がある
	CCP1CON      = 0b00000110;	// every 4th rising edge
	CCP1IF = 0;	// 設定後は必ずIFをリセット

	// タイマー初期化
	T1CON  = 0b00110100;		// T1CKPS 1:8
	TMR1IF = 0;
	TMR1L  = 0;
	TMR1H  = 0;
	
	// 割り込み初期化
	GIE = 1;
	PEIE = 1;
	TMR1IE = 1;		// Timer1 intterupt on
	TMR1ON = 1;		// Timer1 start
	CCP1IE = 1;		// Capture interrupt on

	// NR配列の初期化
	for(ii = 0; ii < NR_DATA_NUM; ii++) {
		GO_DONE = 1;
		while(GO_DONE);
		gPot = PotNoiseReduction((ADRESH * 256 + ADRESL)+1);	// Analog input
	}
	
	// メインループ
	while(1) {
		//LED = MODE;		// モード表示
		
		GO_DONE = 1;		// 変換開始
		while(GO_DONE);	// 変換待ち Tad*11=22us
		gPot = PotNoiseReduction((ADRESH * 256 + ADRESL)+1);	// Analog input

		if(MoterHead(gPot, gRPM, MODE)) {
			__delay_us(MT_STOP_TIME);
		}	
	}
}

/** -----------------------------------------------------------------
  unsigned int PotNoiseReduction(unsigned int pot)
  ポテンションメータのノイズ除去
  @param pot ポテンションメータ(1〜1,024)
  @return ノイズ除去後のポテンションデータ
------------------------------------------------------------------ */
unsigned int PotNoiseReduction(unsigned int pot)
{
	static unsigned int pots[NR_DATA_NUM] = {0};
	unsigned int ave;
	int ii;
	
	ave = pots[NR_DATA_NUM-1];
	for(ii = (NR_DATA_NUM-1); ii > 0; ii--) {
		pots[ii] = pots[ii-1];
		ave += pots[ii];
	}
	ave /= NR_DATA_NUM;
	pots[0] = pot;
	// ノイズ判定
	if((pot > ave+NR_TRIGGER) || (ave-NR_TRIGGER > pot)) {
		return ave;
	}
	return pot;
}

/** -----------------------------------------------------------------
  void MoterHead(unsigned int pot, unsigned int rpm)
  モータ制御
  @param pot ポテンションメータ(1〜1,024)
  @param rpm 回転数(0〜9,999)
  @oaram mode 0 or 1
  @return 100usの待ち時間が必要な場合 0以外
------------------------------------------------------------------ */
int MoterHead(unsigned int pot, unsigned int rpm, unsigned char mode)
{
	unsigned int target;
	static char brake_count = 0;
	static char open_count = 0;
	static char close_count = 0;
	int iRange = INSENSITIVE_RANGE;

	// パルスが入っているならどこからでもEG_STARTED
	if(rpm > 100) {
		gEGState = EG_STARTED;
	}


	// --------------------------------------------------------------
	// モード別のターゲット設定とモード遷移
	// --------------------------------------------------------------
	switch(gEGState) {
	case EG_STARTED: {
		unsigned int index;

		// アイドリング状態の時は過敏に反応しない
		if(rpm < 2000) {
			iRange = INSENSITIVE_RANGE * WIDE_RANGE_RATIO;
		}
		// 1万回転以上は使わない
		if(rpm > 10000) {
			rpm = 9999;
		}

		// ターゲット算出 (*2)
		index = rpm / 1000;
		if(gPotTable[mode][index+1] >= gPotTable[mode][index]) {
			target = gPotTable[mode][index]
				+ ((((gPotTable[mode][index+1] - gPotTable[mode][index]) * ((rpm%1000)/100)))/10);
		} else {
			target = gPotTable[mode][index]
				- ((((gPotTable[mode][index] - gPotTable[mode][index+1]) * ((rpm%1000)/100)))/10);
		}			 
		break;
	}	

	// 電源オン時の最初の全開動作
	case EG_INIT_OPEN1: {

		target = FULL_OPEN_TARGET(mode);
		if(pot >= FULL_OPEN_TARGET(mode) - (INSENSITIVE_RANGE * WIDE_RANGE_RATIO)) {
			gEGState = EG_INIT_CLOSE;
		}	
		break;
	}	
	
	// その次の全閉
	case EG_INIT_CLOSE: {

		target = FULL_CLOSE_TARGET(mode);
		if(pot <= FULL_CLOSE_TARGET(mode) + INSENSITIVE_RANGE) {
			gEGState = EG_INIT_OPEN2;
		}	
		break;
	}	
	
	// その次の全開
	case EG_INIT_OPEN2: {

		target = FULL_OPEN_TARGET(mode);
		if( pot >= FULL_OPEN_TARGET(mode) - INSENSITIVE_RANGE) {
			gEGState = EG_INITIALIZED;
		}	
		break;
	}
	
	// 全開のままエンジン起動待ち
	case EG_INITIALIZED: {

		target = FULL_OPEN_TARGET(mode);
		iRange = INSENSITIVE_RANGE * WIDE_RANGE_RATIO;
		break;
	}	

	default:
		break;
	}
	
	// --------------------------------------------------------------
	// ポテンションメータと比較して制御
	// --------------------------------------------------------------
	if((MT_OPEN == mt_state[0] && MT_OPEN == mt_state[1] && MT_OPEN == mt_state[2]) ||
		(MT_CLOSE == mt_state[0] && MT_CLOSE == mt_state[1] && MT_CLOSE == mt_state[2])) {
		iRange /= 2;
	}
	
	if(target > (pot+iRange)) {
		mt_state[0] = MT_OPEN;
	} else if(target < (pot-iRange)) {
		mt_state[0] = MT_CLOSE;
	} else {
		mt_state[0] = MT_STOP;
	}


	// --------------------------------------------------------------
	// BRAKE制御	
	// --------------------------------------------------------------
	if(MT_STOP == mt_state[0] && MT_STOP == mt_state[1]
			&& (MT_OPEN == mt_state[2] || MT_CLOSE == mt_state[2])) {
		brake_count = BRAKE_TIME;
	} else if(MT_OPEN == mt_state[0] && MT_STOP == mt_state[1]) {
		open_count = DRIVE_TIME;
	} else if(MT_CLOSE == mt_state[0] && MT_STOP == mt_state[1]) {
		close_count = DRIVE_TIME;
	}

	if(open_count > 0) {
		open_count--;
		mt_state[0] = MT_OPEN;
	} else if(close_count > 0) {
		close_count--;
		mt_state[0] = MT_CLOSE;
	} else if(brake_count > 0) {
		brake_count--;
		mt_state[0] = MT_BRAKE;
	}
	// --------------------------------------------------------------
	// モータ出力
	switch(mt_state[0]) {
		case MT_BRAKE: MTBrake();   break;
		case MT_OPEN:  MTOpen();    break;
		case MT_CLOSE: MTClose();   break;
		case MT_STOP:  MTStop();    break;
		default:       MTStop();    break;
	}
	mt_state[2] = mt_state[1];
	mt_state[1] = mt_state[0];

	if(mt_state[0] == MT_STOP && mt_state[1] != MT_STOP) {
		return 1;
	}	
	return 0;
	
}	

/** -----------------------------------------------------------------
  void interrupt ISR(void)
  割り込み制御関数
  Timer1、Captureを処理する
------------------------------------------------------------------ */

void interrupt ISR(void)
{
	static int timeout_count = 0;
	// Input capture 
	if(CCP1IF) {
		// タイマーリセット
		TMR1H=0;
		TMR1L=0;
		// (*1)
		gRPM = 60000000UL/(unsigned long)((CCPR1H*256)+CCPR1L);

		CCP1IF = 0;
	}	

	// Timer1 タイムアウト→エンジン停止と判断
	if(TMR1IF) {
		gRPM = 0;
		if(TIMEOUT_COUNT < timeout_count++) {
			if(EG_STARTED == gEGState) {
				gEGState = EG_INIT_OPEN1;
			}
			timeout_count = 0;
		}	
		TMR1IF = 0;
	}
}

/** -------------------------------------------------------------
// *1 Input capture 補足
-----------------------------------------------------------------
動作8Mhz、Fosc/4 でTimer最小値は (1/8000000)*4 = 0.0005(ms)
プリスケーラを1:8 とすると 0.0005(ms)*8 = 0.004(ms) ←分解能
最大値は 0.004(ms)*65535 = 262.14(ms)

1000rpm は60(ms)、10000rpm は6(ms)間隔のパルス
4回Edgeでカウントするとそれぞれ240(ms)/24(ms)

Capture割り込み時の回転数は 
 count/0.004(ms)*60000(ms)
色々省略して
 60,000,000/count で回転数が出る。

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

/** -------------------------------------------------------------
// *2 Target 計算 補足
-----------------------------------------------------------------
Targetを算出
(d) ... Table間の差分を100分する;
d = (gPotTable[index+1] - gPotTable[index])
(x) ... 1000桁以上を無くして100で割る
x = ((rpm%1000)/1000);
(y) ... 目標値
 = dx + b

target
  |  /|dy
  |_/_|
  |  dx
  +-+-+-+-+-+-...>rpm
  0 1 2 3 4 5...
-------------------------------------------------------------- */