人それぞれに走り方はある、人それぞれに調整のやり方はある。参考にならないだろうが、これが私の方法。
自作迷路用のデータは私の迷路専用だが、取り外すのも面倒なので入れてある。以下のプログラムのとおり。
/* program_name "mouse.c" made by k.notoya */
/* 山形支部標準マウス メインプログラム */
/* last edit 1998/09/10 k.notoya */
/* ＣＰＵmode is mode_7 */
#include "d:\H83048File\3048f.h" /* ＭＣＲ版 Ｈ８cpu のレジスタを定義したヘッダファイル */
/* #include "d:\H83048File\machine.h" */ /* 割り込み設定 etc */
#include "d:\H83048File\stdio.h" /* Ｈ８ standerd I/O hedder */
#include "d:\standardDef.h" /* 標準的な定数data ＆ インターフェイス・制御系のマクロ */
#include "d:\constant.h" /* 定数data （走行速度、走行距離、姿勢制御量） */
#include "d:\global.h" /* 全モジュール共通 広域変数 */
/* #include "d:\mouselib.h" */ /* マイクロマウスのための関数のプロトタイプ宣言 */
void front_walldata_indicate(void) ; /* 前壁data 表示 */
void side_walldata_indicate(void) ; /* 横壁data 表示 */
void side_wall_data_init(void) ; /* 横壁データの最大値、中央値、最小値を記憶する */
void front_wall_data_init(void) ; /* 前壁データを記憶する センサ分解能 1.5167mm */
void reset_side_wall_data(void) ; /* start時に、横壁データの中央値を resetする */
void start_position_set(void) ; /* start位置を迷路の中央に合わせる */
void temporary_wall_data_set(void) ; /* 壁dataを仮設定 */
void emergency_teratment(void) ; /* 緊急停止後の処理(再初期化) */
void main(void) ;
/* 前壁dataを 表示する */
void front_walldata_indicate(void) /* 前壁data 表示 */
{
rs_send_sentence("\r\n") ; rs_send_sentence("\r\n") ; /* 最初は ２行空ける */
rs_send_sentence("\r\n"
" --------------- ATTENTION -------------- \r\n"
" if start switch is pushed then Front_wall data indicate \r\n"
) ;
rs_send_sentence("\r\n") ; /* １行 空ける */
push_sw_with_led() ; /* LED を brinkしながら、push_switchの入力を待つ */
led_blink() ; /* LEDを３回点滅させる */
/* 前壁data 最小値 表示 */
rs_send_sentence(" left_front_min right_front_min ") ; rs_send_sentence("\r\n") ;
rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(left_front_min) ) ; rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(right_front_min) ) ;
rs_send_sentence("\r\n") ; /* １行 空ける */
/* 前壁data 中央値 表示 */
rs_send_sentence(" left_front_mid right_front_mid ") ; rs_send_sentence("\r\n") ;
rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(left_front_mid) ) ; rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(right_front_mid) ) ;
rs_send_sentence("\r\n") ; /* １行 空ける */
/* 前壁data 最大値 表示 */
rs_send_sentence(" left_front_max right_front_max ") ; rs_send_sentence("\r\n") ;
rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(left_front_max) ) ; rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(right_front_max) ) ;
rs_send_sentence("\r\n") ; rs_send_sentence("\r\n") ; /* 最後は ２行空ける */
}
/* 横壁dataを 表示する */
void side_walldata_indicate(void) /* 横壁data 表示 */
{
rs_send_sentence("\r\n") ; rs_send_sentence("\r\n") ; /* 最初は ２行空ける */
rs_send_sentence("\r\n"
" --------------- ATTENTION -------------- \r\n"
" if start switch is pushed then Side_wall data indicate \r\n"
) ;
rs_send_sentence("\r\n") ; /* １行 空ける */
push_sw_with_led() ; /* LED を brinkしながら、push_switchの入力を待つ */
led_blink() ; /* LEDを３回点滅させる */
/* 左横壁 最大値 ＆ 右横壁 最小値 表示 */
rs_send_sentence(" left_side_min right_side_min ") ; rs_send_sentence("\r\n") ;
rs_send_sentence(" ") ; /* 表示の位置合わせ */
Hex16DeciSend( (unsigned short int )(left_side_min) ) ; rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(right_side_min) ) ;
rs_send_sentence("\r\n") ; /* １行 空ける */
/* 左横壁 ＆ 右横壁 中央値 表示 */
rs_send_sentence(" left_side_mid right_side_mid ") ; rs_send_sentence("\r\n") ;
rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(left_side_mid) ) ; rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(right_side_mid) ) ;
rs_send_sentence("\r\n") ;
/* 左横壁 最小値 ＆ 右横壁 最大値 表示 */
rs_send_sentence(" left_side_max right_side_max ") ; rs_send_sentence("\r\n") ;
rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(left_side_max) ) ; rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(right_side_max) ) ;
rs_send_sentence("\r\n") ; rs_send_sentence("\r\n") ; /* 最後は ２行空ける */
}
/* 停止状態で、横壁データの最大値、中央値 ＆ 最小値を記憶する */
void side_wall_data_init(void) /* 横壁データの最大値、中央値、最小値を記憶する */
{ /* 最初は左壁の最大値と右壁の最小値を入力 */
/* 初期設定 */
revision_control(SLEEP) ; /* 姿勢制御 禁止 */
interrupt_control(SLEEP) ; /* 割り込み 禁止 */
ifrray_off() ; /* 赤外LED 発光停止 */
led(0x00) ; /* ｲﾝﾀｰﾌｪｲｽ LED cut off */
power(SLEEP) ; /* 操舵用及び駆動用 電力 停止 */
rs_send_sentence("\r\n"
" --------------- attention -------------- \r\n"
" Side_wall data initialize without running \r\n"
" ******************************************************* \r\n"
) ;
/* データ入力開始 シグナル */
data_read_start_sig() ; /* データ入力開始 シグナル */
/* 左壁横の最大値と右横壁の最小値を設定する */
push_sw_led_Left() ; /* 左壁横の最大値と右横壁の最小値を設定する シグナル */
led_blink() ; /* LEDを３回点滅させる */
/* 左横壁 最大値 left_side_max 設定 */
/* left_side_max= analog[LEFT_SIDE] ; */
left_side_max= (int )(adcread_Phot(LEFT_SIDE) ) ;
/* 右横壁 最小値 right_side_min 設定 */
right_side_min= (int )(adcread_Phot(RIGHT_SIDE) ) ;
/* 左壁横の最大値 ＆ 右横壁の最小値を表示 */
rs_send_sentence("\r\n") ; rs_send_sentence("\r\n") ; /* 最初は、２行空ける */
rs_send_sentence(" left_side_max right_side_min ") ; rs_send_sentence("\r\n") ;
rs_send_sentence(" ") ; /* 表示の位置合わせ */
Hex16DeciSend( (unsigned short int )(left_side_max) ) ; rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(right_side_min) ) ;
rs_send_sentence("\r\n") ; /* １行 空ける */
/* 左壁横 最大値 ＆ 右横壁 最小値 設定終了 */
data_read_end_sig() ; /* データ入力終了 シグナル */
/*-------------------
この間にマウスを移動させる
---------------*/
/* 左壁横と右横壁の中央値を設定する */
/* data_read_start_sig() ; */ /* データ入力開始 シグナル */
push_sw_led_Center() ; /* 左壁横と右横壁の中央値を設定するシグナル */
led_blink() ; /* LEDを３回点滅させる */
/* 左横壁 中央値 left_side_mid 設定 */
left_side_mid= (int )(adcread_Phot(LEFT_SIDE) ) ;
/* 右横壁 中央値 right_side_mid 設定 */
right_side_mid= (int )(adcread_Phot(RIGHT_SIDE) ) ;
/* 左壁横 ＆ 右横壁の中央値を表示 */
rs_send_sentence(" left_side_mid right_side_mid ") ; rs_send_sentence("\r\n") ;
rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(left_side_mid) ) ; rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(right_side_mid) ) ;
rs_send_sentence("\r\n") ;
/* 左壁横 中央値 ＆ 右横壁 中央値 設定終了 */
data_read_end_sig() ; /* データ入力終了 シグナル */
/*-------------------
この間にマウスを移動させる
---------------*/
/* 左壁横の最小値と右横壁の最大値を設定する */
/* data_read_start_sig() ; */ /* データ入力開始 シグナル */
push_sw_led_Right() ; /* 左壁横の最小値と右横壁の最大値を設定するシグナル */
led_blink() ; /* LEDを３回点滅させる */
/* data転送 */
/* 左横壁 最小値 left_side_min 設定 */
left_side_min= (int )(adcread_Phot(LEFT_SIDE) ) ;
/* 右横壁 最大値 right_side_max 設定 */
right_side_max= (int )(adcread_Phot(RIGHT_SIDE) ) ;
/* 左壁横の最小値 ＆ 右横壁の最大値を表示 */
rs_send_sentence(" left_side_min right_side_max ") ; rs_send_sentence("\r\n") ;
rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(left_side_min) ) ; rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(right_side_max) ) ;
rs_send_sentence("\r\n") ; rs_send_sentence("\r\n") ; /* 最後は２行 空ける */
/* 左壁横 最小値 ＆ 右横壁 最大値 設定終了 */
data_read_end_sig() ; /* データ入力終了 シグナル */
}
/* 前壁データを記憶する */
void front_wall_data_init(void) /* 前壁データを記憶する */
{
SSHORT counter ; /* 走行距離 counter */
/* 初期設定 */
revision_control(SLEEP) ; /* 姿勢制御 禁止 */
interrupt_control(SLEEP) ; /* 割り込み 禁止 */
ifrray_off() ; /* 赤外LED 発光停止 */
led(0x00) ; /* ｲﾝﾀｰﾌｪｲｽ LED cut off */
power(SLEEP) ; /* 操舵用及び駆動用 電流制御 */
rs_send_sentence("\r\n"
" --------------- attention -------------- \r\n"
" Front_wall data initiialize with running \r\n"
" ******************************************************* \r\n"
) ;
/* 変数とフラグを初期化 */
map_renewal= SLEEP ; /* if map_renewal is WAKE then 迷路地図を更新する */
deside_flag= SLEEP ; /* if deside_flag is WAKE then call deside direction */
running_condition= HALT ; /* mouseは停止していた */
turn_direction= STRAIGHT ; /* mouseは直進する */
accele_condition= ACCELE ; /* 速度flag reset */
/* 前壁data読み込み シグナル */
data_read_start_sig() ; /* データ入力開始 シグナル */
led_blink() ; /* LEDを３回点滅させる */
/* mouseが後退するので、loopを使って(割り込みを使わない)走行する */
/* SLA7033 PWM基準電圧設定 */
current= TANSA_CURRENT ; /* motor の電流値(A/D.Cに書きこむ値) */
power(WAKE) ; /* 操舵用及び駆動用 電流制御 */
/* 走行距離設定 １−２相励磁 250mm 後退 250mm/0.41mm= 609.7ﾊﾟﾙｽ */
for ( counter= 610; counter >= 0; counter= counter- 1 )
{
/* 左motor励磁data adress 更新 */
if ( Left_Reiji > 0 ) Left_Reiji= Left_Reiji- 1 ;
else Left_Reiji= 7 ;
/* 右motor励磁data adress 更新 */
if ( Right_Reiji > 0 ) Right_Reiji= Right_Reiji- 1 ;
else Right_Reiji= 7 ;
/* 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) ) ;
/* 後退の速度を設定(時間稼ぎの無駄演算) */
wait(3000) ; /* 後退の速度を設定(時間稼ぎの無駄演算) */
}
/* mouseは設定距離を走行した 前進方向に励磁する */
long_wait(10,10000) ;
/* 励磁data 更新 前進方向に励磁する */
Left_Reiji= Left_Reiji+ 1 ; /* 前進方向に励磁する */
Right_Reiji= Right_Reiji+ 1 ;
/* 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) ) ;
long_wait(50,10000) ;
/* SLA7033 PWM基準電圧設定 */
power(SLEEP) ; /* 操舵用及び駆動用 電流制御 */
led_blink() ; /* LEDを３回点滅させる */
/*-------------------------------------------
前進しながら、前壁dataを読みこむ
-------------------------------------------------------*/
/* 変数とフラグを初期化 */
map_renewal= WAKE ; /* if map_renewal is WAKE then 迷路地図を更新する */
deside_flag= WAKE ; /* if deside_flag is WAKE then call deside direction */
running_condition= HALT ; /* mouseは停止していた */
turn_direction= STRAIGHT ; /* mouseは直進する */
accele_condition= ACCELE ; /* 速度flag reset */
/* 走行距離設定 １−２相励磁 250mm 後退 250mm/0.41mm= 609.7ﾊﾟﾙｽ */
mark_distance= 610 ; /* 基本走行パルス数 set(error_pulseによる補正) */
position_now= mark_distance ; /* position_now is ﾀﾞｳﾝ･ｶｳﾝﾄ(残りの走行ﾊﾟﾙｽ数) */
/* 走行速度設定 ＆ 加減速rate reset */
accele_pulse= ( mark_distance- 30 ) ; /* 加速ﾊﾟﾙｽ数 set */
reduce_pulse= 45 ; /* 減速ﾊﾟﾙｽ数 set 加速ﾊﾟﾙｽ数の1/2ﾊﾟﾙｽ 減速する */
/* mouse の走行速度を設定する */
speed_select= speed_1 ; /* speed_select is 加速テーブルadressの offset(初速) */
adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* SLA7033 PWM基準電圧設定 */
current= TANSA_CURRENT ; /* motor の電流値(A/D.Cに書きこむ値) */
power(WAKE) ; /* 操舵用及び駆動用 電流制御 */
/* 全割り込み、マスク解除 */
interrupt_control(WAKE) ; /* 割り込み 禁止 or 許可 */
/* センサ入力start ＆ 姿勢制御 禁止 */
revision_control(SLEEP) ; /* 姿勢制御 禁止 */
/* 設定距離を走る 250mm(610ﾊﾟﾙｽ) 前進 */
/* 前壁データ基準値 設定 */
/* 車軸中心から先端までの長さ 約 65mm */
/* 前壁の有無 (180mm+ 45mm)- 65mm is 160mm 160mm/0.41mm 390.2 ﾊﾟﾙｽの位置 */
/* ｽﾗﾛｰﾑ進入位置 (180mm- 6mm)- 65mm is 109mm 109mm/0.41mm 265.9 ﾊﾟﾙｽの位置 */
/* 迷路の中心(緊急停止) ( 90mm- 6mm)- 65mm is 19mm 19mm/0.41mm 39.0 ﾊﾟﾙｽの位置 */
while (position_now >= 5) {
/* 前壁dataを 入力する */
/* 前壁判定位置 */
if (position_now >= 400) {
left_front_min= analog[LEFT_FRONT] ;
right_front_min= analog[RIGHT_FRONT] ; }
/* slalom 侵入位置 */
else if (position_now >= 266) {
left_front_mid= analog[LEFT_FRONT] ;
right_front_mid= analog[RIGHT_FRONT] ; }
/* 絶対停止位置(迷路の中心) */
else if (position_now >= 39) {
left_front_max= analog[LEFT_FRONT] ;
right_front_max= analog[RIGHT_FRONT] ; }
/* 走行中に前壁のdataを転送する */
rs_send_sentence(" position_now left_front right_front ") ;
rs_send_sentence("\r\n") ; /* １行の終わり */
rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(position_now) ) ; /* position_now is 残りの走行パルス数 */
rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(analog[LEFT_FRONT]) ) ; /* 左・前壁data */
rs_send_sentence(" ") ;
Hex16DeciSend( (unsigned short int )(analog[RIGHT_FRONT]) ) ; /* 右・前壁data */
rs_send_sentence("\r\n") ; rs_send_sentence("\r\n") ;
}
/* 設定距離を走行した マウスを止める */
distance_compare= 0x00 ; /* distance_compare is 残りの走行距離 */
count_distance() ; /* mouseは設定距離を走行する */
/* センサ入力 停止 ＆ 姿勢制御 禁止 */
revision_control(SLEEP) ; /* センサ入力 停止 ＆ 姿勢制御 禁止 */
/* 設定距離を走行した マウスを止める */
interrupt_control(SLEEP) ; /* 割り込み 禁止 or 許可 */
ifrray_off() ; /* 赤外LED 発光停止 */
led(0x00) ; /* ｲﾝﾀｰﾌｪｲｽ LED cut off */
/* mouseは停止した 慣性が消えるまで待つ */
long_wait(10,10000) ;
reverse() ; /* 進行方向(旋回方向)と反対向きに励磁し、強制的に停止させる */
long_wait(90,10000) ;
/* SLA7033 PWM基準電圧設定 */
power(SLEEP) ; /* 操舵用及び駆動用 電流制御 */
/* 前壁data 設定終了 */
data_read_end_sig() ; /* データ入力終了 シグナル */
led_blink() ; /* LEDを３回点滅させる */
}
/* start時に、横壁データの中央値を resetする */
void reset_side_wall_data(void) /* start時に、横壁データの中央値を resetする */
{
unsigned char i,j ;
/* 初期設定 */
revision_control(SLEEP) ; /* 姿勢制御 禁止 */
interrupt_control(SLEEP) ; /* 割り込み 禁止 */
ifrray_off() ; /* 赤外LED 発光停止 */
led(0x00) ; /* ｲﾝﾀｰﾌｪｲｽ LED cut off */
power(SLEEP) ; /* 操舵用及び駆動用 電流制御 */
/* 左壁横と右横壁の中央値を設定するシグナル */
for (i= 0x00 ; i < 3; i++) {
for( j=0; j<10; j++ ) {
led(led_Center[j]) ;
long_wait(5,10000) ; }
}
/* 左壁横と右横壁の中央値を設定する */
/* 左横壁 中央値 left_side_mid 設定 */
left_side_mid= (int )(adcread_Phot(LEFT_SIDE) ) ;
long_wait(10,10000) ;
/* 右横壁 中央値 right_side_mid 設定 */
right_side_mid= (int )(adcread_Phot(RIGHT_SIDE) ) ;
led_blink() ; /* LEDを３回点滅させる */
/* 左壁横と右横壁の中央値を設定する */
/* 左横壁 中央値 left_side_mid 設定 */
left_side_mid= (int )(adcread_Phot(LEFT_SIDE) ) ;
long_wait(10,10000) ;
/* 右横壁 中央値 right_side_mid 設定 */
right_side_mid= (int )(adcread_Phot(RIGHT_SIDE) ) ;
/* データ入力終了 シグナル */
for (i= 0x00 ; i < 3; i++) {
led(0x03) ;
long_wait(25,10000) ; /* 時間待ちの無駄演算 */
led(0xc0) ;
long_wait(25,10000) ;
}
}
/* start位置を迷路の中央に合わせる */
void start_position_set(void) /* start位置を迷路の中央に合わせる */
{
SSHORT counter ; /* 走行距離 counter */
/* 初期設定 */
revision_control(SLEEP) ; /* 姿勢制御 禁止 */
interrupt_control(SLEEP) ; /* 割り込み 禁止 */
ifrray_off() ; /* 赤外LED 発光停止 */
led(0x00) ; /* ｲﾝﾀｰﾌｪｲｽ LED cut off */
power(SLEEP) ; /* 操舵用及び駆動用 電流制御 */
/* 変数とフラグを初期化 */
map_renewal= SLEEP ; /* if map_renewal is WAKE then 迷路地図を更新する */
deside_flag= SLEEP ; /* if deside_flag is WAKE then call deside direction */
running_condition= HALT ; /* mouseは停止していた */
turn_direction= STRAIGHT ; /* mouseは直進する */
accele_condition= ACCELE ; /* 速度flag reset */
/* mouseが後退するので、loopを使って(割り込みを使わない)走行する */
/* SLA7033 PWM基準電圧設定 */
current= TANSA_CURRENT ; /* motor の電流値(A/D.Cに書きこむ値) */
power(WAKE) ; /* 操舵用及び駆動用 電流制御 */
/* 走行距離設定 １−２相励磁 10mm 後退 10mm/0.41mm= 24.39ﾊﾟﾙｽ */
for ( counter= 25; counter >= 0; counter= counter- 1 )
{
/* 左motor励磁data adress 更新 */
if ( Left_Reiji > 0 ) Left_Reiji= Left_Reiji- 1 ;
else Left_Reiji= 7 ;
/* 右motor励磁data adress 更新 */
if ( Right_Reiji > 0 ) Right_Reiji= Right_Reiji- 1 ;
else Right_Reiji= 7 ;
/* 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) ) ;
/* 後退の速度を設定(時間稼ぎの無駄演算) */
wait(3000) ; /* 後退の速度を設定(時間稼ぎの無駄演算) */
}
/* mouseは設定距離を走行した 前進方向に励磁する */
long_wait(10,10000) ;
/* 励磁data 更新 前進方向に励磁する */
Left_Reiji= Left_Reiji+ 1 ; /* 前進方向に励磁する */
Right_Reiji= Right_Reiji+ 1 ;
/* 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) ) ;
long_wait(50,10000) ;
/* SLA7033 PWM基準電圧設定 */
power(SLEEP) ; /* 操舵用及び駆動用 電流制御 */
led_blink() ; /* LEDを３回点滅させる */
/* 【注意】次の1歩目を設定距離よりも多く走り、迷路の中心で止まらない */
/* 迷路の中心まで前進するﾊﾟﾙｽ数を設定(走行距離に加算するﾊﾟﾙｽ数) */
error_pulse= CENTER_ADJUST ; /* mouseの後端が壁に接触した位置から、前進するﾊﾟﾙｽ数 */
/* 走行準備完了の合図をする */
led_blink() ; /* LEDを３回点滅させる */
led(0x00) ; /* all led cut_off */
}
/* 壁dataを仮設定 */
void temporary_wall_data_set(void) /* 壁dataを仮設定 */
{
/* 右・横壁dataを仮設定 */
right_side_max= Right_Side_MAX, right_side_mid= Right_Side_MID, right_side_min= Right_Side_MIN ;
/* 右・前壁dataを仮設定 */
right_front_max= Right_Front_MAX, right_front_mid= Right_Front_MID, right_front_min= Right_Front_MIN ;
/* 左・前壁dataを仮設定 */
left_front_max= Left_Front_MAX, left_front_mid= Left_Front_MID, left_front_min= Left_Front_MIN ;
/* 左・横壁dataを仮設定 */
left_side_max= Left_Side_MAX, left_side_mid= Left_Side_MID, left_side_min= Left_Side_MIN ;
}
/* 緊急停止後の処理(再初期化) */
void emergency_teratment(void) /* 緊急停止後の処理(再初期化) */
{
char escape ; /* A/D.C channel 退避エリア */
/* 初期設定 */
/* H8cpu_init() ; */ /* Ｈ８cpu 初期化 */
revision_control(SLEEP) ; /* 姿勢制御 禁止 */
interrupt_control(SLEEP) ; /* 割り込み 禁止 */
ifrray_off() ; /* 赤外LED 発光停止 */
led(0x00) ; /* ｲﾝﾀｰﾌｪｲｽ LED cut off */
power(SLEEP) ; /* 操舵用及び駆動用 電流制御 */
/* 座標の初期化 緊急停止をしたので、座標と方向を reset する */
axis= 0x00 ; /* mouse は start 地点に戻った */
dir= 0x00 ; /* mouse の 現在の絶対方向は 北 */
axis_x= 0x00 ; /* 未探査の座標は start地点の座標に仮設定 */
dir_x= 0x00 ; /* 未探査の座標のmouseの絶対方向は 北 に仮設定 */
/* crisis() ; */ /* 探査中に走行に失敗したときの処理 */
/* 変数とフラグを初期化 */
/* turn_direction= STRAIGHT ; */ /* mouseは直進する */
/* turn_direction= SPIN_LEFT ; */ /* 旋回方向は左 */
/* turn_direction= SPIN_RIGHT ; */ /* 旋回方向は右 */
/* turn_direction= REVERS_SPIN ; */ /* 右１８０度 反転 */
/* map_renewal= WAKE ; */ /* if map_renewal is WAKE then 迷路地図を更新する */
/* deside_flag= WAKE ; */ /* if deside_flag is WAKE then call deside direction */
accele_condition= ACCELE ; /* 速度flag reset */
running_condition= HALT ; /* mouseは停止していた */
/* mouse の走行速度を設定する */
speed_select= speed_0 ; /* speed_select is 加速テーブルadressの offset(初速) */
adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* ウオッチドッグタイマ ＯＶＦflag reset */
escape= TCSRR ; /* TCSR 下位 ８bit data 読み込み */
escape= escape & 0x7f ; /* 0x7f is 0xxx-xxxx OVF flag cls */
TCSRW= ( 0xa500 | ((int )escape) ) ; /* 0xa5xx is TCSR 制御語 */
/* TCSRW= ( 0xa500 | ((int )( (TCSRR & 0x7f) )) ) ; */ /* 0xa5xx is TCSR 制御語 */
led(0x00) ;
}
void main(void)
{
UCHAR i,j ;
UCHAR channel ;
unsigned int counter, counter2 ;
unsigned int result ; /* 変換結果格納用変数 */
/* 初期設定 */
revision_control(SLEEP) ; /* 姿勢制御 禁止 */
interrupt_control(SLEEP) ; /* 割り込み 禁止 */
ifrray_off() ; /* 赤外LED 発光停止 */
led(0x00) ; /* ｲﾝﾀｰﾌｪｲｽ LED cut off */
power(SLEEP) ; /* 操舵用及び駆動用 電流制御 */
/* Ｈ８cpu 初期化 */
H8cpu_init() ; /* Ｈ８cpu 初期化 ＆ ＩＴＵタイマstart */
revision_control(SLEEP) ; /* 姿勢制御 禁止 */
interrupt_control(SLEEP) ; /* 割り込み 禁止 */
ifrray_off() ; /* 赤外LED 発光停止 */
led(0x00) ; /* ｲﾝﾀｰﾌｪｲｽ LED cut off */
power(SLEEP) ; /* 操舵用及び駆動用 電流制御 */
/* mouse の走行速度を設定する */
speed_select= speed_1 ; /* speed_select is 加速テーブルadressの offset(初速) */
adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* 励磁data(励磁パターン)配列を、ルーチンの最初に、1回だけ初期化する */
Right_Reiji= 0 ; /* 励磁data配列変数の添字を、初期値(0x00)に戻す */
Left_Reiji= 0 ;
/* ＣＰＵモードの表示 ＆ シリアル通信 作動確認 */
rs_send_sentence("\r\n") ; rs_send_sentence("\r\n") ;
rs_send_sentence(" CPU mode is ") ;
Hex16DeciSend( (unsigned short int )(MDCR.BYTE & 0x07) ) ; /* 16bit binary --> ASCII --> 転送 */
rs_send_sentence("\r\n") ; /* １行の終わり */
/* INT dataのバス幅 確認 */
rs_send_sentence(" INT data size is ") ;
Hex16DeciSend( (unsigned short int )(sizeof(int)) ) ;
rs_send_sentence(" byte( 1 byte is 8 bit) ") ;
rs_send_sentence("\r\n"), send_end() ; /* １行の終わり ＆ １行 空ける */
preparation_communicat() ; /* シリアル通信 作動確認 */
/* 初期化 終了 Ｈ８cpu 作動 準備完了 start_switchの入力を待つ */
push_sw_with_led() ; /* LED を brinkしながら、push_switchの入力を待つ */
led_blink() ; /* LEDを３回点滅させる */
/* 走行mode 表示 ＆ 確認 */
/* led(mode&0x0f) ; */ /* 走行mode 表示 */
led( (dip_switch()&0x0f) ) ; /* 走行mode 表示 */
do {
wall_data_indication(0x06) ; /* 前壁data indication LED -----x11x */
long_wait(20,10000) ;
wall_data_indication(0x09) ; /* 横壁data indication LED -----1xx1 */
long_wait(20,10000) ;
} while ( push_switch() != WAKE ) ;
wall_data_indication(0x00) ; /* wall_data indication LED cut_off */
led(0x00) ; /* 走行mode 表示 end ｲﾝﾀｰﾌｪｲｽ LED cut off */
/* 走行開始 無限loop */
for (;;) {
/* 走行準備 完了 dip_switch のdata確認 */
push_sw_with_led() ; /* LED is blink for push_sw on */
led_blink() ; /* LEDを３回点滅させる */
switch ( (dip_switch()&0x0f) ) /* dip_switch のデータ入力 */
{
case 0 : /* 重ね探査のため、１回だけ迷路地図を初期化する */
led(0x00) ; /* all led cut_off */
warning_sin() ; /* 迷路地図 クリア 警告シグナル cls_warning_sin */
/* 迷路地図の初期化 ＆ 壁data転送の再確認 */
if ((dip_switch() & 0x0f) != 0x00) break ; /* 再入力 */
/* 迷路地図の初期化 全区画 壁無し ＆ 未探査 */
wall_cls() ; /* 迷路地図の初期化 全区画 壁無し ＆ 未探査 */
/* 記憶しておいた壁dataを変数(memory)に転送する */
temporary_wall_data_set() ; /* 壁dataを仮設定 */
/* 走行modeの初期化 mode_1 is start --> goal --> start */
init_1() ; /* 走行modeの初期化 mode_1 is start --> goal --> start */
data_read_end_sig() ; /* データ入力終了 シグナル */
break ;
/* 横壁データの最大値、中央値、最小値を記憶する */
case 1 : /* 横壁データの最大値、中央値、最小値を記憶する */
led(0x00) ; /* all led cut_off */
side_wall_data_init() ; /* 横壁データの最大値、中央値、最小値を記憶する */
break ;
/* 前壁データを記憶する */
case 2 : /* 前壁データを記憶する */
led(0x00) ; /* all led cut_off */
/* 走行モードの初期化 start --> goal --> start */
running_mode= SEARCH ; /* mouseは探査走行を行う */
front_wall_data_init() ; /* 前壁データを記憶する センサ分解能 1.5167mm */
break ;
/* 探査走行 */
case 3 : /* 探査走行 開始 */
/* 横壁データの中央値を reset ＆＆ start位置を合わせる */
reset_side_wall_data() ; /* start時に、横壁データの中央値を resetする */
start_position_set() ; /* start位置を迷路の中央に合わせる */
/* mouse の走行速度を設定する */
speed_select= speed_0 ; /* speed_select is 加速テーブルadressの offset(初速) */
adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* 走行モードの初期化 start --> goal --> start */
running_mode= SEARCH ; /* mouseは探査走行を行う */
init_1() ; /* 探査走行 変数の初期化 start --> goal --> start */
/* SLA7033 PWM基準電圧設定 */
current= TANSA_CURRENT ; /* motor の電流値(A/D.Cに書きこむ値) */
current_flows= current ; /* motor の電流値(A/D.Cに書きこむ値)を記憶 */
/* 探査走行 main loop */
first() ; /* 探査走行 主プログラム */
break ;
/* slalom旋廻 高速走行 */
case 4 :
/* 横壁データの中央値を reset ＆＆ start位置を合わせる */
reset_side_wall_data() ; /* start時に、横壁データの中央値を resetする */
start_position_set() ; /* start位置を迷路の中央に合わせる */
/* mouse の走行速度を設定する */
speed_select= speed_0 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* speed_select= speed_1 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* speed_select= speed_2 ; /* speed_select is 加速テーブルadressの offset(初速) */
adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* 走行モードの初期化 start --> goal --> start */
running_mode= ATTACK_SLA1 ; /* time attack slalom 定常走行 */
init_2() ; /* ﾏｳｽは start --> goal を走行する */
/* SLA7033 PWM基準電圧設定 */
current= TANSA_CURRENT ; /* motor の電流値(A/D.Cに書きこむ値) */
current_flows= current ; /* motor の電流値(A/D.Cに書きこむ値)を記憶 */
/* slalom旋回 高速走行 */
slalom() ; /* slalom旋回 高速走行 */
/* ﾏｳｽを goalから start地点まで戻す */
/* mouse の走行速度を設定する */
/* speed_select= speed_1 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* 走行モードの初期化 start --> goal --> start */
/* running_mode= ATTACK_SLA1 ; /* time attack slalom 低速走行 */
/* init_3() ; /* ﾏｳｽは goalから start地点まで戻る */
/* slalom旋回 高速走行 */
/* slalomLow() ; /* slalom旋回 高速走行 */
break ;
/* slalom 斜め走行 */
case 5 :
/* 横壁データの中央値を reset ＆＆ start位置を合わせる */
reset_side_wall_data() ; /* start時に、横壁データの中央値を resetする */
start_position_set() ; /* start位置を迷路の中央に合わせる */
/* mouse の走行速度を設定する */
speed_select= speed_1 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* speed_select= speed_2 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* speed_select= speed_3 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* speed_select= speed_4 ; /* speed_select is 加速テーブルadressの offset(初速) */
adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* 走行モードの初期化 start --> goal --> start */
running_mode= ATTACK_ASL1 ; /* time attack slalom 斜め走行 */
init_2() ; /* ﾏｳｽは start --> goal を走行する */
/* slalom 斜め走行の dataを 造る */
short_d() ; /* slalom 斜め走行の dataを 造る */
short_d2() ; /* ｽﾗﾛｰﾑ 斜め走行の dataを 造る No_2 */
/* SLA7033 PWM基準電圧設定 */
current= TANSA_CURRENT ; /* motor の電流値(A/D.Cに書きこむ値) */
current_flows= current ; /* motor の電流値(A/D.Cに書きこむ値)を記憶 */
/* slalom旋回 高速走行 */
slalom() ; /* slalom旋回 高速走行 */
/* ﾏｳｽを goalから start地点まで戻す */
/* mouse の走行速度を設定する */
/* speed_select= speed_1 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* 走行モードの初期化 start --> goal --> start */
/* running_mode= ATTACK_SLA1 ; /* time attack slalom 低速走行 */
/* init_3() ; /* ﾏｳｽは goalから start地点まで戻る */
/* slalom旋回 高速走行 */
/* slalomLow() ; /* slalom旋回 高速走行 */
break ;
/* slalom斜め走行試験 基準クロック ２分周 */
case 13 :
/* ＩＴＵの再設定 走行試験用 クロックを２分周する */
itu_reset_low() ; /* ＩＴＵの再設定 走行試験用 クロックを２分周する */
/* 横壁データの中央値を reset ＆＆ start位置を合わせる */
reset_side_wall_data() ; /* start時に、横壁データの中央値を resetする */
start_position_set() ; /* start位置を迷路の中央に合わせる */
/* mouse の走行速度を設定する */
/* speed_select= speed_1 ; /* speed_select is 加速テーブルadressの offset(初速) */
speed_select= speed_2 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* speed_select= speed_3 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* speed_select= speed_4 ; /* speed_select is 加速テーブルadressの offset(初速) */
adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* 走行モードの初期化 start --> goal --> start */
running_mode= ATTACK_ASL1 ; /* time attack slalom 定常走行 */
/* init_2() ; */ /* ﾏｳｽは start --> goal を走行する */
/* 連続ｽﾗﾛｰﾑ試験用 最短ｺｰｽ data */
route_map[0x00]= 0x84, route_map[0x01]= 0x21 ; /* 4 R */
route_map[0x02]= 0x80, route_map[0x03]= 0x21 ; /* 0 R */
route_map[0x04]= 0x80, route_map[0x05]= 0x31 ; /* 0 L */
route_map[0x06]= 0x80, route_map[0x07]= 0x21 ; /* 0 R */
route_map[0x08]= 0x81, route_map[0x09]= 0x31 ; /* 1 L 5 */
route_map[0x0a]= 0x80, route_map[0x0b]= 0x21 ; /* 0 R */
route_map[0x0c]= 0x81, route_map[0x0d]= 0x31 ; /* 1 L */
route_map[0x0e]= 0x80, route_map[0x0f]= 0x31 ; /* 0 L */
route_map[0x10]= 0x80, route_map[0x11]= 0x21 ; /* 0 R */
route_map[0x12]= 0x80, route_map[0x13]= 0x31 ; /* 0 L 10 */
route_map[0x14]= 0x80, route_map[0x15]= 0x31 ; /* 0 L */
route_map[0x16]= 0x80, route_map[0x17]= 0x21 ; /* 0 R */
route_map[0x18]= 0x81, route_map[0x19]= 0x21 ; /* 1 R */
route_map[0x1a]= 0x80, route_map[0x1b]= 0x31 ; /* 0 L */
route_map[0x1c]= 0x80, route_map[0x1d]= 0x21 ; /* 0 R 15 */
route_map[0x1e]= 0x80, route_map[0x1f]= 0x31 ; /* 0 L */
route_map[0x20]= 0x80, route_map[0x21]= 0xff ; /* 0 ｺﾞｰﾙ */
short_d() ; /* slalom 斜め走行の dataを 造る */
short_d2() ; /* ｽﾗﾛｰﾑ 斜め走行の dataを 造る No_2 */
/* SLA7033 PWM基準電圧設定 */
current= TANSA_CURRENT ; /* motor の電流値(A/D.Cに書きこむ値) */
current_flows= current ; /* motor の電流値(A/D.Cに書きこむ値)を記憶 */
/* slalom旋回 高速走行 */
slalom() ; /* slalom旋回 高速走行 */
/* ﾏｳｽを goalから start地点まで戻す */
/* mouse の走行速度を設定する */
/* speed_select= speed_1 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* 走行モードの初期化 start --> goal --> start */
/* running_mode= ATTACK_SLA1 ; /* time attack slalom 低速走行 */
/* init_3() ; /* ﾏｳｽは goalから start地点まで戻る */
/* slalom旋回 高速走行 */
/* slalomLow() ; /* slalom旋回 高速走行 */
/* ＩＴＵの再設定 ２分周したクロックを初期値に戻す */
itu_init() ; /* ＩＴＵ 初期化 */
break ;
/* slalom 斜め走行 高速走行試験 */
case 14 :
/* 横壁データの中央値を reset ＆＆ start位置を合わせる */
reset_side_wall_data() ; /* start時に、横壁データの中央値を resetする */
start_position_set() ; /* start位置を迷路の中央に合わせる */
/* mouse の走行速度を設定する */
speed_select= speed_1 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* speed_select= speed_2 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* speed_select= speed_3 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* speed_select= speed_4 ; /* speed_select is 加速テーブルadressの offset(初速) */
adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* 走行モードの初期化 start --> goal --> start */
running_mode= ATTACK_ASL1 ; /* time attack slalom 定常走行 */
/* init_2() ; */ /* ﾏｳｽは start --> goal を走行する */
/* 連続ｽﾗﾛｰﾑ試験用 最短ｺｰｽ data */
route_map[0x00]= 0x84, route_map[0x01]= 0x21 ; /* 4 R */
route_map[0x02]= 0x80, route_map[0x03]= 0x21 ; /* 0 R */
route_map[0x04]= 0x80, route_map[0x05]= 0x31 ; /* 0 L */
route_map[0x06]= 0x80, route_map[0x07]= 0x21 ; /* 0 R */
route_map[0x08]= 0x81, route_map[0x09]= 0x31 ; /* 1 L 5 */
route_map[0x0a]= 0x80, route_map[0x0b]= 0x21 ; /* 0 R */
route_map[0x0c]= 0x81, route_map[0x0d]= 0x31 ; /* 1 L */
route_map[0x0e]= 0x80, route_map[0x0f]= 0x31 ; /* 0 L */
route_map[0x10]= 0x80, route_map[0x11]= 0x21 ; /* 0 R */
route_map[0x12]= 0x80, route_map[0x13]= 0x31 ; /* 0 L 10 */
route_map[0x14]= 0x80, route_map[0x15]= 0x31 ; /* 0 L */
route_map[0x16]= 0x80, route_map[0x17]= 0x21 ; /* 0 R */
route_map[0x18]= 0x81, route_map[0x19]= 0x21 ; /* 1 R */
route_map[0x1a]= 0x80, route_map[0x1b]= 0x31 ; /* 0 L */
route_map[0x1c]= 0x80, route_map[0x1d]= 0x21 ; /* 0 R 15 */
route_map[0x1e]= 0x80, route_map[0x1f]= 0x31 ; /* 0 L */
route_map[0x20]= 0x80, route_map[0x21]= 0xff ; /* 0 ｺﾞｰﾙ */
short_d() ; /* slalom 斜め走行の dataを 造る */
short_d2() ; /* ｽﾗﾛｰﾑ 斜め走行の dataを 造る No_2 */
/* SLA7033 PWM基準電圧設定 */
current= TANSA_CURRENT ; /* motor の電流値(A/D.Cに書きこむ値) */
current_flows= current ; /* motor の電流値(A/D.Cに書きこむ値)を記憶 */
/* slalom旋回 高速走行 */
slalom() ; /* slalom旋回 高速走行 */
/* ﾏｳｽを goalから start地点まで戻す */
/* mouse の走行速度を設定する */
/* speed_select= speed_1 ; /* speed_select is 加速テーブルadressの offset(初速) */
/* adress_set() ; /* 仮想車輪、駆動輪の速度テーブルのadress(初期値+ offset) ＆ 分周比を設定 */
/* 走行モードの初期化 start --> goal --> start */
/* running_mode= ATTACK_SLA1 ; /* time attack slalom 低速走行 */
/* init_3() ; /* ﾏｳｽは goalから start地点まで戻る */
/* slalom旋回 高速走行 */
/* slalomLow() ; /* slalom旋回 高速走行 */
break ;
/* 設定された、横壁data ＆＆ 前壁dataを 表示する */
case 15 :
/* 横壁dataを 表示する */
side_walldata_indicate() ; /* 横壁data 表示 */
/* 前壁dataを 表示する */
front_walldata_indicate() ; /* 前壁data 表示 */
break ;
default :
break ;
}
}
/* 走行状態flag set */
/* running_condition= CONTINUE ; */ /* if running_condition == CONTINUE then mouseは定常走行をしていた */
/* running_condition= SLALOM ; */ /* if running_condition == SLALOM then mouseは slalom をした */
/* running_condition= ASLANT ; */ /* if running_condition == ASLANT then mouseは斜め走行をしていた */
running_condition= HALT ; /* if running_condition == HALT then mouseは停止していた */
}
