BasicStamp用　お掃除ロボットプログラム

' {$STAMP BS2}
' {$PBASIC 2.5}
' {$PORT COM3}

'お掃除ロボット制御プログラム   Ver2.1
'
'四角い部屋の縁に沿って動く。はじめは右側の縁に沿って動き，縁に一定回数センサーがはみ出すと
'逆側の縁に沿って動くようになる。正面のマイクロスイッチが押されると180度回転して止まる。
'止まった状態から動作を開始させるためには，大きな音で柏手を打つとスタートする。
'

 '初期設定
  Front con  %1010  '前進
  Back  con  %0101  '後退
  T_r   con  %0001  '右に旋回
  T_l   con  %0100  '左に旋回
  R_r   con  %1001  '右回りに回転
  R_l   con  %0110  '左回りに回転
  Sto   con  %0000  'ストップ
  B_r   con  %1111  'ブレーキ

   '　inb　=　%7654
   '　inb　=　%1000    正面衝突        8
   ' 　  　=　%0100    音センサー動作  4
   '  　 　=　%0010    右脱輪注意      2
   '  　 　=　%0001    左脱輪注意      1
   '  　 　=　%0011    脱輪注意        3

  dirc  =  %1111    '	bit8,9,10,11     output ,m_otor   outc
  dird  =  %1111    '	bit12,12,14,15   output ,LED_    outd
  dirb  =  %0000    '	bit4,5,6,7       input   センサー

  c  var  inl.bit7  '1 で正面衝突
  b  var  inl.bit6  'マイク出力
  r  var  inl.bit5  '1 で右脱輪注意
  l  var  inl.bit4  '1 で左脱輪注意

  i   var word      'センサー動作回数カウンター
  t   var nib       '現在のセンサー値を保存する
  tb  var nib       '前回のセンサーの値を保存
  tg  var nib       '回転方向フラグ　1：左回転（反時計）　0：右回り（時計回り）



  tg=0

 re: '+++++++++++++++++++++　再スタート　+++++++++++++++++++++++++++++++++++++++++++++++

	if tg=1 then tg=0 else tg=1
  	outc=Sto
                                 'プログラム例　　読み込みピン変数を参照するだけで良い
  	do while(b<>1)                 'c<>1   音センサーの値が変化　1　に鳴るまで待つ
    	 outd=inb
  	loop
  	outc=front
      	i=0
      	tb=inb
      	t=inb


st:'+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    '*******************      センサー読み込み      変化するまでまつ
	do while (t=tb)
          t=inb
          outd=t
      	loop
      	tb=t
      	i=i+1

    '******************       Ａ3面積を走行するのに10秒    iの値で60ぐらい
    	if i=100 then
       	  if tg=1 then tg=0 else tg=1
       	  i=0
    	endif

    'debug bin4 t,cr

    	select t
           case %1000
                       outc=Sto
                       gosub jyuuden
                       outc = Sto
                       goto re
           case %0000
                       outc = front
           case %0011
                       outc =Back
           case %0010
                       if tg=1 then  outc =T_l  else outc=T_r
           case %0001
                       if tg=1 then  outc =T_l  else outc=T_r
           case else
                       outc = Sto
    	endselect
goto st '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

jyuuden:      '************************  車庫に到達し，180度回転し　車庫入れ
    	if tg=0 then                   '時計回りで侵入
          outc=Back                                                             
          pause 1000                   '回転角度はモータの動作時間で決めている。       
          outc = R_l                   '電池の電圧で回転数は変化するので時間の調整必要 
          pause 2500
          outc=Sto
     	else                     　    '反時計回りで侵入
          outc=Back
          pause 1000
          outc=R_r
          pause 2500
          outc=Sto
    	endif

     	outc=Back
     	pause 1500
     	outc = Sto
return