■課題12■

■応用課題■

左センサ値をフィードバック制御し，壁から5cm〜10cmの距離を維持するように制御してみよ．基準値よりも壁に少し近づいたようならば左モータの速度を少し下げて，微妙に左へカーブをかけ，逆に壁から離れたようなら左モータの速度を微妙に上げて右へカーブをかけるようにする.そのような関数followLeft()を作成せよ．

int followLeft(int msec10, int sensor_thold); //引数は走行時間の指定(10ミリ秒単位)と，センサの基準値

ただし，forward()と同様に，もしも指定時間が0ならば無限に動作しつづけるものとする．

followLeft()の内部では，壁からの距離を調整するのにsencor.cで用意されているreadSensor()が利用できる．


      前センサ
         ● ●範囲内であれば点灯
左センサ● ●右センサ

また、センサ値の反応が基準値の±10程度の場合に、ロボットに搭載したLED4個のうち，緑のLEDが点灯させるようにしておくとデバッグしやすい。また、followLeftの内部で、readSensor()だけでなく、checkSensor()を呼び出せば、それ以外の3個のLEDも点灯させられる。

次のプログラム例を参考にすること。


extern unsigned int optledbuf;    // sensor.cの中で定義されているLED点灯用の変数
int followLeft(int msec, int sensor_thold)
{
   int sensor, left, diff; 
   int target_R, target_L;

   forward(1); //まず全速前進
   for(;;)
   {
	   wait()と同様の枠組みをここに書く。
	   指定時間が経過した→return 0;
	
	   sensor = checkSensor(sensor_thold-10);        //壁からの距離を示す基準値
       if(前方センサに反応があった)
          return -1;

	   left = readSensor(2);　//左センサ
	   
	   diff = left - sensor_thold;   // 基準値からの差
	   操作量 = diff や diffの累積値や前回のdiff値を使用してPIDの式
	   target_R = dutydata_R + 操作量; 
	   target_L = dutydata_L - 操作量;
	　　　　//dutydata_Rとdutydata_Lは、driveMotorで設定された現在のデューティを格納してある大域変数(課題7参照)
	　　　　//右モータと左モータの片方を操作量だけ早くし、もう片方を操作量だけ遅くする。→ロボットを操作量の分だけカーブさせる
	　　　　// 操作量はマイナスの値になる場合もある。その時は逆方向にカーブする。
	　driveMotor(target_R, target_L, sensor_thold-10);
	  if( -10 ≤ diff && diff ≤ 10 ) //もしもセンサ値の差が-10〜10の範囲内ならLEDを点灯
	  {
		optledbuf = 0x00; //点灯
	  }
	  else
	  {
		optledbuf = 0x08; //消灯
	  }
	}
]

比例定数Kpは、適当に設定して調整せよ。最初は、次を参考にKpの桁を合わせること。

Kpの値を桁数を変化させて、大きすぎ→小さすぎ(または小さすぎ→大きすぎ)になった間のKpが目的の定数付近を示す。そうなったら後はKpの値を微調整せよ。

ヒント

緊急停止は，次の関数を用意して呼び出せばよい．

int emergencyStop()
{
  p1=ブレーキ
　p3=ブレーキ
　ta2 = モータ速度をいきなり0に
　ta3 = モータ速度をいきなり0に
  SetCpuBoardLED( ON );// CPUボードLED点灯
　wait()でしばらく待機(※これがないと，LEDがすぐ消灯して気づかない)
  SetCpuBoardLED( OFF );// CPUボードLEDオフ
}

SetCpuBoardLED()は，sensor.cの中に最初から用意されていた関数で，CPUボード上のLEDを制御する機能を提供している．

■締め切り■