おびやまロボット研究所　～マイコン電子工作と自作ドローン～

ソフトウェア

ロボトレースの製作その９，ソフト作り始めました．

手始めに速度一定でセンターから外れると修正をかけるだけでゆっくり走らせてみた．
その動画．
でも，ちょっと速度を上げたら振動を始めてすぐに暴走した．
単純なON-OFF的な制御だとかなり限界が低い．

暴走の理由はブレーキがかかっていなかったせい．
モータの速度をモニタして回りすぎたら逆転ブレーキをかけるようにしたら，動きが固いけどラインから外れにくくなった．
ただし，速度（パルスカウント）やPWMの制御タイミングの最適化はこれから．
現時点での固い走りの動画．

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
ロボトレースの製作その１０，ソフト難しい・・・

作ってみると難しさが分かってきました．
モータの制御がまだまだ安定しません．

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
ロボトレースの製作その１１，位置をアナログ値で読み取り

ライン上の位置をディジタル値で読み取ってモータ制御を作っていたのですが，センサの間隔が広すぎて走行中に振動を起こす原因がモータ制御が悪いのか，それとも位置検出の幅が広いためなのか判断できません．なので最初の計画通り，ライン上の位置をアナログ値で読み取ってより細かい位置分解能を得ることにしました．
プログラムを組んだらどうやらうまく出来たようです．
センターを0として-70から70の値をLEDに出力する動画

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
ロボトレースの製作その１２，地味にプログラム中


雑用に追われ，最近更新してませんでしたが，地味にプログラム作成中．

マーカーの先読みをやってないので，ターン速度上限が直線スピードの上限．
そこそこ速度を上げたいと思ってパラメータをいじるがふらつき納まらず．
ログを取って考えるが，どうやら最初に考えたモータ制御と走行制御の区分けと機体の
構造に原因があるようで，いちからやり直す気力と時間がないので今回はそのままで行く．
マーカー検出は結構面倒．ラインがクロスするところで右マーカー検出が反応するので，
直線連続区間である条件かつ時系列に左右のマーカーを読み取って孤立的に右マーカーのみが反応したときをスタート，ゴールと認識するようにした．
同時に，直線距離と，ターンの距離と左右タイヤ回転差から半径を求めて記録していくプログラムを作り始めたところ．

ロボトレース，マーカー検出できた動画


－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
ロボトレースの製作その１３，コースログ


コースログ取る機能を付けました．
直線とコーナーに分けて，それぞれ距離と左右回転の差を記録しました．
ターン中に曲がりすぎから補正をかける部分で直線と間違って記録されますが，
コースの追従性をあげていけばより正しくログが取れるようになると思います．
R10のコーナーで最少速度，Rが大きいコーナーでは速度を上げて，
直線で加速・減速を組み入れてライントレースさせれば，それなりに速くなりそうです．
あとは，コースの追従性をどこまで上げられるか，
その後，センサ幅を有効につかってライン取りを可変してショートカット，
ができないかな．



－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
ロボトレースの製作その１４，２回目で加減速

１回目でコースログを取り，２回目でログをもとに直線で加減速する走行ができました．
ターンと直線の２段階の速度でライントレースはP制御のみでそれなりに走りだしました．
IとD制御を入れれば更に高速化できそうですが，高速化より
次はセンサ全域を使ってラインを滑らかにトレースする構造を作ってみます．
ターンをアウトインアウトで走るのが次の目標．
ロボトレーサー，２回目走行で直線を加速する動画

-追記-
もうちょっと早くなったので動画入替え．

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
ロボトレースの製作その１５，センサにオフセットを加える

センサは端から端までを使ってラインの位置を-35から35までの数値で返すように作っています．なのでセンサ値にオフセットを加えると，その分ラインからずれた位置をトレースして走るようになります．
0.5秒毎に-10と10のオフセットを与えて走らせてみましたー＞　その動画
１回目の走行履歴をもとに適当なオフセットを滑らかに与えてやれば，いいライン取りで走らせることができるんじゃないかと考えていますが，オフセットをどのように算出するかが問題ですね．

-追記-
アウト・イン・アウトは歩数がきっちり取れるか，先読みができないと無理っぽいので，イン・アウトでのコーナリングを考えてみます．

-追記2-
センサにオフセットを加えてコーナリングするプログラムを作ってみましたが，１回目走行の歩数をもとにオフセットをなだらかに変化させようとしても，実際のラインとの整合性（ライン取りが違うと当然歩数も変わる）と同期が悪く（ターンの切り返しとか），スムーズな走行ができずにふらふらしてしまいます．
やっぱり先読みセンサがないと無理かなぁ．．．

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
ロボトレースの製作その１６，終了！

センサにオフセットを加えてショートカットをトライしました．
コーナーでイン・アウトの走行をさせたり，連続ターンでイン・インの切り返しを
させてみたりしましたが，先読みができないため，オフセットを変化させるタイミングが
うまく取れず，かなり不安定な走行になってまったく高速化が見込めませんでした．
これ以上は労力の割りに得るものが少なそうなので一旦終了します．

