Ｈ８ｃｐｕの初期化と割り込みルーチン。姿勢制御ルーチンなど、マウスの走行を直接制御するためのルーチン。
非常に冗長で、無駄が多く、使い勝手の悪いプログラムだが、以下のとうり。
/* program "3052drive.c" */
/* ’０２ マイクロマウスプログラムのための file No_1 */
/* for 秋月Ｈ８／３０５２ boad 秋月版ＧＣＣ ＆ 秋月compiler 対応 */
/* lase edit '99/09/10 k.notoya */
#include "d:\H83048File\3048f.h" /* ＭＣＲ版 h8のレジスタを定義したヘッダファイル */
#include "d:\H83048File\machine.h" /* 割り込み設定 etc */
#include "d:\H83048File\stdio.h" /* Ｈ８ standerd I/O hedder */
/* #include "d:\H83048File\stdlib.h" */
#include "d:\standardDef.h" /* 標準的な定数data ＆ インターフェイス・制御系のマクロ */
#include "d:\constant.h" /* 定数data （走行速度、走行距離、姿勢制御量） */
#include "d:\global.h" /* 全モジュール共通 広域変数 */
/* #include "d:\mouselib.h" */ /* マイクロマウスのための関数のプロトタイプ宣言 */
void port_init(void) ; /* 入出力ポートの初期化 DDR_reg set 0 is 入力 set 1 is 出力 */
void itu_init(void) ; /* ＩＴＵ 初期化 */
void dac_init(void) ; /* D/A.C 初期化 */
void wdt_init(void) ; /* ウオッチドッグタイマ 初期化 */
void H8cpu_init(void) ; /* Ｈ８cpu 初期化 */
void interrupt_control(UCHAR sw) ; /* 割り込み制御 禁止 or 許可 */
USHORT adcread_Phot(UCHAR ChannelData) ; /* アナログ距離センサ data 読み込み */
void revision_control(UCHAR sw) ; /* センサ入力 ＆ 姿勢制御 制御 */
void aslant_revision_steer_PHOT(void) ; /* 斜め走行姿勢制御 ステアリングの補正 revision_steering */
void revision_steer_PHOT(void) ; /* 定常走行姿勢制御 ステアリングの補正 revision_steering */
/* 【注意】 割り込みﾙｰﾁﾝはプロトタイプ宣言をする必要が無い */
/* void interrupt_wdt_enter(void) ; */ /* ウオッチドッグタイマ 割り込み処理ルーチン */
void wdt_interrupt_control(UCHAR sw) ; /* ウオッチドッグタイマ 割り込み制御 */
/* void interrupt_ITU0_enter(void) ; */ /* ＩＴＵ channel_0 割り込み処理ルーチン */
/* void interrupt_ITU1_enter(void) ; */ /* ＩＴＵ channel_1 割り込み処理ルーチン */
/* void interrupt_ITU2_enter(void) ; */ /* ＩＴＵ channel_2 割り込み処理ルーチン */
/* void interrupt_ITU3_enter(void) ; */ /* ＩＴＵ channel_３ 割り込み処理ルーチン */
void reverse(void) ; /* 進行方向(旋回方向)と反対向きに励磁し、強制的に停止させる */
void adress_set(void) ; /* 変更された加速テーブルのadressを、初期値に戻す */
void timer_reset(void) ; /* slalom旋回時に変更されたtimerの分周比を、初期値に戻す */
void power(unsigned char sw) ; /* SLA7033 PWM基準電圧設定 */
UCHAR push_switch(void) ; /* push_switch のデータ入力 */
UCHAR dip_switch(void) ; /* dip_switch のデータ入力 */
void ifrray_off(void) ; /* 赤外LED 発光停止 */
void wall_data_indication(UCHAR WallData) ; /* 壁data indication LED */
UCHAR read_wall_data_indi(void) ; /* 壁data indication LED data読み込み */
void led(UCHAR data) ; /* interface LED 駆動 */
UCHAR read_led(void) ; /* interface LED data読み込み */
void led_blink(void) ; /* LEDを３回点滅させる */
void push_sw_with_led(void) ; /* LED を brinkしながら、push_switchの入力を待つ */
void push_sw_led_Left(void) ; /* 左壁横の最大値と右横壁の最小値を設定するシグナル */
void push_sw_led_Center(void) ; /* 左壁横と右横壁の中央値を設定するシグナル */
void push_sw_led_Right(void) ; /* 左壁横の最小値と右横壁の最大値を設定するシグナル */
void warning_sin(void) ; /* 迷路地図 クリア 警告シグナル cls_warning_sin */
void warning_sin2(void) ; /* 事故時 緊急シグナル led が非常停止したことを知らせる */
void data_read_start_sig(void) ; /* データ入力開始 シグナル */
void data_read_end_sig(void) ; /* データ入力終了 シグナル */
/*--------------- H8/3048 CPU mode_７ single_tip mode ---------------
【注意】 ポートの使用状況は、Ｍｏｄｅ５（内蔵ＲＡＭ有効 拡張メモリmode）と整合性をとってある
未使用のポートは原則として出力ポートに設定 ＆ 0x00 を出力する
【注意】 ＩＴＵが信号を取り込む兼用ポートは、ＩＴＵの初期化を行う前にＩ／Ｏポートの設定を行い、
入力ポートに設定しておくこと。 逆になるとＩＴＵは信号を読みこめない
port_1 未使用 8_bit の入出力端子 or adress 出力(Low)
port_2 interface LED 8個 8_bit の入出力端子 or adress 出力(High)
port_3 未使用 8_bit の入出力端子 or data bus(High)
port_4 下位 4ﾋﾞｯﾄ dip_switchi 入力 8_bit の入出力端子 or data bus(Low)
port_5 増設LED 4_bit の入出力端子 or adress 出力(High)
port_6 未使用 7_bit の入出力端子 or ﾊﾞｽ制御信号出力
port_7 ｱﾅﾛｸﾞﾎﾟｰﾄ
port_8 未使用 5_bit の入出力端子 or (ﾁｯﾌﾟｾﾚｸﾄ or 割り込み要求)
port_9 start_switch bit_5 6_bit の入出力端子 or RS232c
秋月boadは TxD1(bit_1) ＆ RxD1(bit_3)を使用
port_A 下位 ４bit 赤外線発光制御 8_bit の入出力端子 or timer入出力
port_B motor駆動信号 出力 8_bit の入出力端子 or timer入出力
左motor bit 7 <-- bit 4 右motor bit 3 <-- bit 0
-----------*/
/* 入出力ポートの初期化 mode_７ single_tip mode */
void port_init(void) /* 入出力ポートの初期化 DDR_reg set 0 is 入力 set 1 is 出力 */
{
P1.DDR= 0xff ; /* poat_1 未使用 */
P1.DR.BYTE= 0x00 ;
P2.DDR= 0xff ; /* poat_2 ｲﾝﾀｰﾌｪｲｽLED 8個 */
P2.DR.BYTE= 0xff ; /* all_LED cut_off */
P3.DDR= 0xff ; /* poat_3 未使用 */
P3.DR.BYTE= 0x00 ;
P4.DDR= 0xf0 ; /* poat_4 下位 ４bit dip_switchi 入力 */
P4.DR.BYTE= 0x00 ;
P5.DDR= 0xff ; /* poat_5 増設LED */
P5.DR.BYTE= 0xff ; /* 増設LED cut_off */
P6.DDR= 0x80 ; /* port_6 未使用 */
P6.DR.BYTE= 0x00 ;
P8.DDR= 0xff ; /* port_8 未使用 */
P8.DR.BYTE= 0x00 ;
P9.DDR= ( 0xff &(~0x20 ) ) ; /* poat_9 bit 5 start_switch */
P9.DR.BYTE= 0x00 ;
PA.DDR= 0xff ; /* poat_A 下位 ４bit 赤外LED制御 */
PA.DR.BYTE= 0x0f ; /* 赤外 led cut off */
PB.DDR= 0xff ; /* poat_B 励磁信号出力 */
/* PB.DR.BYTE= 0xc9 ; SLA7033は励磁信号が入ると電流が流れる */
PB.DR.BYTE= 0x00 ; /* SLA7033 電流cut */
}
/*--- ＩＴＵ 全体の仕様設定と初期化
ch_0 timer mode 仮想車輪
ch_1 timer mode 左motor(圧電Buzzer)
ch_2 timer mode 右motor
ch_3 timer mode センサ取り込み ＆ 姿勢制御
ch_4 未使用 ---*/
/*【注意】 ＩＴＵが信号を取り込む兼用ポートは、ＩＴＵの初期化を行う前に入力ポートに設定されていなければならない
ＩＴＵの初期化が終わってからポートを入力ポートに設定しても、ＩＴＵは信号を読み込むことができない */
/* ＩＴＵの初期化 */
void itu_init(void) /* ＩＴＵ 初期化 */
{
/* ＩＴＵ 初期化 ﾀｲﾏｽﾀｰﾄﾚｼﾞｽﾀ 設定 全ＩＴＵ 停止 */
ITU.TSTR.BYTE= ( (((((ITU.TSTR.BYTE & 0xc0) /* 111x-xxxx 初期値 cls */
/* bit7 to bit5 is reserve bit */
&(~0x10)) /* xxx1-xxxx ch_4 able or stop */
&(~0x08)) /* xxxx-1xxx ch_3 able or stop */
&(~0x04)) /* xxxx-x1xx ch_2 able or stop */
&(~0x02)) /* xxxx-xx1x ch_1 able or stop */
&(~0x01) ) ; /* xxxx-xxx1 ch_0 able or stop */
/*【注意】 TSNC ﾀｲﾏｼﾝｸﾛﾚｼﾞｽﾀは、ﾀｲﾏの独立動作／同期動作を選択する。 */
/* 今回は同期動作は必要無いので、初期値のままで変更する必要は無い */
/* TSNC ﾀｲﾏｼﾝｸﾛﾚｼﾞｽﾀの設定 設定しない（初期値で可） */
/* ITU.TSNC.BYTE= ( ITU.TSNC.BYTE & (~0x1f) ) ; */
/* TMDR ﾀｲﾏﾓｰﾄﾞﾚｼﾞｽﾀの設定 */
ITU.TMDR.BYTE= ( (((((((ITU.TMDR.BYTE & 0x80) /* 1xxx-xxxx 初期値 cls bit7 is reserve bit */
&(~0x40)) /* x1xx-xxxx ch_2 is 位相計数ﾓｰﾄﾞ */
&(~0x20)) /* xx1x-xxxx ch_2 ＯＶＦ flag reset */
&(~0x10)) /* xxx1-xxxx ch_4 is 通常動作 */
&(~0x08)) /* xxxx-1xxx ch_3 is 通常動作 */
&(~0x04)) /* xxxx-x1xx ch_2 is 通常動作 */
&(~0x02)) /* xxxx-xx1x ch_1 is 通常動作 */
&(~0x01) ) ; /* xxxx-xxx1 ch_0 is 通常動作 */
/*【注意】 TFCR ﾀｲﾏファンクションコントロールﾚｼﾞｽﾀは相補ＰＷＭ／ﾘｾｯﾄ同期ＰＷＭﾓｰﾄﾞの設定と、 */
/* バッファﾓｰﾄﾞの設定をする。 従って、今回は初期値のままで変更する必要は無い */
/* TFCR ﾀｲﾏファンクションコントロールﾚｼﾞｽﾀの設定 設定しない（初期値で可）*/
/* ITU channel_4 のﾊﾞｯﾌｧｰﾚｼﾞｽﾀの使い方を設定する */
/* ITU.TFCR.BYTE= ((((ITU.TFCR.BYTE & (~0x30)) */ /* xx11-xxxx channel_3,4 normal mode */
/* & (~0x0c)) */ /* xxxx-11xx channel_4 設定 cls */
/* | 0x04) */ /* xxxx-BAxx channel_4 GRA ﾊﾞｯﾌｧｰmode */
/* */ /* GRB normal mode */
/* & (~0x03)) ; */ /* xxxx-xxBA channel_3 GRA,GRB normal mode */
/*【注意】 TOER ﾀｲﾏｱｳﾄﾌﾟｯﾄﾏｽﾀｰｲﾈｰﾌﾞﾙﾚｼﾞｽﾀは、Port_B（ch3とch4の出力端子）を */
/* 出力ﾎﾟｰﾄとして使う場合の設定をする。 初期値はＩＴＵの端子として設定されているので注意 */
/* TOER ﾀｲﾏｱｳﾄﾌﾟｯﾄﾏｽﾀｰｲﾈｰﾌﾞﾙﾚｼﾞｽﾀの設定 */
ITU.TOER.BYTE= ( ((((((ITU.TOER.BYTE & 0xc0 ) /* 11xx-xxxx 初期値 cls bit7,bit6 is reserve bit*/
&(~0x20)) /* xx1x-xxxx TOCXB4 端子制御 ﾀｲﾏー優先 */
&(~0x10)) /* xxx1-xxxx TOCXA4 端子制御 ﾀｲﾏー優先 */
&(~0x08)) /* xxxx-1xxx TIOCB3 端子制御 ﾀｲﾏー優先 */
&(~0x04)) /* xxxx-x1xx TIOCB4 端子制御 ﾀｲﾏー優先 */
&(~0x02)) /* xxxx-xx1x TIOCA4 端子制御 ﾀｲﾏー優先 */
&(~0x01) ) ; /* xxxx-xxx1 TIOCA3 端子制御 ﾀｲﾏー優先 */
/* ITU channel_0 主割り込み */
/* 通常動作(timer mode) 内部クロック GRA ｺﾝﾍﾟｱﾏｯﾁ 割り込み有り 信号出力 無し */
ITU0.TCR.BYTE= ( (((ITU0.TCR.BYTE & 0x80) /* 1xxx-xxxx 初期値 cls bit7 is reserve bit */
| 0x20) /* xBAx-xxxx TCNTのクリア条件設定 GRA ｺﾝﾍﾟｱﾏｯﾁ */
&(~0x18)) /* xxx1-1xxx 外部クロックの入力エッジ制御 */
| 0x00) ; /* xxxx-x000 内部クロック Φ/1 count */
/* | 0x01) ; */ /* xxxx-x001 内部クロック Φ/２ count */
/* GRA,GRB の機能選択 ＆ ｺﾝﾍﾟｱﾏｯﾁ(comparematch) による端子出力の許可 */
ITU0.TIOR.BYTE= ( ((ITU0.TIOR.BYTE & 0x88) /* 1xxx-1xxx 初期値 cls bit7,bit4 is reserve bit */
&(~0x70)) /* x111-xxxx GRB is ｱｳﾄﾌﾟｯﾄ・ｺﾝﾍﾟｱ ＆ Ｂ端子 出力禁止 */
&(~0x07)) ; /* xxxx-x111 GRA is ｱｳﾄﾌﾟｯﾄ・ｺﾝﾍﾟｱ ＆ Ａ端子 出力禁止 */
/* ｺﾝﾍﾟｱﾏｯﾁ(comparematch) ＆ over flow による割り込みの許可 */
ITU0.TIER.BYTE= ( (((ITU0.TIER.BYTE & 0xf8) /* 1111-1xxx 初期値 cls bit7 to bit4 is reserve bit */
&(~0x04)) /* xxxx-x1xx over flow flag による割り込み禁止 */
&(~0x02)) /* xxxx-xx1x ｺﾝﾍﾟｱﾏｯﾁflag IMIB による割り込み禁止 */
| 0x01) ; /* xxxx-xxx1 ｺﾝﾍﾟｱﾏｯﾁflag IMIA による割り込み許可 */
/* 左ステッピングモータは、内部clockをcountし、割り込みでGRAを更新、poat_B 上位４bit から 駆動信号出力 */
/* ITU channel_1 通常動作(timer mode) 内部クロック入力 割り込み有り GRA ｺﾝﾍﾟｱﾏｯﾁ 出力無し */
ITU1.TCR.BYTE= ( (((ITU1.TCR.BYTE & 0x80) /* 1xxx-xxxx 初期値 cls bit7 is reserve bit */
| 0x20) /* xBAx-xxxx TCNTのクリア条件設定 GRA ｺﾝﾍﾟｱﾏｯﾁ */
&(~0x18)) /* xxx1-1xxx 外部クロックの入力エッジ制御 */
| 0x00) ; /* xxxx-x000 内部クロック Φ/1 count */
/* | 0x01) ; /* xxxx-x001 内部クロック Φ/２ count */
/* GRA,GRB の機能選択 ＆ ｺﾝﾍﾟｱﾏｯﾁ(comparematch) による端子出力の許可 */
ITU1.TIOR.BYTE= ( ((ITU1.TIOR.BYTE & 0x88) /* 1xxx-1xxx 初期値 cls bit7,bit4 is reserve bit */
&(~0x70)) /* x111-xxxx GRB is ｱｳﾄﾌﾟｯﾄ・ｺﾝﾍﾟｱ ＆ Ｂ端子 出力禁止 */
&(~0x07)) ; /* xxxx-x111 GRA is ｱｳﾄﾌﾟｯﾄ・ｺﾝﾍﾟｱ ＆ Ａ端子 出力禁止 */
/* ｺﾝﾍﾟｱﾏｯﾁ(comparematch) ＆ over flow による割り込みの許可 */
ITU1.TIER.BYTE= ( (((ITU1.TIER.BYTE & 0xf8) /* 1111-1xxx 初期値 cls bit7 to bit4 is reserve bit */
&(~0x04)) /* xxxx-x1xx over flow flag による割り込み許可 */
&(~0x02)) /* xxxx-xx1x ｺﾝﾍﾟｱﾏｯﾁflag IMIB による割り込み禁止 */
| 0x01) ; /* xxxx-xxx1 ｺﾝﾍﾟｱﾏｯﾁflag IMIA による割り込み許可 */
/* 右ステッピングモータは、内部clockをcountし、割り込みでGRAを更新、poat_B 下位４bit から 駆動信号出力 */
/* ITU channel_2 通常動作(timer mode) 内部クロック入力 割り込み有り GRA ｺﾝﾍﾟｱﾏｯﾁ 出力無し */
ITU2.TCR.BYTE= ( (((ITU2.TCR.BYTE & 0x80) /* 1xxx-xxxx 初期値 cls bit7 is reserve bit */
| 0x20) /* xBAx-xxxx TCNTのクリア条件設定 GRA ｺﾝﾍﾟｱﾏｯﾁ */
&(~0x18)) /* xxx1-1xxx 外部クロックの入力エッジ制御 */
| 0x00) ; /* xxxx-x000 内部クロック Φ/1 count */
/* | 0x01) ; */ /* xxxx-x001 内部クロック Φ/２ count */
/* GRA,GRB の機能選択 ＆ ｺﾝﾍﾟｱﾏｯﾁ(comparematch) による端子出力の許可 */
ITU2.TIOR.BYTE= ( ((ITU2.TIOR.BYTE & 0x88) /* 1xxx-1xxx 初期値 cls bit7,bit4 is reserve bit */
&(~0x70)) /* x111-xxxx GRB is ｱｳﾄﾌﾟｯﾄ・ｺﾝﾍﾟｱ ＆ Ｂ端子 出力禁止 */
&(~0x07)) ; /* xxxx-x111 GRA is ｱｳﾄﾌﾟｯﾄ・ｺﾝﾍﾟｱ ＆ Ａ端子 出力禁止 */
/* ｺﾝﾍﾟｱﾏｯﾁ(comparematch) ＆ over flow による割り込みの許可 */
ITU2.TIER.BYTE= ( (((ITU2.TIER.BYTE & 0xf8) /* 1111-1xxx 初期値 cls bit7 to bit4 is reserve bit */
&(~0x04)) /* xxxx-x1xx over flow flag による割り込み許可 */
&(~0x02)) /* xxxx-xx1x ｺﾝﾍﾟｱﾏｯﾁflag IMIB による割り込み禁止 */
| 0x01) ; /* xxxx-xxx1 ｺﾝﾍﾟｱﾏｯﾁflag IMIA による割り込み許可 */
/* センサ系センサ系データ取り込み ＆ 姿勢制御 内部clockをcount GRA更新及び信号出力無し */
/* 【注意】割り込み優先順位が低いので、時間軸を固定できない ｼﾞｬｲﾛを使うときは注意が必要 */
/* ITU channel_3 通常動作(timer mode) 内部クロック入力 割り込み有り GRA ｺﾝﾍﾟｱﾏｯﾁ 出力無し */
ITU3.TCR.BYTE= ( (((ITU3.TCR.BYTE & 0x80) /* 1xxx-xxxx 初期値 cls bit7 is reserve bit */
| 0x20) /* xBAx-xxxx TCNTのクリア条件設定 GRA ｺﾝﾍﾟｱﾏｯﾁ */
&(~0x18)) /* xxx1-1xxx 外部クロックの入力エッジ制御 */
| 0x00) ; /* xxxx-x000 内部クロック Φ/1 count */
/* | 0x01) ; */ /* xxxx-x001 内部クロック Φ/２ count */
/* GRA,GRB の機能選択 ＆ ｺﾝﾍﾟｱﾏｯﾁ(comparematch) による端子出力の許可 */
ITU3.TIOR.BYTE= ( ((ITU3.TIOR.BYTE & 0x88) /* 1xxx-1xxx 初期値 cls bit7,bit4 is reserve bit */
&(~0x70)) /* x111-xxxx GRB is ｱｳﾄﾌﾟｯﾄ・ｺﾝﾍﾟｱ ＆ Ｂ端子 出力禁止 */
&(~0x07)) ; /* xxxx-x111 GRA is ｱｳﾄﾌﾟｯﾄ・ｺﾝﾍﾟｱ ＆ Ａ端子 出力禁止 */
/* ｺﾝﾍﾟｱﾏｯﾁ(comparematch) ＆ over flow による割り込みの許可 */
ITU3.TIER.BYTE= ( (((ITU3.TIER.BYTE & 0xf8) /* 1111-1xxx 初期値 cls bit7 to bit4 is reserve bit */
&(~0x04)) /* xxxx-x1xx over flow flag による割り込み許可 */
&(~0x02)) /* xxxx-xx1x ｺﾝﾍﾟｱﾏｯﾁflag IMIB による割り込み禁止 */
| 0x01) ; /* xxxx-xxx1 ｺﾝﾍﾟｱﾏｯﾁflag IMIA による割り込み許可 */
/* 初期化 終了 ＩＴＵ・GRA 初期値 set */
/* 加速dataの配列変数の添字は、ルーチンの最初、及び mouseが停止したときにだけ初期化する */
/* 【注意】 accele_adress は、常に仮想車輪の現在の 速度data adress を記憶している */
/* 従って、走行中の accele_adress の reset は絶対禁止 */
/* 仮想車輪の現在の 速度data adress cls */
accele_adress= 0 ;
/* 仮想車輪の現在の 速度data adress 初期化 速度の offset を 加算する */
/* accele_adress は、常に仮想車輪の現在の 速度data adress を記憶している */
accele_adress= accele_adress+ speed_select ; /* 仮想車輪の現在の 速度data adress 設定 */
/* 左右の motor の 速度data adress 設定 */
left_motor_adress= accele_adress ;
right_motor_adress= accele_adress ;
/* 励磁data(励磁パターン)配列を、ルーチンの最初に、1回だけ初期化する */
Right_Reiji= 0 ; /* 励磁data配列変数の添字を、初期値(0x00)に戻す */
Left_Reiji= 0 ;
/* 初期化 終了 ＩＴＵ・GRA 初期値 set */
ITU0.GRA= accele121[accele_adress] ; /* GRA comparematch 仮想車輪 初速 set */
ITU1.GRA= accele121[left_motor_adress] ; /* GRA compare data 左motor 初速 set */
ITU2.GRA= accele121[right_motor_adress] ; /* GRA compare data 右motor 初速 set */
ITU3.GRA= 16000 ; /* 16000000/16000 is 1000 1 msec ｲﾝﾀｰﾊﾞﾙ ﾀｲﾏ */
/* 姿勢制御data reset */
revisLM= 0x00, revisRM= 0x00 ; /* revision price 制御値 */
propoFL= 0x00, propoFR= 0x00 ; /* proportion price 比例値 */
/* 初期化 終了 全ＩＴＵ 作動開始 */
ITU.TSTR.BYTE= ( (((((ITU.TSTR.BYTE & 0xc0) /* 111x-xxxx 初期値 cls */
&(~0x10)) /* xxx1-xxxx ch_4 start */
| 0x08) /* xxxx-1xxx ch_3 start */
| 0x04) /* xxxx-x1xx ch_2 start */
| 0x02) /* xxxx-xx1x ch_1 start */
| 0x01) ; /* xxxx-xxx1 ch_0 start */
}
/* D/A.C 初期化 DADR0 is SLA7033 VRef制御 DADR1 is 圧電 buzzer */
void dac_init(void) /* D/A.C 初期化 */
{
DA.CR.BYTE= ( (((DA.CR.BYTE & 0x1f) /* xxx1-1111 is DACR 初期値 */
&(~0x80)) /* 1xxx-xxxx enable D/A_1 */
| 0x40) /* x1xx-xxxx enable D/A_0 */
&(~0x20)) ; /* xx1x-xxxx D/A.C 一括制御 */
/* in stand_by D/A.C is sleep */
DA.STCR.BYTE= (DA.STCR.BYTE | 0x01) ; /* xxxx-xxx1 D/A.C sleep時 D/A.C data 出力許可 */
/* D/A.C data cls */
DA.DR0= 0x00 ; /* SLA7033 VRef is 0V */
}
/* A/D.C 初期化 */
/* 単一ﾓｰﾄﾞ ＆ 高速変換 ＆ A/D変換終了時の割り込み禁止 ＆ 外部信号によるA/D変換禁止 */
void adc_init(void) /* A/D.C 初期化 */
{
AD.CSR.BYTE= ( (((((AD.CSR.BYTE & 0x00) /* xxxx-xxxx 初期値 cls reserve bit 無し */
&(~0x80)) /* 1xxx-xxxx A/D変換終了flag クリア */
&(~0x40)) /* x1xx-xxxx A/D変換終了時の割り込み要求制御 */
&(~0x20)) /* xx1x-xxxx A/D変換 start */
&(~0x10)) /* xxx1-xxxx 単一変換mode or scan mode */
| 0x08) ; /* xxxx-1xxx clok select(高速変換に設定) */
/* 外部信号によるA/D変換禁止 */
AD.CR.BYTE= ( (AD.CR.BYTE & 0x7f) /* 0111-1111 初期値 cls bit 0 to bit 6 reserve bit */
&(~0x80)) ; /* 1xxx-xxxx 外部信号によるA/D変換開始を許可 */
}
/* ウオッチドッグタイマの初期化 インターバルタイマモード 割り込み有り */
void wdt_init(void) /* ウオッチドッグタイマ 初期化 */
{
/* ウオッチドッグタイマ 作動停止 */
TCSRW= ( 0xa500 & (~0x0020)) ; /* xxxx-xxxx-xx1x-xxxx timer start */
/* ウオッチドッグタイマ 初期化 */
TCNTW= ( 0x5a00 & 0xff00 ) ; /* カウンタ 初期値 0x00 set 【注意】counter is 8 bit */
/* TCSRW= (((((0xa500) & 0xff7f) & 0xffbf) & 0xffdf) | 0x0002) ; */
TCSRW= ((((( 0xa500 ) /* 0xa5xx is TCSR 制御語 */
&(~0x0080)) /* xxxx-xxxx-1xxx-xxxx OVF flag cls */
&(~0x0040)) /* xxxx-xxxx-x1xx-xxxx インターバルタイマモード */
&(~0x0020)) /* xxxx-xxxx-xx1x-xxxx タイマ stop */
/* | 0x0002 ) ; */ /* φ/64 (1/16000000*64)*256 is 1.024msec */
| 0x0007 ) ; /* φ/4096 (1/16000000*4096)*256 is 65.536msec */
/* WDT counter is 8 bit */
}
/* Ｈ８cpu 初期化 */
/* 【注意】 ＩＴＵが信号を取り込む兼用ポートは、ＩＴＵの初期化を行う前にＩ／Ｏポートの設定を行い、 */
/* 入力ポートに設定しておくこと。ＩＴＵの初期化が終わってからＩ／Ｏポートを入力ポートに設定しても、*/
/* ＩＴＵは信号を読み込むことができない */
void H8cpu_init(void) /* Ｈ８cpu 初期化 */
{
/* 【注意】 ＳＹＳＣＲレジスタの bit_3(UE) は、ＣＣＲレジスタの bit_7(I)、bit_6(UI) と連動して */
/* cpu 全体の割り込みの許可 禁止を設定する。 */
/* SYSCR のbit_3(UE)が、初期値 １ のままならば、CCR の bit_6(UI) は割り込みに無関係となる。 */
/* 従って、ＳＹＳＣＲレジスタの初期化には充分注意すること */
/* SYSCR システムコントロールレジスタの設定 初期値のままで良い*/ /* 0000-1011 初期値 */
/* MSTCR モジュールスタンバイ コントロール レジスタ設定 */
MSTCR.BYTE= (((((((( MSTCR.BYTE & 0x40) /* x1xx-xxxx 初期値 cls bit6 is reserve bit */
&(~0x80)) /* 1xxx-xxxx クロック出力 禁止 */
&(~0x20)) /* xx1x-xxxx ITU スタンバイ */
| 0x10) /* xxx1-xxxx SCI_０ スタンバイ */
&(~0x08)) /* xxxx-1xxx SCI_１ スタンバイ */
| 0x04) /* xxxx-x1xx DMAC スタンバイ */
| 0x02) /* xxxx-xx1x refresh controler スタンバイ */
&(~0x01)) ; /* xxxx-xxx1 A/D.C スタンバイ */
/* 割り込み制御 設定 むちゃくちゃ面倒なので設定しない（初期値のまま） */
/* バス幅コントロールレジスタ ＆ ウエイトコントロールレジスタの初期化 */
/* 【注意】バス幅コントロールレジスタは、cpu の modeによって初期値が変わるのチェックが必要 */
BSC.ABWCR.BYTE= 0xff ; /* バス幅コントロールレジスタ 全エリア バス幅 is ８bit */
/* アクセスステートコントロールレジスタ 初期値 0xff 1111-1111 */
BSC.ASTCR.BYTE= 0x00 ; /* 全エリア ２アクセスステート */
/* バスリリース・コントロールﾚｼﾞｽﾀの設定 初期値 0xfe 1111-1110 */
/* BRCR is poat_A アドレス選択 ＆ バスリクエストの許可 初期値 0xfe 1111-1110 */
BSC.BRCR.BYTE= 0xfe ; /* poat_A poat設定 ＆ バスリクエストの禁止 1111-1110 */
/* WCR is ウエイトコントロールレジスタの設定 初期値 0xf3 1111-0011 */
BSC.WCR.BYTE= 0xf3 & (~0x0c) ; /* WMS1 WMS0 xxxx-11xx */
/* WCER is ウェイトコントローラーイネーブルﾚｼﾞｽﾀの設定 初期値 0xff */
BSC.WCER.BYTE= 0xff ; /* 全アクセス空間 ウエイト 禁止 */
/* チップセレクトコントロールﾚｼﾞｽﾀの設定 CS 7-> CS 4 の チップセレクト信号出力 制御 */
BSC.CSCR.BYTE= 0x0f ; /* 初期値 0x0f is CS 7-> CS 4 の チップセレクト信号出力 禁止 */
/* リフレッシュコントロールﾚｼﾞｽﾀの設定 初期値 0x02 */
/* poat_8 初期化のためには、リフレッシュコントロールﾚｼﾞｽﾀの設定が必要 */
RFSHC.RFSHCR.BYTE= 0x02 ; /* リフレッシュ 禁止 初期値 0x02 0000-0010 */
/* ＴＰＣ(プログラマブル・タイミングパターン・コントローラの初期化 */
/* poat_A 初期化のために必要だが、初期値 0x00 で良いので、設定しない */
/* 周辺機能 初期化 */
adc_init() ; /* A/D.C 初期化 */
dac_init() ; /* D/A.C 初期化 */
/* 【注意】 以下は、この順で初期化しないと、レジスタの設定がごちゃごちゃになる */
wdt_init() ; /* ウオッチドッグタイマ 初期化 */
sci_init() ; /* シリアル通信 初期化 */
itu_init() ; /* ＩＴＵ 初期化 */
port_init() ; /* 入出力ポートの初期化 */
}
/* 割り込み制御 禁止 or 許可 */
void interrupt_control(UCHAR sw) /* 割り込み制御 禁止 or 許可 */
{
if ( ((unsigned char)sw) == SLEEP ) {
/* ウオッチドッグタイマ 割り込み制御 割り込み禁止 */
wdt_interrupt_control(SLEEP) ; /* ウオッチドッグタイマ 割り込み制御 */
/* ＩＴＵ 割り込みflag reset(割り込み不可) */
ITU0.TIER.BIT.IMIEA = 0 ; /* Dis_enable comparematch_A interrupt */
ITU1.TIER.BIT.IMIEA = 0 ; /* Dis_enable comparematch_A interrupt */
ITU2.TIER.BIT.IMIEA = 0 ; /* Dis_enable comparematch_A interrupt */
ITU3.TIER.BIT.IMIEA = 0 ; /* Dis_enable comparematch_A interrupt */
/* ＩＴＵ 作動停止 */
ITU.TSTR.BYTE= ( (((((ITU.TSTR.BYTE & 0xc0) /* 111x-xxxx 初期値 cls bit7 to bit5 is reserve bit */
&(~0x10)) /* xxx1-xxxx ch_4 able or stop */
&(~0x08)) /* xxxx-1xxx ch_3 able or stop */
&(~0x04)) /* xxxx-x1xx ch_2 able or stop */
&(~0x02)) /* xxxx-xx1x ch_1 able or stop */
&(~0x01) ) ; /* xxxx-xxx1 ch_0 able or stop */
/* 割り込みマスクreg 割り込み禁止 set */
set_ccr( (unsigned char )(get_ccr() | 0x80) ) ;/* 割り込みマスクreg 割り込み禁止 set */
}
else if ( ((unsigned char)sw) == WAKE ) {
/* ウオッチドッグタイマ 割り込み制御 割り込み許可 */
wdt_interrupt_control(WAKE) ; /* ウオッチドッグタイマ 割り込み制御 */
/* ＩＴＵ 割り込みflag set(割り込み可) */
ITU0.TIER.BIT.IMIEA = 1 ; /* enable comparematch_A interrupt */
ITU1.TIER.BIT.IMIEA = 1 ; /* enable comparematch_A interrupt */
ITU2.TIER.BIT.IMIEA = 1 ; /* enable comparematch_A interrupt */
ITU3.TIER.BIT.IMIEA = 1 ; /* enable comparematch_A interrupt */
/* ＩＴＵ 作動開始 */
ITU.TSTR.BYTE= ( (((((ITU.TSTR.BYTE & 0xc0) /* 111x-xxxx 初期値 cls bit7 to bit5 is reserve bit */
&(~0x10)) /* xxx1-xxxx ch_4 able or stop */
| 0x08) /* xxxx-1xxx ch_3 able or stop */
| 0x04) /* xxxx-x1xx ch_2 able or stop */
| 0x02) /* xxxx-xx1x ch_1 able or stop */
| 0x01 ) ; /* xxxx-xxx1 ch_0 able or stop */
/* 割り込みマスクreg 割り込み許可 set */
set_ccr( (unsigned char )(get_ccr() & 0x7f) ) ; /* 割り込みマスクreg 割り込み許可 set */
}
}
/* アナログ距離センサ data 読み込み */
USHORT adcread_Phot(UCHAR ChannelData) /* アナログ距離センサ data 読み込み */
{
USHORT result ; /* 変換結果格納用変数 */
UCHAR channel ;
channel= ChannelData ; /* アナログ距離センサ channel data 退避 */
channel= channel & 0x03 ; /* 下位 3bitがセンサのchannel */
/* 今回は４個のセンサだけを使うので、ここでchannelのANDをとっても良いが
channel 4 --> channel 7 までを使うときは、ここで ANDをとらないこと */
/* 赤外(infrared_ray)LED 発光制御(active Low) */
if (channel == 0x00) PA.DR.BYTE= (~0x01) ; /* xxxx-xxx1 bit 0 右前・赤外LED 発光 */
else if (channel == 0x01) PA.DR.BYTE= (~0x02) ; /* xxxx-xx1x bit 1 右横・赤外LED 発光 */
else if (channel == 0x02) PA.DR.BYTE= (~0x04) ; /* xxxx-x1xx bit 2 左横・赤外LED 発光 */
else PA.DR.BYTE= (~0x08) ; /* xxxx-1xxx bit 3 左前・赤外LED 発光 */
/* 発光時間調整 オシロで確認すること */
/* FLASH_TIME が 30では赤外線が強すぎて、近くでサチレーションを起こす */
for (result= 0 ;result< FLASH_TIME ; result++ ) ; /* 発光時間調整 オシロで確認すること */
/* A/D変換機 初期化 */
/* AD.CSR.BYTE= ( (((((AD.CSR.BYTE & 0x00) /* xxxx-xxxx 初期値 cls reserve bit 無し */
/* &(~0x80)) /* 1xxx-xxxx A/D変換終了flag クリア */
/* &(~0x40)) /* x1xx-xxxx A/D変換終了時の割り込み要求制御 */
/* &(~0x20)) /* xx1x-xxxx A/D変換 start */
/* &(~0x10)) /* xxx1-xxxx 単一変換mode or scan mode */
/* | 0x08) ; /* xxxx-1xxx clok select(高速変換に設定) */
/* A/D変換開始 */
AD.CSR.BYTE= ( (((AD.CSR.BYTE &(~0x80)) /* 1xxx-xxxx A/D変換終了flag クリア */
&(~0x07)) /* xxxx-x111 作動チャネル reset */
| channel) /* Ａ／Ｄ変換するチャネルを設定 */
| 0x20 ) ; /* xx1x-xxxx A/D変換 start */
/* 赤外LED cut off */
PA.DR.BYTE= 0x0f ; /* 赤外 led cut off active Low */
/* A/D変換終了まで待つ ADCSR bit7 is 1 */
while( (!(AD.CSR.BYTE & 0x80)) ) ; /* A/D変換終了flag が on になるまで待つ waite ADCSR bit7 is 1 */
AD.CSR.BYTE= AD.CSR.BYTE & 0x7f ; /* A/D変換終了flag(ADCSR bit7 ) cls */
/* チャネル番号に相当するADDRの値を読む */
/* channel= channel & 0x03 ; */ /* adress 設定のため チャンネル4〜7を 0〜3にする */
result= ( *(volatile unsigned int*)(0xFFFFE0+ (channel*2)) ) ;
return( (unsigned int)( (result >> 6) & 0x03ff ) ) ; /* 0x03ff is 0000-0011-1111-1111 */
}
/* 姿勢制御 制御 */
void revision_control(UCHAR sw) /* 姿勢制御 制御 */
{
/* WAKE is 0xf0 SLEEP is 0x00 */
if ( ((unsigned char)sw) == WAKE ) revision_flag= WAKE ; /* 姿勢制御 許可 */
else revision_flag= SLEEP ; /* 姿勢制御 禁止 */
/* 制御data cls */
revisLM= 0x0000, revisRM= 0x0000 ; /* revision price 制御値 */
propoFL= 0x0000, propoFR= 0x0000 ; /* proportion price 比例値 */
/* diffeFL= 0x0000, diffeFR= 0x0000 ; */ /* differential price 微分値 */
integFL= 0x0000, integFR= 0x0000 ; /* integral price 積分値 */
/* deflecation_integral= 0x0000 ; */ /* 偏差の積分値 */
/* 制御方向data cls */
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 reset */
old_revision_direction= STRAIGHT ; /* 前回の姿勢制御の方向 reset */
}
/*【注意】 左右のdataに差が有る
左横センサは、壁に近くなったときの中央値との差分の変化が少ない */
/* アナログ距離センサのデータを元にマウスが迷路の中心に位置するようにステアリングを制御する */
/* 左右、両壁無しのばあいは、このルーチンは呼ばれない */
/* slalom旋回中の姿勢制御(ステアリングの補正 revision_steering_slalom) */
void revision_steer_SLA(void) /* slalom旋回中の姿勢制御(revision_steering_slalom) */
{
propoFL= 0x00 ; /* 比例値 初期化 */
propoFR= 0x00 ;
/* 左右、両方に壁が有るか ？？？ */
if ( (left_side_min < analog[LEFT_SIDE]) && (right_side_min < analog[RIGHT_SIDE]) ) {
/* 中央値とアナログセンサ値との差分を求める */
propoFL= left_side_mid- analog[LEFT_SIDE] ; /* 中央値とアナログセンサ値との差分を求める */
propoFR= right_side_mid- analog[RIGHT_SIDE] ;
/* マウスが左に傾いている場合、中央値とanalog[LEFT_SIDE]との差分は負になる */
/* 中央値とanalog[RIGHT_SIDE]との差分は正になる */
if ( (propoFL < 0x00) && (propoFR > 0x00) ) { /* ステアリングを右に修正する */
propoFR= propoFR/16 ; /* mouseが左に遷移しているので、右motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFL= (-propoFR) ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= RIGHT ; /* 現在の姿勢制御の方向 set */
}
/* マウスが右に傾いている場合、中央値とanalog[RIGHT_SIDE]との差分は負になる */
/* 中央値とanalog[LEFT_SIDE]との差分は正になる */
else if ( (propoFL > 0x00) && (propoFR < 0x00) ) { /* ステアリングを左に修正する */
propoFL= propoFL/16 ; /* mouseが右に遷移しているので、左motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFR= (-propoFL) ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= LEFT ; /* 現在の姿勢制御の方向 set */
}
/* mouseは迷路の中央にいる */
else {
/* 比例制御dataを 零にresetする */
propoFL= 0x00 ;
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
}
/* 左側にだけ壁は有るか ？？？ */
else if ( (left_side_min < analog[LEFT_SIDE]) && (right_side_min >= analog[RIGHT_SIDE]) ) {
/* 中央値とアナログセンサ値との差分を求める */
propoFL= left_side_mid- analog[LEFT_SIDE] ; /* 中央値とアナログセンサ値との差分を求める */
/* マウスが左に傾いている場合、中央値とanalog[LEFT_SIDE]との差分は負になる */
if (propoFL < 0x00) { /* ステアリングを右に修正する propoFL is ﾏｲﾅｽ */
propoFL= propoFL/16 ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
propoFR= -(propoFL) ; /* mouseが左に遷移しているので、右motorの速度を下げる data is ﾌﾟﾗｽdata */
revision_direction= RIGHT ; /* 現在の姿勢制御の方向 set */
}
/* マウスが右に傾いている場合、中央値とanalog[LEFT_SIDE]との差分は正になる */
else if (propoFL > 0x00) { /* ステアリングを左に修正する propoFL is ﾌﾟﾗｽ */
propoFL= propoFL/16 ; /* mouseが右に遷移しているので、左motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFR= -(propoFL) ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= LEFT ; /* 現在の姿勢制御の方向 set */
}
/* mouseは迷路の中央にいる */
else {
propoFL= 0x00 ; /* 比例制御dataを 零にresetする */
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
}
/* 右側にだけ壁は有るか ？？？ */
else if ( (left_side_min >= analog[LEFT_SIDE]) && (right_side_min < analog[RIGHT_SIDE]) ) {
/* 中央値とアナログセンサ値との差分を求める */
propoFR= right_side_mid- analog[RIGHT_SIDE] ; /* 中央値とアナログセンサ値との差分を求める */
/* マウスが左に傾いている場合、中央値とanalog[RIGHT_SIDE]との差分は正になる */
if (propoFR > 0x00) { /* ステアリングを右に修正する propoFR is ﾌﾟﾗｽ */
/* 制御dataを造る propoFR is ﾌﾟﾗｽdata */
propoFR= propoFR/16 ; /* mouseが左に遷移しているので、右motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFL= -(propoFR) ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= RIGHT ; /* 現在の姿勢制御の方向 set */
}
/* マウスが右に傾いている場合、中央値とanalog[RIGHT_SIDE]との差分は負になる */
else if (propoFR < 0x00) { /* ステアリングを左に修正する propoFR is ﾏｲﾅｽ */
propoFR= propoFR/16 ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
propoFL= -(propoFR) ; /* mouseが右に遷移しているので、左motorの速度を下げる data is ﾌﾟﾗｽdata */
revision_direction= LEFT ; /* 現在の姿勢制御の方向 set */
}
/* mouseは迷路の中央にいる */
else {
propoFL= 0x00 ; /* 比例制御dataを 零にresetする */
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
}
/* data miss 左右、どちらにも壁が無い */
else {
propoFL= 0x00 ; /* 比例制御dataを 零にresetする */
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
/* 前回の制御方向と今回の制御方向が異なった場合は積分値を零にreset する */
if ( revision_direction != old_revision_direction ) integFL= 0x00, integFR= 0x00 ; /* integral price(積分値) reset */
old_revision_direction= revision_direction ; /* 前回の制御の方向 <-- 今回の制御の方向 */
/* 比例値の制御dataが零の場合は、制御daraを零にreset する */
if ( (propoFL == 0x00) && (propoFR == 0x00) ) {
/* 姿勢制御data reset */
propoFL= 0x00, propoFR= 0x00 ; /* proportion price(比例値) reset */
/* diffeFL= 0x00, diffeFR= 0x00 ; */ /* differential price(微分値) reset */
integFL= 0x00, integFR= 0x00 ; /* integral price(積分値) reset */
}
else {
/* 制御dataを造る(積分値を加算する) */
integFL= integFL+ propoFL, integFR= integFR+ propoFR ; /* 比例 ＆ 積分 制御 */
/* revisLM= propoFL, revisRM= propoFR ; */ /* 比例制御 */
/* 姿勢制御dataを転送する */
revisLM= integFL, revisRM= integFR ; /* 比例 ＆ 積分 制御 */
/* 制御daraに、限界を設定する */
if (revisLM >= SLALOM_LIMIT) revisLM= SLALOM_LIMIT ; /* 制御dataの限界値 set */
else if (revisLM <= -(SLALOM_LIMIT) ) revisLM= -(SLALOM_LIMIT) ;
if (revisRM >= SLALOM_LIMIT ) revisRM= SLALOM_LIMIT ;
else if (revisRM <= -(SLALOM_LIMIT) ) revisRM= -(SLALOM_LIMIT) ;
}
/* 積分値を resetする */
if (integFL >= 0x00) integFL= ( (revisLM+ 254)/256 ) ;
else if (integFL < 0x00) integFL= ( (revisLM- 254)/256 ) ;
if (integFR >= 0x00) integFR= ( (revisRM+ 254)/256 ) ;
else if (integFR < 0x00) integFR= ( (revisRM- 254)/256 ) ;
}
/* 姿勢制御(ステアリングの補正 revision_steering) */
void revision_steer_RED(void) /* 減速走行姿勢制御 ステアリングの補正 revision_steering */
{
propoFL= 0x00 ; /* 比例値 初期化 */
propoFR= 0x00 ;
/* 左右、両方に壁が有るか ？？？ */
if ( (left_side_min < analog[LEFT_SIDE]) && (right_side_min < analog[RIGHT_SIDE]) ) {
/* 中央値とアナログセンサ値との差分を求める */
propoFL= left_side_mid- analog[LEFT_SIDE] ; /* 中央値とアナログセンサ値との差分を求める */
propoFR= right_side_mid- analog[RIGHT_SIDE] ;
/* マウスが左に傾いている場合、中央値とanalog[LEFT_SIDE]との差分は負になる */
/* 中央値とanalog[RIGHT_SIDE]との差分は正になる */
if ( (propoFL < 0x00) && (propoFR > 0x00) ) { /* ステアリングを右に修正する */
propoFR= propoFR* 2 ; /* mouseが左に遷移しているので、右motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFL= (-propoFR) ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= RIGHT ; /* 現在の姿勢制御の方向 set */
}
/* マウスが右に傾いている場合、中央値とanalog[RIGHT_SIDE]との差分は負になる */
/* 中央値とanalog[LEFT_SIDE]との差分は正になる */
else if ( (propoFL > 0x00) && (propoFR < 0x00) ) { /* ステアリングを左に修正する */
propoFL= propoFL* 2 ; /* mouseが右に遷移しているので、左motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFR= (-propoFL) ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= LEFT ; /* 現在の姿勢制御の方向 set */
}
/* mouseは迷路の中央にいる */
else {
/* 比例制御dataを 零にresetする */
propoFL= 0x00 ;
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
}
/* 左側にだけ壁は有るか ？？？ */
else if ( (left_side_min < analog[LEFT_SIDE]) && (right_side_min >= analog[RIGHT_SIDE]) ) {
/* 中央値とアナログセンサ値との差分を求める */
propoFL= left_side_mid- analog[LEFT_SIDE] ; /* 中央値とアナログセンサ値との差分を求める */
/* マウスが左に傾いている場合、中央値とanalog[LEFT_SIDE]との差分は負になる */
if (propoFL < 0x00) { /* ステアリングを右に修正する propoFL is ﾏｲﾅｽ */
propoFL= propoFL* 2 ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
propoFR= -(propoFL) ; /* mouseが左に遷移しているので、右motorの速度を下げる data is ﾌﾟﾗｽdata */
revision_direction= RIGHT ; /* 現在の姿勢制御の方向 set */
}
/* マウスが右に傾いている場合、中央値とanalog[LEFT_SIDE]との差分は正になる */
else if (propoFL > 0x00) { /* ステアリングを左に修正する propoFL is ﾌﾟﾗｽ */
propoFL= propoFL* 2 ; /* mouseが右に遷移しているので、左motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFR= -(propoFL) ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= LEFT ; /* 現在の姿勢制御の方向 set */
}
/* mouseは迷路の中央にいる */
else {
propoFL= 0x00 ; /* 比例制御dataを 零にresetする */
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
}
/* 右側にだけ壁は有るか ？？？ */
else if ( (left_side_min >= analog[LEFT_SIDE]) && (right_side_min < analog[RIGHT_SIDE]) ) {
/* 中央値とアナログセンサ値との差分を求める */
propoFR= right_side_mid- analog[RIGHT_SIDE] ; /* 中央値とアナログセンサ値との差分を求める */
/* マウスが左に傾いている場合、中央値とanalog[RIGHT_SIDE]との差分は正になる */
if (propoFR > 0x00) { /* ステアリングを右に修正する propoFR is ﾌﾟﾗｽ */
/* 制御dataを造る propoFR is ﾌﾟﾗｽdata */
propoFR= propoFR* 2 ; /* mouseが左に遷移しているので、右motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFL= -(propoFR) ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= RIGHT ; /* 現在の姿勢制御の方向 set */
}
/* マウスが右に傾いている場合、中央値とanalog[RIGHT_SIDE]との差分は負になる */
else if (propoFR < 0x00) { /* ステアリングを左に修正する propoFR is ﾏｲﾅｽ */
propoFR= propoFR* 2 ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
propoFL= -(propoFR) ; /* mouseが右に遷移しているので、左motorの速度を下げる data is ﾌﾟﾗｽdata */
revision_direction= LEFT ; /* 現在の姿勢制御の方向 set */
}
/* mouseは迷路の中央にいる */
else {
propoFL= 0x00 ; /* 比例制御dataを 零にresetする */
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
}
/* data miss 左右、どちらにも壁が無い */
else {
propoFL= 0x00 ; /* 比例制御dataを 零にresetする */
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
/* 前回の制御方向と今回の制御方向が異なった場合は積分値を零にreset する */
if ( revision_direction != old_revision_direction ) integFL= 0x00, integFR= 0x00 ; /* integral price(積分値) reset */
old_revision_direction= revision_direction ; /* 前回の制御の方向 <-- 今回の制御の方向 */
/* 比例値の制御dataが零の場合は、制御daraを零にreset する */
if ( (propoFL == 0x00) && (propoFR == 0x00) ) {
/* 姿勢制御data reset */
propoFL= 0x00, propoFR= 0x00 ; /* proportion price(比例値) reset */
/* diffeFL= 0x00, diffeFR= 0x00 ; */ /* differential price(微分値) reset */
integFL= 0x00, integFR= 0x00 ; /* integral price(積分値) reset */
revisLM= 0x00, revisRM= 0x00 ; /* 姿勢制御data reset */
}
else {
/* 制御dataを造る(積分値を加算する) */
integFL= integFL+ propoFL, integFR= integFR+ propoFR ; /* 比例 ＆ 積分 制御 */
/* revisLM= propoFL, revisRM= propoFR ; */ /* 比例制御 */
/* 姿勢制御dataを転送する */
revisLM= integFL+ (ITU1.GRA/128) ; /* 現在の速度を考慮する */
revisRM= integFR+ (ITU2.GRA/128) ;
/* 制御daraに、限界を設定する */
if (revisLM >= REDUCE_LIMIT) revisLM= REDUCE_LIMIT ; /* 制御dataの限界値 set */
else if ( revisLM <= -(REDUCE_LIMIT) ) revisLM= -(REDUCE_LIMIT) ;
if (revisRM >= REDUCE_LIMIT ) revisRM= REDUCE_LIMIT ;
else if ( revisRM <= -(REDUCE_LIMIT) ) revisRM= -(REDUCE_LIMIT) ;
}
/* 積分値を resetする */
if (integFL >= 0x00) integFL= ( (revisLM+ 254)/256 ) ;
else if (integFL < 0x00) integFL= ( (revisLM- 254)/256 ) ;
if (integFR >= 0x00) integFR= ( (revisRM+ 254)/256 ) ;
else if (integFR < 0x00) integFR= ( (revisRM- 254)/256 ) ;
}
/* 姿勢制御(ステアリングの補正 revision_steering) */
void revision_steer_PHOT(void) /* 定常走行姿勢制御 ステアリングの補正 revision_steering */
{
propoFL= 0x00 ; /* 比例値 初期化 */
propoFR= 0x00 ;
/* 左右、両方に壁が有るか ？？？ */
if ( (left_side_min < analog[LEFT_SIDE]) && (right_side_min < analog[RIGHT_SIDE]) ) {
/* 中央値とアナログセンサ値との差分を求める */
propoFL= left_side_mid- analog[LEFT_SIDE] ; /* 中央値とアナログセンサ値との差分を求める */
propoFR= right_side_mid- analog[RIGHT_SIDE] ;
/* マウスが左に傾いている場合、中央値とanalog[LEFT_SIDE]との差分は負になる */
/* 中央値とanalog[RIGHT_SIDE]との差分は正になる */
if ( (propoFL < 0x00) && (propoFR > 0x00) ) { /* ステアリングを右に修正する */
propoFR= propoFR* 8 ; /* mouseが左に遷移しているので、右motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFL= (-propoFR) ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= RIGHT ; /* 現在の姿勢制御の方向 set */
}
/* マウスが右に傾いている場合、中央値とanalog[RIGHT_SIDE]との差分は負になる */
/* 中央値とanalog[LEFT_SIDE]との差分は正になる */
else if ( (propoFL > 0x00) && (propoFR < 0x00) ) { /* ステアリングを左に修正する */
propoFL= propoFL* 8 ; /* mouseが右に遷移しているので、左motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFR= (-propoFL) ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= LEFT ; /* 現在の姿勢制御の方向 set */
}
/* mouseは迷路の中央にいる */
else {
/* 比例制御dataを 零にresetする */
propoFL= 0x00 ;
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
}
/* 左側にだけ壁は有るか ？？？ */
else if ( (left_side_min < analog[LEFT_SIDE]) && (right_side_min >= analog[RIGHT_SIDE]) ) {
/* 中央値とアナログセンサ値との差分を求める */
propoFL= left_side_mid- analog[LEFT_SIDE] ; /* 中央値とアナログセンサ値との差分を求める */
/* マウスが左に傾いている場合、中央値とanalog[LEFT_SIDE]との差分は負になる */
if (propoFL < 0x00) { /* ステアリングを右に修正する propoFL is ﾏｲﾅｽ */
propoFL= propoFL* 8 ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
propoFR= -(propoFL) ; /* mouseが左に遷移しているので、右motorの速度を下げる data is ﾌﾟﾗｽdata */
revision_direction= RIGHT ; /* 現在の姿勢制御の方向 set */
}
/* マウスが右に傾いている場合、中央値とanalog[LEFT_SIDE]との差分は正になる */
else if (propoFL > 0x00) { /* ステアリングを左に修正する propoFL is ﾌﾟﾗｽ */
propoFL= propoFL* 8 ; /* mouseが右に遷移しているので、左motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFR= -(propoFL) ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= LEFT ; /* 現在の姿勢制御の方向 set */
}
/* mouseは迷路の中央にいる */
else {
propoFL= 0x00 ; /* 比例制御dataを 零にresetする */
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
}
/* 右側にだけ壁は有るか ？？？ */
else if ( (left_side_min >= analog[LEFT_SIDE]) && (right_side_min < analog[RIGHT_SIDE]) ) {
/* 中央値とアナログセンサ値との差分を求める */
propoFR= right_side_mid- analog[RIGHT_SIDE] ; /* 中央値とアナログセンサ値との差分を求める */
/* マウスが左に傾いている場合、中央値とanalog[RIGHT_SIDE]との差分は正になる */
if (propoFR > 0x00) { /* ステアリングを右に修正する propoFR is ﾌﾟﾗｽ */
/* 制御dataを造る propoFR is ﾌﾟﾗｽdata */
propoFR= propoFR* 8 ; /* mouseが左に遷移しているので、右motorの速度を下げる data is ﾌﾟﾗｽdata */
propoFL= -(propoFR) ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
revision_direction= RIGHT ; /* 現在の姿勢制御の方向 set */
}
/* マウスが右に傾いている場合、中央値とanalog[RIGHT_SIDE]との差分は負になる */
else if (propoFR < 0x00) { /* ステアリングを左に修正する propoFR is ﾏｲﾅｽ */
propoFR= propoFR* 8 ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
propoFL= -(propoFR) ; /* mouseが右に遷移しているので、左motorの速度を下げる data is ﾌﾟﾗｽdata */
revision_direction= LEFT ; /* 現在の姿勢制御の方向 set */
}
/* mouseは迷路の中央にいる */
else {
propoFL= 0x00 ; /* 比例制御dataを 零にresetする */
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
}
/* data miss 左右、どちらにも壁が無い */
else {
propoFL= 0x00 ; /* 比例制御dataを 零にresetする */
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
/* 前回の制御方向と今回の制御方向が異なった場合は積分値を零にreset する */
if ( revision_direction != old_revision_direction ) integFL= 0x00, integFR= 0x00 ; /* integral price(積分値) reset */
old_revision_direction= revision_direction ; /* 前回の制御の方向 <-- 今回の制御の方向 */
/* 比例値の制御dataが零の場合は、制御daraを零にreset する */
if ( (propoFL == 0x00) && (propoFR == 0x00) ) {
/* 姿勢制御data reset */
propoFL= 0x00, propoFR= 0x00 ; /* proportion price(比例値) reset */
/* diffeFL= 0x00, diffeFR= 0x00 ; */ /* differential price(微分値) reset */
integFL= 0x00, integFR= 0x00 ; /* integral price(積分値) reset */
revisLM= 0x00, revisRM= 0x00 ; /* 姿勢制御data reset */
}
else {
/* 制御dataを造る(積分値を加算する) */
integFL= integFL+ propoFL, integFR= integFR+ propoFR ; /* 比例 ＆ 積分 制御 */
/* revisLM= propoFL, revisRM= propoFR ; */ /* 比例制御 */
/* 姿勢制御dataを転送する */
revisLM= integFL+ (ITU1.GRA/128) ; /* 現在の速度を考慮する */
revisRM= integFR+ (ITU2.GRA/128) ;
/* 制御daraに、限界を設定する */
if (revisLM >= ACCELE_LIMIT) revisLM= ACCELE_LIMIT ; /* 制御dataの限界値 set */
else if ( revisLM <= -(ACCELE_LIMIT) ) revisLM= -(ACCELE_LIMIT) ;
if (revisRM >= ACCELE_LIMIT ) revisRM= ACCELE_LIMIT ;
else if ( revisRM <= -(ACCELE_LIMIT) ) revisRM= -(ACCELE_LIMIT) ;
}
/* 積分値を resetする */
if (integFL >= 0x00) integFL= ( (revisLM+ 254)/256 ) ;
else if (integFL < 0x00) integFL= ( (revisLM- 254)/256 ) ;
if (integFR >= 0x00) integFR= ( (revisRM+ 254)/256 ) ;
else if (integFR < 0x00) integFR= ( (revisRM- 254)/256 ) ;
}
/* 斜め走行専用 姿勢制御(ステアリングの補正 revision_steering_aslant) */
void aslant_revision_steer_PHOT(void) /* 斜め走行姿勢制御 ステアリングの補正 revision_steering_aslant */
{
propoFL= 0x00 ; /* 比例値 初期化 */
propoFR= 0x00 ;
/* 左右、両方に壁が有るか ？？？ */
/* 中央値とアナログセンサ値との差分を求める */
propoFL= left_front_min- analog[LEFT_FRONT] ; /* 中央値とアナログセンサ値との差分を求める */
propoFR= right_front_min- analog[RIGHT_FRONT] ;
/* マウスが左に傾いている場合、中央値とanalog[LEFT_FRONT]との差分は負になる */
/* 中央値とanalog[RIGHT_FRONT]との差分は正になる */
if (propoFL < propoFR) { /* ステアリングを右に修正する */
/* propoFL= propoFL* 64 ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
/* propoFL= propoFL* 20 ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
propoFL= propoFL* 32 ; /* mouseが左に遷移しているので、左motorの速度を上げる data is ﾏｲﾅｽdata */
propoFR= -propoFL ; /* mouseが左に遷移しているので、右motorの速度を下げる data is ﾌﾟﾗｽdata */
revision_direction= RIGHT ; /* 現在の姿勢制御の方向 set */
}
/* マウスが右に傾いている場合、中央値とanalog[RIGHT_FRONT]との差分は負になる */
/* 中央値とanalog[LEFT_FRONT]との差分は正になる */
else if (propoFL > propoFR) { /* ステアリングを左に修正する */
/* propoFR= propoFR* 64 ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
/* propoFR= propoFR* 24 ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
propoFR= propoFR* 32 ; /* mouseが右に遷移しているので、右motorの速度を上げる data is ﾏｲﾅｽdata */
propoFL= -propoFR ; /* mouseが右に遷移しているので、左motorの速度を下げる data is ﾌﾟﾗｽdata */
revision_direction= LEFT ; /* 現在の姿勢制御の方向 set */
}
/* 左右の差分が同じ mouseは迷路の中央にいる */
else {
/* 比例制御dataを 零にresetする */
propoFL= 0x00 ;
propoFR= 0x00 ;
revision_direction= STRAIGHT ; /* 現在の姿勢制御の方向 set */
}
/* 前回の制御方向と今回の制御方向が異なった場合は積分値を零にreset する */
if ( revision_direction != old_revision_direction ) integFL= 0x00, integFR= 0x00 ; /* integral price(積分値) reset */
old_revision_direction= revision_direction ; /* 前回の制御の方向 <-- 今回の制御の方向 */
/* 比例値の制御dataが零の場合は、制御daraを零にreset する */
if ( (propoFL == 0x00) && (propoFR == 0x00) ) {
/* 姿勢制御data reset */
propoFL= 0x00, propoFR= 0x00 ; /* proportion price(比例値) reset */
/* diffeFL= 0x00, diffeFR= 0x00 ; */ /* differential price(微分値) reset */
integFL= 0x00, integFR= 0x00 ; /* integral price(積分値) reset */
}
/* 制御dataを造る(積分値を加算する) */
integFL= integFL+ propoFL, integFR= integFR+ propoFR ; /* 比例 ＆ 積分 制御 */
/* revisLM= propoFL, revisRM= propoFR ; */ /* 比例制御 */
/* 姿勢制御dataを転送する */
revisLM= integFL, revisRM= integFR ; /* 比例 ＆ 積分 制御 */
/* 積分値を resetする */
if (integFL >= 0x00) integFL= ( (revisLM+ 7)/8 ) ;
else if (integFL < 0x00) integFL= ( (revisLM- 7)/8 ) ;
if (integFR >= 0x00) integFR= ( (revisRM+ 7)/8 ) ;
else if (integFR < 0x00) integFR= ( (revisRM- 7)/8 ) ;
}
/* ウオッチドッグタイマ 割り込み処理ルーチン 経過時間のcount 探査を中止させる */
/* １） ３分３０秒をカウントさせて、ゴールに入っていたら走行を停止するプログラムを組み込む */
/* ２） 時間軸が固定されるので、ジャイロの dataを積分する 今回は未使用 */
#pragma interrupt(interrupt_wdt_enter)
void interrupt_wdt_enter(void) /* ウオッチドッグタイマ 割り込み処理ルーチン 経過時間のcount */
{
/* ＷＤＴ(ｳｵｯﾁﾄﾞｯｸﾀｲﾏ)経過時間count (65.536msec* WDT_counter) */
WDT_counter= WDT_counter+ 1 ; /* ＷＤＴ(ｳｵｯﾁﾄﾞｯｸﾀｲﾏ)経過時間count (65.536msec* WDT_counter) */
/* ウオッチドッグタイマ ＯＶＦflag reset */
TCSRW= ( 0xa500 | ((int )( (TCSRR & 0x7f) )) ) ; /* 0xa5xx is TCSR 制御語 */
}
/* ウオッチドッグタイマ 割り込み制御 */
void wdt_interrupt_control(UCHAR sw) /* ウオッチドッグタイマ 割り込み制御 */
{
UCHAR i ;
/* LED 制御 */
P2.DR.BYTE= 0xff ; /* ｲﾝﾀｰﾌｪｲｽ LED cut off */
P5.DR.BYTE= 0xff ; /* 増設 LED cut off */
PA.DR.BYTE= 0x0f ; /* 赤外 LED cut off */
/* ウオッチドッグタイマ 作動停止 */
i= TCSRR ; /* TCSR 下位 ８bit data 読み込み */
i= i & 0xdf ; /* 0xdf is xx0x-xxxx タイマ stop */
TCSRW= ( 0xa500 | ((int )i) ) ; /* 0xa5xx is TCSR 制御語 */
if( sw == WAKE ) { /* ウオッチドッグタイマ 作動 */
TCNTW= ( 0x5a00 & 0xff00 ) ; /* カウンタ 初期値 0x00 set */
i= TCSRR ; /* TCSR 下位 ８bit data 読み込み */
i= ( ((i & 0x7f) /* 0x7f is 0xxx-xxxx OVF flag cls */
| 0x20) ) ; /* 0x20 is 0010-0000 タイマ start */
TCSRW= ( 0xa500 | ((int )i) ) ; /* 0xa5xx is TCSR 制御語 */
}
else { /* ウオッチドッグタイマ 作動停止 */
i= TCSRR ; /* TCSR 下位 ８bit data 読み込み */
i= ( ((i & 0x7f) /* 0x7f is 0xxx-xxxx OVF flag cls */
& 0xdf) ) ; /* 0xdf is xx0x-xxxx タイマ stop */
TCSRW= ( 0xa500 | ((int )i) ) ; /* 0xa5xx is TCSR 制御語 */
}
}
/* ＩＴＵ channel_0 GRA ｺﾝﾍﾟｱﾏｯﾁ(comparematch) 割り込み処理ルーチン 仮想車輪 走行ﾊﾟﾙｽ数ｶｳﾝﾄ */
#pragma interrupt(interrupt_ITU0_enter)
void interrupt_ITU0_enter(void) /* ＩＴＵ channel_0 割り込み処理ルーチン */
{
/* ITU channel_0 割り込みflag reset */
ITU0.TSR.BIT.IMFA = 0 ; /* interrupt flag reset(stop interrupt request) */
/* 【注意】仮想車輪の加減速は、直進走行だけではなく、旋回の状態によっても制限される */
/* 【注意】旋回方向 確認 slalom旋回中は、主割り込みのdataでは加減速をしない */
/* 【注意】旋回方向 確認 pivot_turn中は、主割り込みのdataでは加減速をしない */
/* mouseは slalom旋回 or pivot_turn 中か ??? 仮想車輪を加速する or Ｎot */
if ( (turn_direction & SLALOM) != 0xf0 ) { /* 旋回時に、仮想車輪を加速する or Ｎot */
/* mouseは加速走行中か ??? */
if (accele_condition == ACCELE) { /* accele_condition is ACCELE then mouseは加速する */
/* yes mouseは加速中 定速走行に入れるか ??? */
if (position_now <= accele_pulse) accele_condition= GENERAL ; /* accele_condition reset 定常走行 */
/* no mouseは加速を続ける */
else accele_adress= accele_adress+ 1 ;
}
/* mouseは定常走行中か ??? */
else if (accele_condition == GENERAL ) { /* accele_condition is GENERAL then mouseは定常走行をする */
/* yes mouseは定常走行中 減速走行に入れるか ??? */
if (position_now <= reduce_pulse) accele_condition= REDUCE ; /* accele_condition reset 減速走行 */
}
/* mouseは減速走行中か ??? ２倍のレートで減速する */
else if (accele_condition == REDUCE) { /* accele_condition is REDUCE then mouseは減速する */
if (position_now > 0) { /* mouseは減速走行中 定常走行に入れるか ??? */
/* (Ｎot 探査走行) ＆＆ (mouseは直進している) */
/* if ( (turn_direction == STRAIGHT) && (running_mode != SEARCH) ) */
if ( (turn_direction == SPIN_LEFT) || (turn_direction == SPIN_RIGHT) ) {
/* 信地旋回中 は、加速したﾊﾟﾙｽ数だけ減速する */
/* 最低速度を、slalom進入速度以下に下げない */
if ( (accele_adress >= 1) && (accele_adress > (reduce_limit+ 1)) ) {
accele_adress= accele_adress- 1 ; }
}
else {
/* (Ｎot 信地旋回) ＆＆ (直進行中)は、加速ﾚｰﾄの２倍のﾚｰﾄで減速する */
/* 最低速度を、slalom進入速度以下に下げない */
if ( (accele_adress >= 2) && (accele_adress > (reduce_limit+ 2)) ) {
accele_adress= accele_adress- 2 ; }
}
}
/* else *//* mouseは設定速度まで減速した 定常走行に入れる */
}
/* 加減速dataテーブルの adress 転送 */
left_motor_adress= accele_adress ;
right_motor_adress= accele_adress ;
}
/* 加速・減速ﾚｰﾄ設定終了 ITU channel_0(仮想車輪)の時定数を設定 */
ITU0.GRA= accele121[accele_adress] ; /* ITU channel_0(仮想車輪)の時定数を reset */
/* 走行距離 reset */
if (position_now >= 1) position_now= position_now- 1 ; /* pulse counter */
}
/* ＩＴＵ channel_1 GRA ｺﾝﾍﾟｱﾏｯﾁ(comparematch) 割り込み処理ルーチン */
/* 左motor駆動 ＆ 姿勢制御 【注意】加減速は、主割り込み(channel_0)が担う */
#pragma interrupt(interrupt_ITU1_enter)
void interrupt_ITU1_enter(void) /* ＩＴＵ channel_1 割り込み処理ルーチン */
{
ITU1.TSR.BIT.IMFA= 0 ; /* interrupt flag reset(stop interrupt request) */
/* 励磁data 更新 */
/* 走行は pivot 旋回か ??? */
if ( (turn_direction == PIVO_LEFT) || (turn_direction == PIVO_RIGHT) )
{
/* 走行は 右・pivot 旋回か ??? */
if ( turn_direction == PIVO_RIGHT ) {
/* 左motor(外側車輪)の励磁data adress 更新 */
if ( Left_Reiji < 7 ) Left_Reiji= Left_Reiji+ 1 ;
else Left_Reiji= 0 ;
}
/* 走行は 左・pivot 旋回 */
/* 【注意】左・pivot 旋回では、左motor(内側車輪)は回転させない */
else {
;;;
}
} /* 励磁data 更新 end */
/* 走行は slalom 旋回か ??? */
else if ( (turn_direction == SLA_LEFT) || (turn_direction == SLA_RIGHT) )
{
/* 左motorの励磁data adress 更新 */
if ( Left_Reiji < 7 ) Left_Reiji= Left_Reiji+ 1 ;
else Left_Reiji= 0 ;
/* 姿勢制御をするか ??? */
/* ITU・GRA ( 加速data+ 左motor・姿勢制御data ) set revisLM is 左motor 姿勢制御 data */
if (revision_flag == WAKE) ITU1.GRA= ITU1.GRA+ revisLM ; /* revisLM is 左motor 姿勢制御 data */
} /* 励磁data 更新 ＆＆ 姿勢制御 終了 */
/* 走行は 直進走行 or 信地旋回 */
else {
/* 旋回方向確認 旋回方向は 左か ??? */
/*【注意】 左信地旋回では、左motorは逆回転する */
if ( turn_direction == SPIN_LEFT ) {
/* 励磁data adress 更新 */
if ( Left_Reiji > 0 ) Left_Reiji= Left_Reiji- 1 ;
else Left_Reiji= 7 ;
}
/* Ｎo 旋回方向は右・信地旋回 or 進行方向は前(直進) */
else {
/* 励磁data adress 更新 */
if ( Left_Reiji < 7 ) Left_Reiji= Left_Reiji+ 1 ;
else Left_Reiji= 0 ;
}
/* 姿勢制御をする 加減速data 更新 */
/* ITU・GRA ( 加速data+ 左motor・姿勢制御data ) set revisLM is 左motor 姿勢制御 data */
ITU1.GRA= accele121[left_motor_adress]+ revisLM ;
}
/* motor駆動信号出力 poat_B */
/* 左motor bit 7 <-- bit 4 右motor bit 3 <-- bit 0 */
/* PB.DR.BYTE= ( (reiji_L12[Left_Reiji] & 0xf0) | (reiji_R12[Right_Reiji] & 0x0f) ) ; */
PB.DR.BYTE= ( (reiji_L12[Left_Reiji] & 0xf0) | (PB.DR.BYTE & 0x0f) ) ;
}
/* ＩＴＵ channel_2 GRA ｺﾝﾍﾟｱﾏｯﾁ(comparematch)割り込み処理ルーチン
右motor駆動 ＆ 姿勢制御 */
#pragma interrupt(interrupt_ITU2_enter)
void interrupt_ITU2_enter(void) /* ＩＴＵ channel_2 割り込み処理ルーチン */
{
ITU2.TSR.BIT.IMFA= 0 ; /* interrupt flag reset(stop interrupt request) */
/* 励磁data 更新 */
/* 走行は pivot 旋回か ??? */
if ( (turn_direction == PIVO_LEFT) || (turn_direction == PIVO_RIGHT) )
{
/* 走行は 左・pivot 旋回か ??? */
if ( turn_direction == PIVO_LEFT ) {
/* 右motor(外側車輪)の励磁data adress 更新 */
if ( Right_Reiji < 7 ) Right_Reiji= Right_Reiji+ 1 ;
else Right_Reiji= 0 ;
}
/* 走行は 右・pivot 旋回 */
/* 【注意】右・pivot 旋回では、右motor(内側車輪)は回転させない */
else {
;;;
}
} /* 励磁data 更新 end */
/* 走行は slalom 旋回か ??? */
else if ( (turn_direction == SLA_LEFT) || (turn_direction == SLA_RIGHT) )
{
/* 励磁data adress 更新 */
if ( Right_Reiji < 7 ) Right_Reiji= Right_Reiji+ 1 ;
else Right_Reiji= 0 ;
/* 姿勢制御をするか ??? */
/* ITU・GRA ( 加速data+ 右motor・姿勢制御data ) set revisRM is 右motor 姿勢制御 data */
if (revision_flag == WAKE) ITU2.GRA= ITU2.GRA+ revisRM ; /* revisRM is 右motor 姿勢制御 data */
} /* 励磁data 更新 end */
/* 走行は 直進走行 or 信地旋回 */
else {
/* 旋回方向確認 旋回方向は 右 or 右・反転 か ??? */
/*【注意】 右信地旋回では、右motorは逆回転する */
/* if ( (turn_direction == SPIN_RIGHT) || (turn_direction == REVERS_SPIN) ) { */
if ( turn_direction == SPIN_RIGHT ) {
/* 励磁data adress 更新 */
if ( Right_Reiji > 0 ) Right_Reiji= Right_Reiji- 1 ;
else Right_Reiji= 7 ;
}
/* Ｎo 旋回方向は左・信地旋回 or 進行方向は前(直進) */
else {
/* 励磁data adress 更新 */
if ( Right_Reiji < 7 ) Right_Reiji= Right_Reiji+ 1 ;
else Right_Reiji= 0 ;
}
/* 姿勢制御をする 加減速data 更新 */
/* ITU・GRA ( 加速data+ 右motor・姿勢制御data ) set revisRM is 右motor 姿勢制御 data */
ITU2.GRA= accele121[right_motor_adress]+ revisRM ;
} /* 励磁data 更新 end */
/* motor駆動信号出力 poat_B */
/* 左motor bit 7 <-- bit 4 右motor bit 3 <-- bit 0 */
/* PB.DR.BYTE= ( (reiji_L12[Left_Reiji] & 0xf0) | (reiji_R12[Right_Reiji] & 0x0f) ) ; */
PB.DR.BYTE= ( (PB.DR.BYTE & 0xf0) | (reiji_R12[Right_Reiji] & 0x0f) ) ;
}
/* ＩＴＵ channel_3 GRA ｺﾝﾍﾟｱﾏｯﾁ(comparematch) 割り込み処理ルーチン センサ系データ取り込み ＆ 姿勢制御 */
/* 割り込み優先順位が低いので、時間軸を固定できない ｼﾞｬｲﾛを使うときは注意が必要 */
#pragma interrupt(interrupt_ITU3_enter)
void interrupt_ITU3_enter(void) /* ＩＴＵ channel_３ 割り込み処理ルーチン */
{
int result ; /* A/D.C data record */
unsigned char channel ; /* A/D.C channel 退避エリア */
/* 割り込み制御flag cls */
ITU3.TSR.BIT.IMFA= 0 ; /* interrupt flag reset(stop interrupt request) */
/* 前回のアナログ・センサの値を記憶 */
/* analog_old[sens_number]= analog[sens_number] ; */
/* アナログ距離センサ data 読み込み */
channel= sens_number ; /* アナログ距離センサ channel data 退避 */
channel= channel & 0x03 ; /* 下位 3bitがセンサのchannel */
/* 今回は４個のセンサだけを使うので、ここでchannelのANDをとっても良いが */
/* channel 4 --> channel 7 までを使うときは、ここで ANDをとらないこと */
/* 赤外(infrared_ray)LED 発光制御 (下位 ４bit --> 上位４bit) ＆ data反転(active Low) */
if (channel == 0x00) PA.DR.BYTE= (~0x01) ; /* xxxx-xxx1 bit 0 右前・赤外LED 発光 */
else if (channel == 0x01) PA.DR.BYTE= (~0x02) ; /* xxxx-xx1x bit 1 右横・赤外LED 発光 */
else if (channel == 0x02) PA.DR.BYTE= (~0x04) ; /* xxxx-x1xx bit 2 左横・赤外LED 発光 */
else PA.DR.BYTE= (~0x08) ; /* xxxx-1xxx bit 3 左前・赤外LED 発光 */
/* 赤外LED発光制御 終了 発光時間調整 */
/* 前壁を読む時は、LEDの発光時間を長くする */
if ( (channel == RIGHT_FRONT) || (channel == LEFT_FRONT) ) {
/* 2004/10/31 for (result= 0 ;result< (FLASH_TIME+ 5) ; result++ ) ; /* 発光時間調整 オシロで確認すること */
for (result= 0 ;result< (FLASH_TIME+ 15) ; result++ ) ; /* 発光時間調整 オシロで確認すること 2005/02/13 */
}
else {
for (result= 0 ;result< FLASH_TIME ; result++ ) ;
}
/* A/D変換機 初期化 */
/* AD.CSR.BYTE= ( (((((AD.CSR.BYTE & 0x00) /* xxxx-xxxx 初期値 cls reserve bit 無し */
/* &(~0x80)) /* 1xxx-xxxx A/D変換終了flag クリア */
/* &(~0x40)) /* x1xx-xxxx A/D変換終了時の割り込み要求制御 */
/* &(~0x20)) /* xx1x-xxxx A/D変換 start */
/* &(~0x10)) /* xxx1-xxxx 単一変換mode or scan mode */
/* | 0x08) ; /* xxxx-1xxx clok select(高速変換に設定) */
/* A/D変換開始 */
AD.CSR.BYTE= ( (((AD.CSR.BYTE &(~0x80)) /* 1xxx-xxxx A/D変換終了flag クリア */
&(~0x07)) /* xxxx-x111 作動チャネル reset */
| channel) /* Ａ／Ｄ変換するチャネルを設定 */
| 0x20 ) ; /* xx1x-xxxx A/D変換 start */
/* 赤外LED cut off */
PA.DR.BYTE= 0x0f ; /* 赤外 led cut off active Low */
/* A/D変換終了まで待つ ADCSR bit7 is 1 */
while( (!(AD.CSR.BYTE & 0x80)) ) ; /* A/D変換終了まで待つ ADCSR bit7 is 1 */
AD.CSR.BYTE= AD.CSR.BYTE & 0x7f ; /* ADCSR bit7 reset */
/* チャネル番号に相当するADDRの値を読む */
/*【注意】 変換結果が格納されるレジスタのadress設定のため、４channel以上の A/D変換器を使用するばあいは、*/
/* ここでchannelのANDをとって チャンネル4〜7を チャンネル 0〜3の adressと同じにする */
/* channel= channel & 0x03 ; */ /* adress 設定のため チャンネル4〜7を 0〜3にする */
result= ( *(volatile unsigned int*)(0xFFFFE0+ (channel*2)) ) ;
result= (unsigned int)( (result >> 6) & 0x03ff ) ; /* 0x03ff is 0000-0011-1111-1111 */
analog[sens_number]= result ; /* アナログ距離センサ data 格納 */
/* センサnumber 更新 【注意】斜め走行対応に変更すること */
if (running_condition == ASLANT) { /* if running_condition is ASLANT then mouseは斜め走行をしている */
if (sens_number == 0x00) sens_number= 0x03 ; /* if sens_number is 0x03 then 左前・赤外LED 発光 */
else sens_number= 0x00 ; /* if sens_number is 0x00 then 右前・赤外LED 発光 */
}
else {
sens_number= sens_number+ 1 ;
if ( sens_number >= 4 ) sens_number= sens_number & 0x00 ;
}
/* 壁の有無を表示 ＆ 壁dataを 壁data set用に変更する */
side_wall_data_management() ; /* 横壁の dataを 壁data set用に変更する */
front_wall_data_management() ; /* 前壁の dataを 壁data set用に変更する */
/* 姿勢制御をするか ??? */
/* 姿勢制許可 ＆＆ 左右のどちらかに壁が有る then 姿勢制御 */
if (revision_flag == WAKE) {
/* 斜め走行対応 姿勢制御 */
if (running_condition == ASLANT) { /* if running_condition is ASLANT then mouseは斜め走行をしている */
/* 姿勢制許可 ＆＆ 左右のどちらかに壁が有る then 姿勢制御 */
if ( (left_front_min <= analog[LEFT_FRONT]) || (right_front_min <= analog[RIGHT_FRONT]) ) {
/* yes 左右、どちらかに壁がある 姿勢制御 start */
aslant_revision_steer_PHOT() ; /* 斜め走行姿勢制御 start */
}
/* 左右、どちらにも壁が無い then 姿勢制御data cls */
else {
/* 姿勢制御data reset */
revisLM= 0x00, revisRM= 0x00 ; /* revision price 制御値 */
propoFL= 0x00, propoFR= 0x00 ; /* proportion price 比例値 */
/* integFL= 0x00, integFR= 0x00 ; */ /* integral price 積分値 */
}
} /* 斜め走行対応 姿勢制御 終了 */
/* 定常走行対応 姿勢制御 */
else {
/* 姿勢制許可 ＆＆ 左右のどちらかに壁が有る then 姿勢制御 */
if ( (left_side_min <= analog[LEFT_SIDE]) || (right_side_min <= analog[RIGHT_SIDE]) ) {
/* yes 左右、どちらかに壁がある 姿勢制御 start */
/* slalom旋回中か ??? */
if ( (turn_direction & SLALOM) == 0xf0 ) revision_steer_SLA() ; /* slalom姿勢制御 start */
/* accele_condition reset 減速走行 */
else if (accele_condition == REDUCE) revision_steer_RED() ; /* 減速走行姿勢制御 start */
else revision_steer_PHOT() ; /* 定常走行姿勢制御 start */
}
/* 左右、どちらにも壁が無い then 姿勢制御data cls */
else {
/* 姿勢制御data reset */
revisLM= 0x00, revisRM= 0x00 ; /* revision price 制御値 */
propoFL= 0x00, propoFR= 0x00 ; /* proportion price 比例値 */
/* integFL= 0x00, integFR= 0x00 ; */ /* integral price 積分値 */
}
} /* 定常走行対応 姿勢制御 */
} /* 姿勢制御 許可ﾙｰﾁﾝ 終了 */
/* 姿勢制御禁止 then 姿勢制御data cls */
else {
/* 姿勢制御data reset */
revisLM= 0x00, revisRM= 0x00 ; /* revision price 制御値 */
propoFL= 0x00, propoFR= 0x00 ; /* proportion price 比例値 */
/* integFL= 0x00, integFR= 0x00 ; */ /* integral price 積分値 */
}
}
/* 進行方向(旋回方向)と反対向きに励磁し、強制的に停止させる */
void reverse(void) /* 進行方向(旋回方向)と反対向きに励磁し、強制的に停止させる */
{
/* 旋回方向は右か ??? */
if ( turn_direction == SPIN_RIGHT ) {
Left_Reiji= Left_Reiji- 1 ;
Right_Reiji= Right_Reiji+ 1 ; }
/* 旋回方向は左か ??? */
else if ( turn_direction == SPIN_LEFT ) {
Left_Reiji= Left_Reiji+ 1 ;
Right_Reiji= Right_Reiji- 1 ; }
/* 進行方向は前進 or slalom旋回 or pivot旋回か ??? */
/* 左右のmotorが、前進方向に旋回している */
else {
Left_Reiji= Left_Reiji- 1 ; /* 左motor 励磁data adress 更新 */
Right_Reiji= Right_Reiji- 1 ; } /* 右motor 励磁data adress 更新 */
/* motor駆動信号出力 poat_B */
/* 左motor bit 7 <-- bit 4 右motor bit 3 <-- bit 0 */
PB.DR.BYTE= ( (reiji_L12[Left_Reiji] & 0xf0) | (reiji_R12[Right_Reiji] & 0x0f) ) ;
}
/* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset)を設定 */
/* プログラムの最初、及び mouseが停止した後に、 */
/* accele_adress= 0 ; */ /* 仮想車輪の現在の 速度data adress cls */
/* accele_adress= accele_adress+ speed_select を実行、初期化する */
/* 【注意】 accele_adress は、常に仮想車輪の現在の 速度data adress を記憶している */
/* 従って、走行中の accele_adress の reset は絶対禁止 */
void adress_set(void) /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset)を設定 */
{
/* 仮想車輪の現在の 速度data adress cls */
accele_adress= 0 ;
/* 仮想車輪の現在の 速度data adress 初期化 速度の offset を 加算する */
/* accele_adress は、常に仮想車輪の現在の 速度data adress を記憶している */
accele_adress= accele_adress+ speed_select ; /* 仮想車輪の現在の 速度data adress 設定 */
/* 左右の motor の 速度data adress 設定 */
left_motor_adress= accele_adress ;
right_motor_adress= accele_adress ;
/* 【注意】 ここで励磁パターンを初期化すると、姿勢が少しずれるので、初期化禁止 */
/* Right_Reiji= 0 ; */ /* 励磁data配列変数の添字を、初期値(0x00)に戻す */
/* Left_Reiji= 0 ; */
/* 初期化 終了 ＩＴＵ・GRA 初期値 set */
ITU0.GRA= accele121[accele_adress] ; /* GRA comparematch 仮想車輪 初速 set */
ITU1.GRA= accele121[left_motor_adress] ; /* GRA compare data 左motor 初速 set */
ITU2.GRA= accele121[right_motor_adress] ; /* GRA compare data 右motor 初速 set */
ITU3.GRA= 16000 ; /* 16000000/16000 is 1000 1 msec ｲﾝﾀｰﾊﾞﾙ ﾀｲﾏ */
/* 姿勢制御data reset */
/* revisLM= 0x00, revisRM= 0x00 ; */ /* revision price 制御値 */
/* diffeFL= 0x00, diffeFR= 0x00 ; */ /* differential price 微分値 */
/* propoFL= 0x00, propoFR= 0x00 ; */ /* proportion price 比例値 */
}
/* 駆動輪の加速テーブルの adressと timerの分周比を、仮想車輪の dataと一致させる */
void timer_reset(void) /* 駆動輪の加速テーブルのadress ＆＆ timerの分周比 reset */
{
/* 駆動輪の ＩＴＵ GRA の値を、仮想輪の dataと一致させる */
/* 【注意】 accele_adress は、常に仮想車輪の現在の 速度data adress を記憶している */
/* 従って、走行中の accele_adress の reset は絶対禁止 */
/* プログラムの最初に accele_adress= accele_adress+ speed_select と、初期化済み */
left_motor_adress= accele_adress ;
right_motor_adress= accele_adress ;
/* ITU0.GRA= accele121[accele_adress] ; */ /* GRA comparematch 仮想車輪 adress rset */
ITU1.GRA= accele121[left_motor_adress] ; /* GRA compare data 左motor adress rset */
ITU2.GRA= accele121[right_motor_adress] ; /* GRA compare data 右motor adress rset */
/* ITU3.GRA= 16000 ; */ /* 16000000/16000 is 1000 1 msec ｲﾝﾀｰﾊﾞﾙ ﾀｲﾏ */
/* 【注意】 ここで励磁パターンを初期化すると、姿勢が少しずれるので、初期化禁止 */
/* Right_Reiji= 0 ; */ /* 励磁data配列変数の添字を、初期値(0x00)に戻す */
/* Left_Reiji= 0 ; */
/* 姿勢制御data reset */
/* revisLM= 0x00, revisRM= 0x00 ; */ /* revision price 制御値 */
/* diffeFL= 0x00, diffeFR= 0x00 ; */ /* differential price 微分値 */
/* propoFL= 0x00, propoFR= 0x00 ; */ /* proportion price 比例値 */
}
/*-----------------------------------------
power 関係 -------------------------------*/
/* SLA7033 PWM基準電圧設定 */
void power(unsigned char sw) /* SLA7033 PWM基準電圧設定 */
{
if (sw == WAKE) {
/* H8/3048's D/A.C is 8bit if analogVRef is 5V then 凾u is 19.6mV */
/* SLA7033 VRef rimitt is 2.4V D/A.C Maxim data is 122 */
DA.DR0= (UCHAR )current ; /* SLA7033 VRef_Maxim is 2400mV */
}
else {
/* 【注意】VRef を変更しただけでは 7033に電流が流れ続けるので、励磁信号に 0x00を出力すること */
DA.DR0= 0 ;
/* SLA7033 励磁信号 cut_off */
PB.DR.BYTE= 0x00 ; /* SLA7033 励磁信号 set */
}
}
/* SLA7033 PWM基準電圧設定 */
void powerOLD(unsigned char sw) /* SLA7033 PWM基準電圧設定 */
{
if (sw == WAKE) {
/* H8/3048's D/A.C is 8bit if analogVRef is 5V then 凾u is 19.6mV */
/* SLA7033 VRef rimitt is 2.4V D/A.C Maxim data is 122 */
DA.CR.BYTE= ( (((DA.CR.BYTE & 0x1f) /* xxx1-1111 初期値 cls bit4 to bit0 is reserve bit */
&(~0x80)) /* 1xxx-xxxx D/A.C_1 enable */
| 0x40) /* x1xx-xxxx D/A.C_0 enable */
&(~0x20)) ; /* xx1x-xxxx D/A.C 一括制御 */
DA.DR0= (UCHAR )current ; /* SLA7033 VRef_Maxim is 2000mV */
}
else {
/* D/A.C を sleep_mode にいれる */
DA.CR.BYTE= ( (((DA.CR.BYTE & 0x1f) /* xxx1-1111 初期値 cls bit4 to bit0 is reserve bit */
&(~0x80)) /* 1xxx-xxxx D/A.C_1 enable */
&(~0x40)) /* x1xx-xxxx D/A.C_0 enable */
&(~0x20)) ; /* xx1x-xxxx D/A.C 一括制御 */
/* SLA7033 VRef cls */
/* 【注意】VRef を変更しただけでは 7033に電流が流れ続けるので、励磁信号に 0x00を出力すること */
DA.DR0= 0 ; /* D/A.C data cls */
/* SLA7033 励磁信号 cut_off */
PB.DR.BYTE= 0x00 ; /* SLA7033 励磁信号 set */
}
}
/*---------------------------------------
switch 関係 --------------------------------*/
/* push_switch のデータ入力 */
UCHAR push_switch(void) /* push_switch のデータ入力 */
{
/* start_switch(push_switch) is Active Low poat_9 bit_5 */
if ( (P9.DR.BYTE & 0x20) == 0x00) return(WAKE) ; /* WAKE is start_switch が押された */
else return(SLEEP) ; /* SLEEP is start_switch は開放されている */
}
/* dip_switch のデータ入力 */
UCHAR dip_switch(void) /* dip_switch のデータ入力 */
{
return( (~P4.DR.BYTE) & 0x0f ) ;
}
/*-----------------------------------------
LED 関係 --------------------------------*/
/* 赤外LED 発光停止 */
void ifrray_off(void) /* 赤外LED 発光停止 */
{
PA.DR.BYTE= 0x0f ; /* 赤外 led cut off active Low */
}
/* ----- ----- ----- ----- bit_3 bit_2 bit_1 bit_0
左前壁 左横壁 右横壁 右前壁 */
/* 壁data indication LED */
void wall_data_indication(UCHAR WallData) /* 壁data indication LED */
{
P5.DR.BYTE= ( (~(WallData)) & 0x0f ) ;
}
/* 壁data indication LED data読み込み */
UCHAR read_wall_data_indi(void) /* 壁data indication LED data読み込み */
{
return( ((~(P5.DR.BYTE)) & 0x0f) ) ;
}
/* interface LED 駆動 */
void led(UCHAR data) /* interface LED 駆動 */
{
P2.DR.BYTE= (~data) ;
}
/* interface LED data読み込み */
UCHAR read_led(void) /* interface LED data読み込み */
{
return( ~(P2.DR.BYTE) ) ;
}
/* LEDを３回点滅させる */
void led_blink(void) /* LEDを３回点滅させる */
{
led(0xff) ; /* all LED on */
long_wait(20,10000) ; /* 時間待ちの無駄演算 */
led(0x00) ; /* all LED off */
long_wait(20,10000) ;
led(0xff) ;
long_wait(20,10000) ;
led(0x00) ;
long_wait(20,10000) ;
led(0xff) ;
long_wait(20,10000) ;
led(0x00) ;
long_wait(20,10000) ;
}
/* LED を brinkしながら、push_switchの入力を待つ */
void push_sw_with_led(void) /* LED を brinkしながら、push_switchの入力を待つ */
{
UINT i ;
for (;;)
{
for( i=0; i<15; i++ ) {
led(led_data1[i]) ;
long_wait(5,10000) ; /* 時間待ちの無駄演算 */
}
/* チャタリングを防止するための時間稼ぎ */
if( push_switch() == WAKE ) long_wait(5,10000) ;
if( push_switch() == WAKE ) break ;
}
led(0x00) ;
}
/* 左壁横の最大値と右横壁の最小値を設定するシグナル */
void push_sw_led_Left(void) /* 左壁横の最大値と右横壁の最小値を設定するシグナル */
{
unsigned int i ;
for (;;)
{
for( i=0; i<35; i++ ) {
led(led_Left[i]) ;
long_wait(5,10000) ; /* 時間待ちの無駄演算 */
}
/* チャタリングを防止するための時間稼ぎ */
if( push_switch() == WAKE ) long_wait(5,10000) ;
if( push_switch() == WAKE ) break ;
}
led(0x00) ;
}
/* 左壁横と右横壁の中央値を設定するシグナル */
void push_sw_led_Center(void) /* 左壁横と右横壁の中央値を設定するシグナル */
{
unsigned int i ;
for (;;)
{
for( i=0; i<10; i++ ) {
led(led_Center[i]) ;
long_wait(5,10000) ; /* 時間待ちの無駄演算 */
}
/* チャタリングを防止するための時間稼ぎ */
if( push_switch() == WAKE ) long_wait(5,10000) ;
if( push_switch() == WAKE ) break ;
}
led(0x00) ;
}
/* 左壁横の最小値と右横壁の最大値を設定するシグナル */
void push_sw_led_Right(void) /* 左壁横の最小値と右横壁の最大値を設定するシグナル */
{
unsigned int i ;
for (;;)
{
for( i=0; i<35; i++ ) {
led(led_Right[i]) ;
long_wait(5,10000) ; /* 時間待ちの無駄演算 */
}
/* チャタリングを防止するための時間稼ぎ */
if( push_switch() == WAKE ) long_wait(5,10000) ;
if( push_switch() == WAKE ) break ;
}
led(0x00) ;
}
/* 迷路地図 クリア 警告シグナル cls_warning_sin */
void warning_sin(void) /* 迷路地図 クリア 警告シグナル cls_warning_sin */
{
led(0xaa) ;
for (;;)
{
long_wait(13,10000) ; /* 時間待ちの無駄演算 */
led(0x55) ;
wall_data_indication(0x06) ; /* 前壁data indication LED -----x11x */
long_wait(13,10000) ;
led(0xaa) ;
wall_data_indication(0x09) ; /* 横壁data indication LED -----1xx1 */
/* チャタリングを防止するための時間稼ぎ */
if( push_switch() == WAKE ) long_wait(5,10000) ;
if( push_switch() == WAKE ) break ;
}
/* 横壁data cut off */
wall_data_indication(0x00) ; /* 横壁data cut off */
led_blink() ; /* LEDを３回点滅させる */
}
/* 事故時 緊急シグナル emergency signal */
void warning_sin2(void) /* 事故時 緊急シグナル led が非常停止したことを知らせる */
{
for (;;)
{
led(0x18) ;
long_wait(8,10000) ; /* 時間待ちの無駄演算 */
led(0x24) ;
long_wait(7,10000) ;
led(0x42) ;
/* チャタリングを防止するための時間稼ぎ */
if( push_switch() == WAKE ) long_wait(5,10000) ;
if( push_switch() == WAKE ) break ;
long_wait(7,10000) ;
led(0x81) ;
long_wait(7,10000) ;
led(0x00) ;
long_wait(7,10000) ;
/* チャタリングを防止するための時間稼ぎ */
if( push_switch() == WAKE ) long_wait(5,10000) ;
if( push_switch() == WAKE ) break ;
}
led_blink() ; /* LEDを３回点滅させる */
}
void data_read_start_sig(void) /* データ入力開始 シグナル */
{
for (;;)
{
led(0x81) ;
long_wait(8,10000) ; /* 時間待ちの無駄演算 */
led(0x42) ;
long_wait(7,10000) ;
led(0x24) ;
/* チャタリングを防止するための時間稼ぎ */
if( push_switch() == WAKE ) long_wait(5,10000) ;
if( push_switch() == WAKE ) break ;
long_wait(7,10000) ;
led(0x18) ;
long_wait(7,10000) ;
led(0x00) ;
long_wait(7,10000) ;
/* チャタリングを防止するための時間稼ぎ */
if( push_switch() == WAKE ) long_wait(5,10000) ;
if( push_switch() == WAKE ) break ;
}
led_blink() ; /* LEDを３回点滅させる */
}
void data_read_end_sig(void) /* データ入力終了 シグナル */
{
for (;;)
{
led(0x03) ;
long_wait(25,10000) ; /* 時間待ちの無駄演算 */
led(0xc0) ;
long_wait(25,10000) ;
/* チャタリングを防止するための時間稼ぎ */
if( push_switch() == WAKE ) long_wait(5,10000) ;
if( push_switch() == WAKE ) break ;
}
led_blink() ; /* LEDを３回点滅させる */
}
