ソフト改良あれこれ
昨日のプログラムは，進行方向のずれを検知して比例したステアリング操作，
ターン時間は約0.5秒固定でした．
0.5秒ターン，0.5秒直進で２段階の走行だったのも工夫の余地があります．
そこで今日は，ターン時間を可変にしてみました．
例えば，進行方向が目的地と正反対の場合は180度ターンするわけですが，昨日の
プログラムだと0.5秒ターンして戻すのを数回繰り返してターンをすることになります．
ターン時間を可変にすると，大きな角度ターンする場合はステアリングをいっぱいに
切って回りきるまでそれを持続させることができるようになります．
問題なのは，ターン時間が長くなると，ターン中に取ったデータでGPSが方位を出力
してしまい，正確な方位との乖離が大きくなってしまう可能性があることで，これを
避けるために，ターン終了後1秒以上直線区間を保って方位検出することにしました．
GPSデータ更新のフラグｆGが0か１だったのを，0,1,2の値を取るようにして，
ターン中はｆG=0,ターンが終わった後ｆG=2のときにステアリング処理するように
しました．
おかげで走行中のループがすっきりしました．
パラメータ調整終わったら全体のリストをアップします．
あとは，ジャイロによる回転角度の制御と，障害物回避の改良です．
while (SW_START==1){ // スタートスイッチが押されたときはループから抜け出す
// 高速で速度制御
if (spR>SPDH) pos2=POSH; else pos2=POSL;
// 直線で方位計測できた場合（fG=2）のみステアリング処理
if (fG==2) {
fG=0;
gps_val();
gps_cal(); // 現在地とウェイポイントとの関係を計算
disp_dwp(); // ウエイポイントとの差を表示
// 角度差に応じて方向に舵を切る．速度により値を可変？
k=POSC-dd*p1;
if (k< STRMAX) pos1=STRMAX; // 右最大切れ角
else if (k>STLMAX) pos1=STLMAX; // 左最大切れ角
else pos1=k;
pos3=pos1; // ステアリングと超音波センサを同期
timP=abs(dd)*p2; // ステアを切っている時間指定
}
if (timP>0) fG=0; // ターン途中でGPS受信しても数値を更新させない
else pos1=POSC; // タイマAでカウントダウン，時間がたったらステアリング戻す
if (disU< URFMIN) kaihi(); // 障害物を検出したときは回避行動
// ウェイポイントとの距離を求め，ポイント更新または走行終了判断
if (dr < wr){
pos1=pos2=POSC; pause(30000); // 一定時間走行停止
np++; if (np>wp) break; // ウェイポイント更新，走行終了判断
slow_start(); // 低速走行で一定時間前進して初期の方向を得る
}
}
pos1=pos2=POSC; // ループを抜けるときはサーボを初期位置へ
}
※gps_cal()は現在地とウェイポイントとの関係計算を関数化．
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
続いて，ターンの角度をステアリングの切り角とターン時間で制御するのではなく，
ジャイロを使って，方位の偏差が一致するまで回転させるように書き換えてみました．
（ただし，動作チェックはまだ）
こっちのほうが，走行速度に依存せずにターンができるので良いはず．
ターンの角度が小さいときはGPSから得られる方位の誤差が大きくなく，
無駄に直進させてしまうことを避けるため，その時のフラグリセットを除外するように
してみました．
while (SW_START==1){ // スタートスイッチが押されたときはループから抜け出す
// 高速で速度制御
if (spR>SPDH) pos2=POSH; else pos2=POSL;
// 直線で方位計測できた場合（fG=2）のみステアリング処理
if (fG==2) {
fG=0;
gps_val();
gps_cal(); // 現在地とウェイポイントとの関係を計算
disp_dwp(); // ウエイポイントとの差を表示
// 角度差に応じて方向に舵を切る．速度により値を可変？
k=POSC-dd*p1;
if (k< STRMAX) pos1=STRMAX; // 右最大切れ角
else if (k >STLMAX) pos1=STLMAX; // 左最大切れ角
else pos1=k;
pos3=pos1; // ステアリングと超音波センサを同期
gyItg=0;
}
if (abs(gyItg>>12)>abs(dd*p2)) pos1=POSC; // ジャイロで角度モニタ，指定角度でステアリング戻す
if (pos1!=0 && abs(dd)>45) fG=0; // ターン途中かつターン角度が大きい時，GPS受信しても数値を更新させない
if (disU< URFMIN) kaihi(); // 障害物を検出したときは回避行動
// ウェイポイントとの距離を求め，ポイント更新または走行終了判断
if (dr < wr){
pos1=pos2=POSC; pause(30000); // 一定時間走行停止
np++; if (np >wp) break; // ウェイポイント更新，走行終了判断
slow_start(); // 低速走行で一定時間前進して初期の方向を得る
}
}
pos1=pos2=POSC; // ループを抜けるときはサーボを初期位置へ
}
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
障害物回避の方法
動作確認はまだですが，障害物回避の方法を考えました．
超音波センサをステアリングを基準に左右に振って障害物検知，障害物の無い方向にステアリングにオフセットを
かけて障害物回避することにします．
まず，超音波センサを振りながらセンサの値をモニタしてみました．
超音波センサを左右に振った動画
ＬＣＤ左から，超音波センサの測定値，左に振 ったときの値，右に振ったときの値です．値が小さいほど近くに
障害物があることになります．
あとはステアリングに障害物との距離に比例したオフセットを加えます．
// 角度差に応じて方向に舵を切る．
k=POSC-dd*p1;
if (disR<0x74) k+=(0x74-disR)/10; // 右障害物に対する回避
if (disL<0x74) k-=(0x74-disL)/10; // 左障害物に対する回避
if (k< STRMAX) pos1=STRMAX; // 右最大切れ角
else if (k>STLMAX) pos1=STLMAX; // 左最大切れ角
else pos1=k;
超音波センサを振りながら距離をもにたするのは，タイマーＡの中に次のような
記述を入れました．
// 障害物の左右差検出のためのサーボ操作および計測
cntW++; if (cntW>4000) cntW=0;
if (cntW<2000) pos3=pos1-3; else pos3=pos1+3; // ステアリングを基準に左右に振る
if (cntW>1900 && cntW<2000) disR=disU;
if (cntW>3900 && cntW<4000) disL=disU;
改良したGPSロボットカーのソース（H8-3664）
GPSロボットカーテスト走行の動画（ジャイロを使った旋回と超音波センサによる障害物回避）