何の情報もないのもなんなので，ソースの一部を公開．
１回目のコースを走行と２－３回目の高速走行．これらは各１００行程度．
全部で1000行くらいの一部です．
たいして難しいことはしてませんがご参考になれば．


//--------------------------------------------------------------------------
// 固定速度トレース,アナログセンサによる走行
// vC,vS別個に指定できる。指定すると直線とターンでなだらかに加減速。同じでもいい。
//--------------------------------------------------------------------------
void TraceFV(int vC, int vS, float vP, float vI, float vD){
// vC:コーナスピード
// vS:直線スピード
// vP:比例係数
// vI:積分係数
// vD:微分係数

int i; // 汎用
int vF=0; // 基準速度
int vMAX=vC; // 速度の最大値：vSまたはvCを選択代入，初期はコーナ速度
int dV=0; // 左右速度の変化分
long iV=0; // 中心に合わせ込む偏差積算成分
int timV=0; // 速度積算の頻度
int accV=3; // 基準速度の加速度（の逆数）
int cntGoal=0; // ゴール後少し走らせるためのカウント

posA=0; // センサ位置を初期化
stepR=stepL=0; // 歩数をリセット
stepRB=stepLB=0; // 歩数の履歴をリセット
Encoder_reset(); // ｖを操作する前は必ずエンコーダリセット
mode=0;
vR=vL=0;
frun=0; // スタート・ゴールマーカフラグの初期化
fmarker=0; // コーナーマーカーの初期化

cpt=0; for(i=0;i < CMAX;i++){ cDis[i]=cRad[i]=0; } // コースログの初期化


while(SW_START==1){

// センサはずれたときは例外的にモータオフ
if ((sensA & 0x3e)==0x00) {vR=vL= 0; beep(3); break; }

// 基準速度を加減速処理
// accV（加速度の逆数）でタイミング調整
if (timV > 0) timV--; // 加減速処理のタイミングカウンタを回す
if (timV==0){
timV=accV; // 加減速処理の頻度（加速度の逆数）を与える
if (vF < vMAX) vF++;
else if (vF > vMAX) vF--;
}

if (abs(sensAN) > 24){ vF--; LED=0xff; }// 大きく外れた時の例外処理

// ラインに対するモータのPID制御
iV+=sensAN; // I制御量計算
if ((iV > 0 && sensAN < -5) || (iV < 0 && sensAN > 5)) iV=0; // 偏差が逆転したらすぐに積算値をリセット

dV=(float)sensAN*vP+(float)(sensAN-sensANB)*vD; // PD制御量計算
sensANB=sensAN;

vR=((vF-dV-iV*vI) >= 0)? (vF-dV-iV*vI): 0; // 左右の加減速，ただし逆回転はさせない
vL=((vF+dV+iV*vI)>=0)? (vF+dV+iV*vI): 0; // 左右の加減速，ただし逆回転はさせない

deltaR=stepR-stepRB; // 右モータの距離を検出
deltaL=stepL-stepLB; // 左モータの距離を検出

// 直線で加速（直線速度に切替）
if ((float)abs(deltaR-deltaL)/(float)(deltaR+deltaL) < 0.1) vMAX=vS; else vMAX=vC;

// コーナー・マーカーのチェック
if (markerX==1) fmarker=0; // クロスラインの時は除外
if (markerX==0 && fmarker==0 && markerC > 5) fmarker=1; // コーナーマーカー検出
if (markerX==0 && fmarker==1 && markerC==0){ // コーナーマーカー確定
fmarker=0;
clog(); // ログを記録
beep(0);
}

// スタート・ゴールマーカーのチェック
if (frun==0 && markerSG>10) frun=1; // スタートマーカー検出
if (frun==1 && markerSG==0){ // スタートマーカー確定
frun=2;
clog(); // ログを記録
beep(1);
}
if (frun==2 && markerSG>10) frun=3; // ゴールマーカー検出
if (frun==3 && markerX==1) frun=2; // ラインクロスの場合を除外
if (frun==3 && markerSG==0){ // ゴールマーカー確定
frun=4;
clog(); // ログを記録
cntGoal=stepR;
beep(1);
}
if (frun==4 && stepR > (cntGoal+5000)) break; // ゴール後1回転，約10cm走ったら終了．

pause(2); // ループの速度制限、1mS 待つ
}

clog(); // ログを記録（有効ログ＋１まで参照して加減速するので必要）

// ゴール後の停止処理
vR=vL=0;
pause(ONESEC); // 1秒停止

}



//---------曲率に合わせた速度計算関数--------------------------------------------
int calvMax(float vs, float crad, float cdis, float vc)
{
if (cdis<2000) return(vc); // 短い距離で曲率が変化してると誤差が大きいので強制的に減速
return( vs*(1-sqrt(crad/cdis)*(1-vc/vs)*1.24) ); // 曲率に合わせてなめらかに速度変化するようにした関数
}

