#include "machine.h" void init(void); void outbyte(unsigned long , unsigned char ); void outword(unsigned long , unsigned int ); void interrupt_init(void); void speed( int , int ); void handle( int angle ); void moter_mode( int mode_l, int mode_r ); int sensor(void); void crank(void); unsigned char *p6,*p7,*pa,*pb; unsigned int timer1; /*-------------------- モータ動作設定-------------------------------*/ #define FREE 1 /* モータモード フリー */ #define BREAK 0 /* モータモード ブレーキ */ void moter_mode( int mode_l, int mode_r ) { if( mode_l !=0 ) {*pa |= 0x08;} /* FREE 左モータ */ else {*pa &= 0xf7;} /* BREAK 左モータ */ if( mode_r !=0 ) {*pa |= 0x20;} /* FREE 右モータ */ else {*pa &= 0xdf;} /* BREAK 右モータ */ } /*--------------------センサ読み込み---------------------------------*/ unsigned char sence; int sensor() { int i_sence; sence=*p7; *pb=sence; if((sence & 0x7c) == 0x7c){i_sence=30;goto s_end;}/* ○●●●●●●○ */ if((sence & 0x18) == 0x18){i_sence= 0;goto s_end;}/* ○○○●●○○○ */ if((sence & 0x10) == 0x10){i_sence= 1;goto s_end;}/* ○○○●○○○○ */ if((sence & 0x08) == 0x08){i_sence=-1;goto s_end;}/* ○○○○●○○○ */ if((sence & 0x20) == 0x20){i_sence= 2;goto s_end;}/* ○○●○○○○○ */ if((sence & 0x04) == 0x04){i_sence=-2;goto s_end;}/* ○○○○○●○○ */ if((sence & 0x40) == 0x40){i_sence= 3;goto s_end;}/* ○●○○○○○○ */ if((sence & 0x02) == 0x02){i_sence=-3;goto s_end;}/* ○○○○○○●○ */ if((sence & 0x80) == 0x80){i_sence= 4;goto s_end;}/* ●○○○○○○○ */ if((sence & 0x01) == 0x01){i_sence=-4;goto s_end;}/* ○○○○○○○● */ s_end:; return(i_sence); } /*--------------------メインプログラム---------------------------------*/ float sp,han; void main(void) { unsigned int c; init(); interrupt_init(); sp=(~*p6 & 0x0f)*6.6*7031; /* ディップスイッチからスピード係数計算 */ han=26; /* サーボ1度あたりのPWMカウント値 */ while(1) { switch( sensor() ) { case 0:/* センター→まっすぐ */ handle( 0*han );moter_mode( FREE, FREE );speed(100*sp,100*sp);break; case 1:/* 微妙に左寄り→右へ微曲げ */ handle( 5*han );moter_mode( FREE, FREE );speed(100*sp,100*sp);break; case 2:/* 少し左寄り→右へ小曲げ */ handle( 15*han );moter_mode( FREE, FREE );speed( 80*sp, 80*sp);break; case 3:/* 中くらい左寄り→右へ中曲げ */ handle( 30*han );moter_mode(BREAK, FREE );speed( 50*sp, 45*sp);break; case 4:/* 大きく左寄り→右へ大曲げ */ handle( 35*han );moter_mode(BREAK, FREE );speed( 30*sp, 15*sp);break; case -1:/* 微妙に右寄り→左へ微曲げ */ handle( -5*han );moter_mode( FREE, FREE );speed(100*sp,100*sp);break; case -2:/* 少し右寄り→左へ小曲げ */ handle(-15*han );moter_mode( FREE, FREE );speed( 80*sp, 80*sp);break; case -3:/* 中くらい右寄り→左へ中曲げ */ handle(-30*han );moter_mode( FREE,BREAK );speed( 45*sp, 50*sp); break; case -4:/* 大きく右寄り→左へ大曲げ */ handle(-35*han );moter_mode( FREE,BREAK );speed( 15*sp, 30*sp);break; case 30:/* クランクライン通過 */ crank();break; default: break; } } } /* ----------------------クランク処理----------------------------------------------- */ void crank(void) { } #pragma interrupt(interrupt_1) void interrupt_1(void) { timer1++; (*(unsigned char*)0xfff85) &= 0xf8; } void interrupt_init(void) { /* 全てのタイマーを通常動作に設定-----------------------------------*/ outbyte(0x0FFF62,0x07); /* ITU0,1,2 PWMモード*/ /* その他 通常動作に設定*/ outbyte(0x0FFF60,0xe0); /* 設定のためタイマー停止 */ /* 1/1000タイマ−設定---------(1ms)--------------------------------*/ outbyte(0x0FFF82,0xa3); /* TU3_TCR タイマ3コントロールレジスタ 1/8 設定 */ outword(0x0FFF88,3072); /* 周期設定 1843*1.66・・・=3072 ********* */ outbyte(0x0FFF84,0x01); /* ITU3の割り込み許可 */ /* モータPWMモード設定 ITU0-----------(1ms周期)----------------------------*/ outbyte(0x0FFF64,0x23); /* ITU0_TCR タイマ0コントロールレジスタ 1/8 設定 */ outword(0x0FFF6A,3072); /* ITU0_GRA PWM周期設定 */ outword(0x0FFF6C,3071); /* ITU0_GRB デューティ設定 とりあえず全力 */ /* モータPWMモード設定 ITU1-----------(1ms周期)----------------------------*/ outbyte(0x0FFF6E,0x23); /* ITU1_TCR タイマ0コントロールレジスタ 1/8 設定 */ outword(0x0FFF74,3072); /* ITU1_GRA PWM周期設定 */ outword(0x0FFF76,3071); /* ITU1_GRB デューティ設定 とりあえず全力 */ /* サーボ設定 ITU2-----------(16ms周期)----------------------------*/ outbyte(0x0FFF78,0x23); /* ITU2_TCR タイマ0コントロールレジスタ 1/8 設定 */ outword(0x0FFF7E,49151); /* ITU2_GRA PWM周期設定 */ outword(0x0FFF80,5000); /* ITU2_GRB デューティ設定 とりあえず中央 */ /* 各種割り込み設定終了---------------------------------------------*/ outbyte(0x0FFF60,0xff); /* タイマースタート */ set_ccr( 0x00 ); /* 全体割り込み許可 */ } /* 指定アドレスに1バイト出力するための関数 */ void outbyte(unsigned long addr, unsigned char data) {unsigned char *p;p = (unsigned char *)addr;*p = data;} void outword(unsigned long addr, unsigned int data) {unsigned int *p;p = (unsigned int *)addr;*p = data;} void init(void) { outbyte(0x0fffc9, 0x00); /* P6DDR ポート6を入力に設定 ディプスイッチ下位*/ outbyte(0x0fffd1, 0xff); /* PADDR ポートAの出力設定 */ outbyte(0x0fffd4, 0xff); /* PBDDR ポートBを出力に設定 */ p6 = (unsigned char *)0x0fffcb; /* P6DR ポート6のアドレス指定*/ p7 = (unsigned char *)0x0fffce; /* P7DR ポート7のアドレス指定(入力専用)*/ pa = (unsigned char *)0x0fffd3; /* PADR ポートAのアドレス指定*/ pb = (unsigned char *)0x0fffd6; /* PADR ポートBのアドレス指定*/ } #define ITU0_CNT (*(unsigned int *)0xfff68) #define ITU0_TCR (*(unsigned char*)0xfff64) #define ITU0_GRA (*(unsigned int *)0xfff6a) #define ITU0_GRB (*(unsigned int *)0xfff6c) #define ITU0_IER (*(unsigned char*)0xfff66) #define ITU0_TSR (*(unsigned char*)0xfff67) #define ITU1_CNT (*(unsigned int *)0xfff72) #define ITU1_TCR (*(unsigned char*)0xfff6e) #define ITU1_GRA (*(unsigned int *)0xfff74) #define ITU1_GRB (*(unsigned int *)0xfff76) #define ITU1_IER (*(unsigned char*)0xfff70) #define ITU1_TSR (*(unsigned char*)0xfff71) void speed( int accele_l, int accele_r ) { unsigned char sw_data; /* 左モータ */ /* GRBがCNTより10以上小さい値かどうかのチェック */ if( ITU0_GRB > 10 ) { /* GRBが10以上なら 単純に比較 */ while( (ITU0_CNT >= ITU0_GRB-10) && (ITU0_CNT <= ITU0_GRB) ); } else { /* GRBが10以下なら 上限値からの値も参照する */ while( (ITU0_CNT >= ITU0_GRA-10) || (ITU0_CNT <= ITU0_GRB) ); } ITU0_TCR = 0x23; ITU0_GRB = accele_l; /* 右モータ */ /* GRBがCNTより10以上小さい値かどうかのチェック */ if( ITU1_GRB > 10 ) { /* GRBが10以上なら 単純に比較 */ while( (ITU1_CNT >= ITU1_GRB-10) && (ITU1_CNT <= ITU1_GRB) ); } else { /* GRBが10以下なら 上限値からの値も参照する */ while( (ITU1_CNT >= ITU1_GRA-10) || (ITU1_CNT <= ITU1_GRB) ); } ITU1_TCR = 0x23; ITU1_GRB = accele_r; } #define ITU2_CNT (*(unsigned int *)0xfff7c) #define ITU2_TCR (*(unsigned char*)0xfff78) #define ITU2_GRA (*(unsigned int *)0xfff7e) #define ITU2_GRB (*(unsigned int *)0xfff80) #define ITU2_IER (*(unsigned char*)0xfff7a) #define ITU2_TSR (*(unsigned char*)0xfff7b) void handle( int angle ) { while( (ITU2_CNT >= ITU2_GRB-10) && (ITU2_CNT <= ITU2_GRB) ); ITU2_GRB = 5000 - angle; }