//--------------------------------------------------------------------------
// コース履歴による高速トレース （センサオフセットは使わない）
//--------------------------------------------------------------------------
void TraceCR(int vC, int vS, float vP, float vI, float vD){
// vC:コーナスピード
// vS:直線スピード
// vP:比例係数
// vI:積分係数
// vD:微分係数

int i; // 汎用
int vF=0; // 基準速度
int vMAX=vC; // 速度の最大値：vSまたはvCを選択代入，初期はコーナ速度
int vMAX2=vC; // 次の区画の速度の最大値：現在の区画の最大値と比較して減速をかけるのに使う
int dV=0; // 左右速度の変化分
long iV=0; // 中心に合わせ込む偏差積算成分
int timV=0; // 速度積算の頻度
int accV=2; // 基準速度の加速度（の逆数）
int cntGoal=0; // ゴール後少し走らせるためのカウント

posA=0; // センサ位置を初期化
stepR=stepL=0; // 歩数をリセット
stepRB=stepLB=0; // 歩数の履歴をリセット
Encoder_reset(); // ｖを操作する前は必ずエンコーダリセット
mode=0;
vR=vL=0;
frun=0; // スタート・ゴールマーカフラグの初期化
fmarker=0; // コーナーマーカーの初期化
i=0; // コースログのインデックスとして使う

while(SW_START==1){

// センサはずれたときは例外的にモータオフ
if ((sensA & 0x3e)==0x00) {vR=vL= 0; beep(3); break; } // 段差で跳ねたら止まった．余裕を持たせるように改良必要

// 基準速度を加減速処理
// accV（加速度の逆数）でタイミング調整
if (timV > 0) timV--; // 加減速処理のタイミングカウンタを回す
if (timV==0){
timV=accV; // 加減速処理の頻度（加速度の逆数）を与える
if (vF < vMAX) vF++;
else if (vF > vMAX) vF--;
}

if (abs(sensAN) > 28){ vF--; LED=0xff; }// 大きく外れた時の例外処理

// ラインに対するモータのPID制御
iV+=sensAN; // I制御量計算
if ((iV > 0 && sensAN < -2) || (iV<0 && sensAN > 2)) iV=0; // 偏差が逆転したらすぐに積算値をリセット

dV=(float)sensAN*vP+(float)(sensAN-sensANB)*vD; // PD制御量計算
sensANB=sensAN;

vR=((vF-dV-iV*vI)>=0)? (vF-dV-iV*vI): 0; // 左右の加減速，ただし逆回転はさせない
vL=((vF+dV+iV*vI)>=0)? (vF+dV+iV*vI): 0; // 左右の加減速，ただし逆回転はさせない

deltaR=stepR-stepRB; // 右モータの距離を検出
deltaL=stepL-stepLB; // 左モータの距離を検出

// コーナー・マーカーのチェック
if (markerX==1) fmarker=0; // クロスラインの時は除外
if (markerX==0 && fmarker==0 && markerC>3) fmarker=1; // コーナーマーカー検出
if (markerX==0 && fmarker==1 && markerC==0){ // コーナーマーカー確定
fmarker=0;

i++; LED=i;
vMAX =calvMax(vS, cRad[i], cDis[i], vC); // その区間の最高速度計算
vMAX2=calvMax(vS, cRad[i+1], cDis[i+1], vC); // 次の区間の最高速度計算
stepRB=stepR; // 一つ前のカウント値を保管
stepLB=stepL; // 　　　　同上
beep(0);
}

// スタート・ゴールマーカーのチェック
if (frun==0 && markerSG>10) frun=1; // スタートマーカー検出
if (frun==1 && markerSG==0){ // スタートマーカー確定
frun=2;

i++; LED=i;
vMAX =calvMax(vS, cRad[i], cDis[i], vC);
vMAX2=calvMax(vS, cRad[i+1], cDis[i+1], vC);
stepRB=stepR; // 一つ前のカウント値を保管
stepLB=stepL; // 　　　　同上
beep(1);
}
if (frun==2 && markerSG>10 && i>=(cpt-3)) frun=3; // ゴールマーカー検出、最後から２番めのマーカー以上から検出
if (frun==3 && markerX==1) frun=2; // ラインクロスの場合を除外
if (frun==3 && markerSG==0){ // ゴールマーカー確定
frun=4;

i++; LED=i;
stepRB=stepR; // 一つ前のカウント値を保管
stepLB=stepL; // 　　　　同上
cntGoal=stepR;
vMAX=vMAX2=20;
beep(1);
}
if (frun==4 && stepR > (cntGoal+5000)) break; // ゴール後1回転，約10cm走ったら終了．

// 減速をかける
if (vMAX>vMAX2){ // 次の区画が現在の区画より低速で走るとき
if ((cDis[i]-(deltaR+deltaL)/2) < (vF-vMAX2)*accV*120){ // 減速に必要な距離を現在速度と次区画の速度より計算
vMAX=vMAX2;
LED=0x0f; // 減速中を表示
}
}

pause(2); // ループの速度制限、1mS 待つ
}

// ゴール後の停止処理
vR=vL=0;
pause(ONESEC); // 1秒停止
}