DATE: --/--/--(--)   CATEGORY: スポンサー広告
スポンサーサイト
上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。
page top
DATE: 2018/09/19(水)   CATEGORY: Nucleoマウス
Nucleo-32boardを使ったクラシックマウスの開発その23 ~壁情報とFLASH~


今回は迷路データの保存についての話です。
前回で基本的な調整が終わり、
迷路を走ることができるようになりました。
また,任意座標で止まれるようになっているということは
マウスの位置を把握できているということですね?
つまり、その位置の壁情報を覚えることも可能になっているわけです。

ということで壁情報を保存して迷路を覚えさせましょう。
→マウスっぽくなってきた


・壁情報の形式

壁情報を保存するにあたりその保存形式を決めなければなりません。
以下に自分が知っている具体例を挙げていきます。
メリットとデメリットを考慮してどの形式で保存していくか
自分で選択するのがいいと思います。

→正直好みの問題

どの形式でも共通していることは
①壁の有無情報、②壁の探索情報
の2つを保存していくという点です。
→探索情報は最短走行の際に非常に重要となります


壁情報については多くの人が書いてくれているので
調べればたくさんでてきます。


① 1区画につき8 bit形式

上位4 bitを探索情報、下位4 bitを有無情報としてとる方法。
参考はこちら。図も書いてくれているのでわかりやすいです。

迷路情報の考え方~WMMC秘伝のタレ 前編~ -ぱわぷろ技術日誌

よく使われている手法です。
自分も昔はこれを使っていました。

16×16の迷路だと256 byte使います。
→32×32だと4倍の1024 byte

メリットは区画単位で考えられることでしょうか。
区画単位で考えられる分理解しやすいです。

デメリットは区画単位で壁情報を保持するため、
一つの壁に対して2 面分のデータを保存しないといけない点です。
壁情報が重複している分無駄にメモリを使います。
そのため使用メモリが他の方法に比べ多くなってしまいますが、
RAM領域的には十分確保できるため
使用メモリについてはあまり気にしなくてもいいかもしれません。

自分は壁の両面保存が好みでは無かったので止めました。
他は特に不満はありませんでした。


② 北壁及び東壁のみ形式

北壁と東壁のみを保存していく形式です。
ある区画を考えたときに、
その区画の南壁は一つ下の区画の北壁を参照すれば問題ありません。
その区画の西壁は一つ左の区画の東壁を参照すれば問題ありません。
この特徴に注目したのがこの形式です。

基本的には16 bit変数(32×32対応なら32 bit変数)を用いて
壁データを保存していきます。

定義を書いておくとこんな感じ。


typedef struct {
uint32_t wall;
uint32_t known;
} walldata;

walldata g_easternWallData[32], g_northernWallData[32];


16×16の迷路だと128 byte使います。
→32×32だと4倍の512 byte

基本的にビット番号がx座標を表し、配列の要素番号がy座標を意味します。
→逆でも可
例えば、(x,y)の東壁を入れるときには
東壁データ[y]|=(1<< x)
のようにすることが可能です。
壁を抜くときには(1<< x)の補数とAND演算すればよいですね。

メリットは①の方法に比べ、
使用するメモリがちょうど半分になっていることです。

デメリットは③に示す方法に比べて、
無駄に壁情報を持ってしっまていることでしょうか。
→無駄と言っても変数4つ分

個人的には考え方がわかりやすく、
壁の重複もないことからこの形式を採用しています。



③ 内壁のみ形式

マイクロマウスのルール上外壁の存在は保証されています。
なので、その内壁だけを保存して利用していく考え方です。
参考はこちら。

壁情報の記録 -hantas's blog

僕の迷路クラスの紹介 -KERI`s Lab

個人的なイメージとしては漢字の「井」のような部分だけ保存する感じですかね。

16×16の迷路だと120 byte使います。
→32×32だと496 byte

メリットは②の方法に比べ、東外壁と北外壁も保存しないので
使用メモリが最も少ないことです。
→(東外壁+北外壁=2列)×(有無情報+探索情報=2種類)で壁データの変数4つ分少なくできます

デメリットというものはあまり思いつきませんが、
強いて言うなら縦方向の壁と横方向の壁で向きが違うので
コードを書くときに間違えやすいことでしょうか。
→これも大したデメリットではない


個人的な感想を簡単にまとめますと、
① 1区画につき8 bit形式 :考え方が簡単。使用メモリ多。
② 北壁及び東壁のみ形式 :考え方が簡単。使用メモリ少。
③ 内壁のみ形式 :考え方がやや複雑。使用メモリ最少。


壁情報の保存形式が決まったら壁情報を変更できる関数を用意しましょう。
→自分は②の方法を採用したためこんな感じ

void setWallDataBit(uint32_t *wallY, uint8_t X) {
*wallY |= (1 << X);
}

void clearWallDataBit(uint32_t *wallY, uint8_t X) {
*wallY = (*wallY & (~(1 << X)));
}



・壁情報の出力関数を作る

方針が決まって、
さあマウスに組み込もうと思った人はちょっと待ってください。
まだ大切なものがありません。
それは保存した壁情報を確認する手段です。

そこで壁情報を視覚的に判断できるように
以下のようにTera termに出力できる関数を作りました。

迷路初期化

横方向の壁は- (半角マイナス)×5、縦方向の壁は| (縦線)、
柱は+(半角プラス)で表しています。

→後で区画中央に数字を入れたいので5個分スペースを作っておきます

色分けしたい場合についてはエスケープシーケンスについて調べてください。


迷路の初期情報として、
① 外壁情報
② (0,0)の区画情報(スタート区画は必ず袋小路)
③ ゴール区画の壁無し情報
→これはどっちでもいい
を与えています。

完成したら、手打ちでいいので
好きな位置に壁を入れて出力して動作確認してみましょう。

壁反映確認


・左手法に壁追加関数を組み込む

ここまでできたら、左手法をしながら壁情報を集めてみます。
移動方向を決定するタイミング=区画境界毎に壁データを追加していきます。
壁の判断はセンサー値を使って判断すればよいでしょう。
→壁の有無さえ判断できればいいので閾値は雑に決めてOK

ゴールをしたら壁情報を先ほどの関数を使って出力してみます。


なんと、、、
Tera termに出力しようとUSBケーブルを繋げたら
マイコンにリセットがかかってしまいました、、、

→Nucleo最大の罠

Nucleoボードは、外部電源で起動していても
USBケーブルを繋ぐと自動でリセットがかかってしまう仕様らしいです。


これではせっかくの壁情報の出力ができません
→どうにかしましょう


これを解決するために、
一旦FLASH領域に迷路データを保存し再起動後読み出すという処理をします。

・FLASH領域への保存

STM32F303K8マイコンではリファレンスマニュアルによると、
以下のような2 kBずつのPageが設けられているようです。

FLASHのアドレス

この領域に迷路データを保存しておくと、
マイコンがリセットされてもこの領域の値は保持されます。


ということでこの領域に迷路データを保存していきます。

保存の流れとしては、
① FLASHのLockを解除
② 消去するPageを選択
③ データを書き込み
④ FLASHを再びLock
となっております。

詳しくはHALのマニュアルを読んでください。
簡単に保存関数を作ってみました。
→雑ですがとりあえず書き込めればいいですね


void saveMazeMapFlash(uint32_t addr) {

FLASH_EraseInitTypeDef EraseInitStruct;
uint32_t PageError = 0;

HAL_StatusTypeDef r;

int i;

//FLASH Unlock
HAL_FLASH_Unlock();

//Page Erase
EraseInitStruct.TypeErase = FLASH_TYPEERASE_PAGES;
EraseInitStruct.PageAddress = addr;
EraseInitStruct.NbPages = 1;

r = HAL_FLASHEx_Erase(&EraseInitStruct, &PageError);
if (r == HAL_OK) {
printf("Page Erase OK\n");
} else {
HAL_FLASH_Lock();
return;
}

printf("Write Eastern Wall\n");
for (i = 0; i < MAZESIZEMAX; i++) {
HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
(uint32_t) (addr + sizeof(uint32_t) * i),
(uint32_t) g_easternWallData[(i % MAZESIZEMAX)].wall);
}

printf("Write Eastern Known\n");
for (i = MAZESIZEMAX; i < 2 * MAZESIZEMAX; i++) {
HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
(uint32_t) (addr + sizeof(uint32_t) * i),
(uint32_t) g_easternWallData[(i % MAZESIZEMAX)].known);
}

printf("Write Northern Wall\n");
for (i = 2 * MAZESIZEMAX; i < 3 * MAZESIZEMAX; i++) {
HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
(uint32_t) (addr + sizeof(uint32_t) * i),
(uint32_t) g_northernWallData[(i % MAZESIZEMAX)].wall);
}

printf("Write Northen Known\n");
for (i = 3 * MAZESIZEMAX; i < 4 * MAZESIZEMAX; i++) {
HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
(uint32_t) (addr + sizeof(uint32_t) * i),
(uint32_t) g_northernWallData[(i % MAZESIZEMAX)].known);

}

//FLASH Lock
HAL_FLASH_Lock();

}


FLASH領域への保存のときに、
自分が嵌ったポイントを書いておきます。

① 適当なPageを消すとプログラムが止まる

FLASH領域に書き込めると言ってもどこにでも書き込んでいいわけではないみたいです。
FLASH領域は基本的にマイコンの動作を書き込む領域(?)と共用らしいです。
→RXでは分かれていた
なので、上の方のPageを消すとプログラムが止まります。

そのため、自分は一番下のPageを使用しています。
Pageの消去の仕様上Page31にはプログラムを書き込まないようにしないといけません。
→事実上FLASH量の制限が62 kBになる

うまくブロックをかけられればいいのですが、
やり方もわからないのでBuild AnalyzerでFLASH領域の使用量を監視しながらプログラムを書いています。

Build Analyzerで今のFLASH領域の使用率を確認しておきましょう。
Build Analyzerに表示されない場合は、
プロジェクトエクスプローラーでプロジェクトを選択すれば表示されます。
→TrueSTUDIOのバグですかね?


② F303K8の一番下のPageの先頭アドレスは0x0800 f800

F303K8のFLASH領域は64 kBです。
つまり一番下のPageはPage 31です。
上の図ではこのアドレスが省略されていて表記されてないんですよね。
自分で計算して求めないといけませんね。


・FLASH領域の読み出し

読み出しは非常に簡単です。
FLASHをLock解除したら、
読み出したいアドレスを指定して読み出すだけです。

書き込んだ位置を読み出すだけなので簡単です。
読み終わったら再びLockしておきます。


・そんなこんなで

無事に迷路データを保存できるようになったので、
壁情報のデバッグができるようになりました。

→走らせて壁情報が間違っていないか調べる
(Tweetは左手法ではありませんがやりたいことはこんなの)





ここまでできれば完走までもう一息です。


スポンサーサイト
DATE: 2018/09/17(月)   CATEGORY: Nucleoマウス
Nucleo-32boardを使ったクラシックマウスの開発その22 ~調整方法~


今回は調整方法についての話です。
前回までで迷路を走るための基本的な部分は
できたので実際に迷路を走っていこうと思います。

調整については自分ルールがあればいいと思います。
こちらが非常に参考になります。

マイクロマウス調整法(フレッシュマン向け) -徒然なるままに書きつくれば

それでは書いていきます。
基本的に調整の順番はこの記事に書いていく順番で行っています。


・壁制御の閾値

以下のようにマウスを中心に置き、
左右の壁を一定距離離した値を閾値としています。

自分は壁面が区画中心から30 mmとしています。

壁制御の閾値

これは壁制御が切れるタイミングを左右で合わせる
ことを目的としています。


・壁制御のリファレンスとゲイン

以下のように片壁だけの状態で走らせて
機体が中央に収束するように調整します。


1~1.5区画程度で区画中央に収束するといいです。
DゲインはPゲインの0.8倍程度入れています。



参考記事がわかりやすいので
そちらを見ていただければなと思います。


これは壁制御で区画中央に戻れることを確認する
ことを目的としています。


・壁制御を無効にする変化量

以下のように3番目以降の壁を取り除き調整します。
吸い込まれなくなるような変化量を設定できれば良いです。






これは壁制御による吸い込まれ現象の影響を確認する
ことを目的としています。


上の工程を左右それぞれで確認したら壁制御関連は終わりです。


次は走行に必要なパラメータを合わせていきます。
これらのパラメータは1つづつ決定していきます。


・タイヤ径調整

以下のように全区画に両壁を入れた状態で
壁制御を入れながら16区画直進を行います。

速度及び加速度は探索で用いるパラメータがいいと思います。
もしも距離が足りない場合にはタイヤ径が大きすぎるので
タイヤ径を小さくします。
距離が足りない場合はその逆です。



タイヤ径はマウスの走行の全てに影響を及ぼします。
しっかりと合わせます。

これは正しい距離を走るためのタイヤ径を決定する
ことを目的としています。


・トレッド幅調整

以下のように180°毎の超信地旋回を10回ほど繰り返します。
10回回転しても大きく角度がズレないように調整します。



トレッド幅はスラロームターンにも影響を及ぼします。
ここで合わせておきます。

これは正しいトレッド幅を決定する
ことを目的としています。


・スラロームの角度調整

スラロームの調整はステッパーの調整で最もしんどい部分ですね。
頑張りましょう。
→左右のターンを合わせるのがしんどいです

以下のように3×3の区画を3周させます。
このとき壁制御は最初のスラロームターンに入るまでの直進だけしか入れません。
他のタイミングで壁制御を入れてしまうと、
スラロームターンの角度のズレがわからなくなってしまいます。

3周しても角度がズレないように回転角度を調整します。
90.0°から始めて内に抉り過ぎていたら角度を減らし、
外に開き過ぎていたら角度を増やしましょう。



これはスラロームターンの旋回角度を決定する
ことを目的としています。


・スラロームのオフセット調整

以下のようにターン1回分だけの動きで合わせます。
安定して止まれるように、加減速させましょう。

オフセットを定規で計測し調整します。



これはスラロームターンのオフセットを決定する
ことを目的としています。


左右スラロームターンの調整が終われば、
基本的な調整は終了です。



・左手法で動作確認

調整が終わったら左手法で動作確認をしましょう。
綺麗な調整ができても、
迷路を走らせたら思わぬバグがあるかもしれません。

左手法のポイントとしては、
次の行き先を判断する部分は区画中央ではなく、
区画境界の位置で判断するといったことでしょうか。

→フレッシュマンは勘違いしやすい


うまく動くことが確認出来たら、
任意座標でゴール判定をできるようにしておきましょう。


DATE: 2018/09/11(火)   CATEGORY: Nucleoマウス
Nucleo-32boardを使ったクラシックマウスの開発その21 ~壁制御~


今回は壁制御のお話しです。
壁制御はマウスの安定感に直結します。
壁制御ができればある程度の迷路は走れます。

壁制御の概要はMice Wikiをご覧ください。

壁制御については初代ステッパマウスの「うむ夫。」
のときから苦しんできました。
初年度の記事はこちら。

うむ夫。探索編(3) ~壁制御編~

今見ると現在実装している壁制御とは異なる部分もありますが、
基本は上に書いてある通りです。

それでは今回は過去の自分と変わった点を中心に書いていきます。
探索で使っている壁制御についてのポイントです。
→最短走行時などの難しい話は知りません


・壁センサ値の取り方(差分取得)

壁センサの値は制御周期毎に取得します。

これはよく聞く方法でLEDを点灯させているときと
消灯しているときの値の差分を取ります。

こうすると外乱の影響を減らすことができます。

これについてはDMAの割り込み関数を利用します。
→DMAの扱い方については以前の記事を参照

Nucleo-32boardを使ったクラシックマウスの開発その16 ~ADC~


実装の流れとしては、

①回目の割り込み
→LED消灯時の値を取る
→左右方向のLEDを点灯
②回目の割り込み
→左右方向の値を取る
→左右方向のLEDを消灯
→前方向のLEDを点灯
③回目の割り込み
→前方向の値を取る
→前方向のLEDを消灯
→とった値でセンサ値を計算

です。
イメージはできたのでこんな感じで実装しました。


void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) {
getWallSensorValue(&g_ADCCount);
}

void getWallSensorValue(uint8_t *count) {
volatile uint16_t i;
switch (*count) {
case OFFVALUE:

HAL_ADC_Stop_DMA(&hadc1);
g_offWallSensorValue.frontLeft = g_ADCBuffer[FRONTLEFT];
g_offWallSensorValue.frontRight = g_ADCBuffer[FRONTRIGHT];
g_offWallSensorValue.left = g_ADCBuffer[LEFT];
g_offWallSensorValue.right = g_ADCBuffer[RIGHT];

*count = SIDEVALUE;
if (g_sensorLEDFlag == 1) {
sensorSideLED(HIGH);
for (i = 0; i < SENSORLEDWAITCOUNT; i++) {
}
}
HAL_ADC_Start_DMA(&hadc1, g_ADCBuffer, ADLENGTH);

break;

case SIDEVALUE:

sensorSideLED(LOW);
HAL_ADC_Stop_DMA(&hadc1);
g_onWallSensorValue.left = g_ADCBuffer[LEFT];
g_onWallSensorValue.right = g_ADCBuffer[RIGHT];

*count = FRONTVALUE;
if (g_sensorLEDFlag == 1) {
sensorFrontLED(HIGH);
for (i = 0; i < SENSORLEDWAITCOUNT; i++) {
}
}
HAL_ADC_Start_DMA(&hadc1, g_ADCBuffer, ADLENGTH);

break;

case FRONTVALUE:

sensorFrontLED(LOW);
HAL_ADC_Stop_DMA(&hadc1);
g_onWallSensorValue.frontLeft = g_ADCBuffer[FRONTLEFT];
g_onWallSensorValue.frontRight = g_ADCBuffer[FRONTRIGHT];

*count = END;

g_nowWallSensorValue.frontLeft = g_onWallSensorValue.frontLeft
- g_offWallSensorValue.frontLeft;
g_nowWallSensorValue.frontRight = g_onWallSensorValue.frontRight
- g_offWallSensorValue.frontRight;
g_nowWallSensorValue.left = g_onWallSensorValue.left
- g_offWallSensorValue.left;
g_nowWallSensorValue.right = g_onWallSensorValue.right
- g_offWallSensorValue.right;

break;

default:
break;
}
}



迷路を走らせて壁がなくなるときのセンサ値のログをとりました。
→ぼちぼちですかね?

壁生値


・壁センサ値の取り方(移動平均)

壁センサの値は回路によって、
ノイズが発生してしまうことがあります。
自分も過去に苦労しました。

その対策として壁センサの値を移動平均をとって使用しています。
移動平均をとることにより、細かな値の変動を抑えることができます。


移動平均を使う上で重要になってくるのはサンプル数です。
移動平均のサンプル数は機体毎に決めればいいと思います。

3 ms移動平均

しかし、サンプル数を多くとりすぎると
遅延時間の影響も大きくなってしまいます。
各センサ値のログを取ってみて決めましょう。

移動平均による遅延の影響

探索で用いる場合には、3~5個がいいと思います。
→多くても10。それでも改善しないなら回路を見直すべき


・変化量の取り方

変化量は吸い込まれ対策として重要です。
この変化量の取り方にも注意が必要です。

単純に思いつく変化量の取り方は、
今の値-過去の値
で求める方法です。
これを1 msでとった場合がこちらです。

変化量

ひどいです。
正直言って使い物になりません。
一番変化量を検知したい160~220 msの部分すら怪しいです。
これでは、ノイズの影響なのか壁が無くなっている影響なのか、
それとも機体が壁から離れようとしている影響なのか、
どういった原因で変化量が生じているのか判断が難しいです。
→吸い込まれ対策として壁制御を切る判断が難しい

きちんと吸い込まれ対策をするには変化量をうまく取り出さなければいけません。

この変化量をうまく取り出す方法の一つとして、
前に説明した移動平均が役に立ちます。

下に示すのは壁センサ値を移動平均を3 msで用いた場合の差分データです。

移動平均後の変化量

ノイズの影響が減っていることがわかります。
壁センサ値の移動移均をとることは、変化量のノイズを減らす効果もあることがわかります。

これは移動平均の性質から考察できます。
移動平均データ同士の差分をとった場合に出てくる値は、

\[\begin{align}\overline{val_0}-\overline{val_1}&=\frac{val_0+…+val_{n-1}}{n}-\frac{val_1+…+val_n}{n}\\ &= \frac{val_0-val_n}{n}\end{align}\]
です。
結局、移動平均同士の差分を計算すると
より長い時間間隔で差分をとっている
ことになります。
このことから、
変化量をとる場合には
長い時間間隔で差分をとると区別がつきやすい
ということがわかりますね。

この性質を踏まえて、
遅延時間が気にならない程度に時間間隔をあけて差分をとり、
自分で扱いやすいと思えるよう変化量を算出しました。

→処理内容を書くと長くなるので割愛
→1/msに換算したら7 ms分くらいの変化量な気がしました(真偽は不明)

なんだかんだで求めた変化量

とてもわかりやすくなりました。
センサ値との対応を見てみます。

壁センサ値と変化量の対応

壁が無くなっていく160 ms付近から変化量が発生し、
柱を通り過ぎた後に変化量が急激に大きくなっていることがわかります。
これなら、微妙なノイズと壁の切れ目との区別がつけられます。

最後にどれだけ改善したかを示すために、
生値だけのものを示しておきます。

比較用生値


・制御量の取り方

以前は制御量の算出には比例項のみしか入れていませんでしたが、
微分項も加えてPD制御にしてみました。
こちらを参考にさせてもらいました。

[rogy Advent Calendar 2015] マイクロマウスと制御理論 (1)壁トレース制御 -Tokoro's Tech-Note


・制御量の入れ方

制御量の入れ方です。
前回に加えてMice Wikiに従うと、

\[v_R=v+\omega \frac{W}{2}+(制御量)\]
\[v_L=v-\omega \frac{W}{2}-(制御量)\]

となります。
ステッパなら別にこれでもいいのですが、
壁制御の性質を考えてみると
これって左右に速度差を発生させる点では角速度の目標値を設定するのと同じですね。
なので、角速度項と壁制御項を合わせて以下のようにしました。
→DCマウスで学んだ

\[v_R=v+\omega '\frac{W}{2}\]
\[v_L=v-\omega '\frac{W}{2}\]
\[\omega '=\omega_{tar}+\sum{wall_i}\]

ここで合えて総和として表しているのは、
今回触れた探索用の壁制御の他にも、
櫛対策で入れる壁制御や
斜め走行時での壁制御での制御量もω'に加えているからです。

ω'内の各項については、使わないものは0としておけば大丈夫です。
例)
直進動作→ω&斜め制御=0、探索用壁制御&櫛制御ON,
旋回動作→ω=目標値、壁制御=0



こんな具合で壁制御を導入してみました。
過去記事でもありましたが、
実際に壁制御を運用するにあたり調整しなければいけないパラメータがあります。
そのへんは調整方法の方に書こうかなと思います。
→疲れた


DATE: 2018/09/07(金)   CATEGORY: Nucleoマウス
Nucleo-32boardを使ったクラシックマウスの開発その20 ~超信地旋回とスラローム走行~


今回は回るお話しです。
前回までで走って止まれるようになったと思いますので、
今回は旋回できることを目指します。


・並進方向と回転方向

こちらが非常にわかりやすいです。

車輪移動ロボット - 機械知能工学科 - 東北学院大学

左右の車輪速度は重心速度vと角速度ωを用いて以下のように表せます。
ここでWはトレッド幅を表します。

\[v_R=v+\omega \frac{W}{2}\]
\[v_L=v-\omega \frac{W}{2}\]

重心速度vと角速度ωをうまく与えてあげれば様々な動きに対応できます。


・超信地旋回

超信地旋回はこういう動作です。




超信地旋回では重心速度は0、角速度ωのみ変化します。
超信地旋回のvとωの値はこんな感じです。

超信地

前回作った台形加速関数を流用できることがわかると思います。
注意すべきは、重心速度が0なので
最低速度の値に注意した初角速度にしなければいけません。

超信地左右速度


・スラローム走行

スラローム走行はこういう動作です。




スラロームについての詳しいことはこちら。

マウスにおけるスラローム走行 -徒然なるままに書きつくれば

ステッパにおいてスラローム走行は難しいものと思っている方もいますが、
理論的には簡単なものです。
しかも、Miceの先輩方はスラローム走行に関する
わかりやすい記事をたくさん残してくれていますのでどんどん挑戦してください。

超信地旋回のvとωの値はこんな感じです。

左スラローム

スラローム走行に関しては、
最大角速度時において内輪側が最低速度を下回らないようにしましょう。

左スラローム左右速度

探索用のスラロームターンであれば、
オフセットが15~25 mm程度あるターンがいいと思います。(経験則)


これで進んで止まって回れるロボットになったと思います。


DATE: 2018/08/29(水)   CATEGORY: Nucleoマウス
Nucleo-32boardを使ったクラシックマウスの開発その19 ~台形加速の変更点~


今回は台形加速を中心に前作までからの変更した話です。
思い出話として記事にしておきます。

・加速度の変更を割り込み関数で行えるようにした

前作までは台形加速の加速度の変更をmain文で行っていました。
理由としては、
加速区間のwhileループ
等速区間のwhileループ
減速区間のwhileループ
と書けるので自分の中でわかりやすかったからです。

しかし、main文でこのような実装をすると
何か重い処理をする際
加速区間中に処理を終えないと加速モデルが破綻してしまう可能性が浮上しました。

そのため前作では、重い処理は袋小路or直進中のみに縛っていました。

この問題は探索アルゴリズムを改良しようと思った際に大きな壁となります。
所謂歩数マップベースの足立法くらいの簡単な探索なら全く問題にならないのですが、
少し凝った探索をしようとした場合には計算時間を長くとれることは非常に重要となります。


この問題を解決するために加速度の変更を割り込み関数で行えるようにしました。

従来の実装では、
・main文
v-tモデル設定
→while3段ループ

・割り込み関数
速度及び距離の積分

でした。

新しい実装では、
・main文
v-tモデル設定
→while1段ループ

・割り込み関数
速度及び距離の積分
New 加速度の変更関数

としました。
そのため、v-tモデルの設定値を引き継ぐために
パラメータを渡す構造体が必要となりました。



typedef struct {
float targetAccel;
float targetStartVelocity;
float targetMaxVelocity;
float targetEndVelocity;
float targetDistance;

float accelDistance;
float deaccelDistance;

int8_t triFlag;
int8_t stateFlag;
} linearAccel;

void setLinearAccel(linearAccel *linear, float targetAccel,
float targetStartVelocity, float targetMaxVelocity,
float targetEndVelocity, float targetDistance) {

//計算開始
linear->stateFlag = 1;

//入力パラメータの設定
linear->targetAccel = targetAccel;
linear->targetStartVelocity = targetStartVelocity;
linear->targetMaxVelocity = targetMaxVelocity;
linear->targetEndVelocity = targetEndVelocity;
linear->targetDistance = targetDistance;

//加減速距離の計算
if (targetAccel != 0.0) {
linear->accelDistance = (targetMaxVelocity
         * targetMaxVelocity - targetStartVelocity
* targetStartVelocity) / (2.0 * targetAccel);
linear->deaccelDistance = (targetMaxVelocity
* targetMaxVelocity - targetEndVelocity
* targetEndVelocity) / (2.0 * targetAccel);
} else {
linear->accelDistance = 0.0;
linear->deaccelDistance = 0.0;
}

//三角関数の判定
linear->triFlag = 0;

if (fabs(linear->accelDistance + linear->deaccelDistance)
> fabs(targetDistance)) {
linear->accelDistance = targetDistance / 2.0
+ ((targetEndVelocity * targetEndVelocity)
- (targetStartVelocity
* targetStartVelocity)) / (4.0 * targetAccel);

linear->deaccelDistance = targetDistance / 2.0
+ ((targetStartVelocity * targetStartVelocity)
- (targetEndVelocity * targetEndVelocity))
/ (4.0 * targetAccel);

//とりあえず加速
if ((linear->accelDistance / targetAccel < 0)
|| (linear->deaccelDistance
/ targetAccel < 0)) {
linear->accelDistance = 0.0;
linear->deaccelDistance = 0.0;
return;
}

linear->targetMaxVelocity = targetAccel
/ fabs(targetAccel) * sqrtf( targetAccel
* targetDistance + (targetStartVelocity
* targetStartVelocity + targetEndVelocity
* targetEndVelocity) / 2.0);

linear->triFlag = 1;
}

}


割り込み関数内で加速度を変更する関数はこんなのにしてみました。
全て負の値を入れた際には、後ろ向きに進めるようにしておきます。
→スラロームのときに楽


void controlAccel(tarparameter *motion, linearAccel *linear) {
if (linear->stateFlag == 1) { //計算中
if (fabs(motion->dis)
< fabs(linear->accelDistance)) {
//加速
if (linear->targetMaxVelocity
!= linear->targetStartVelocity) {
motion->acc = linear->targetAccel;
} else {
motion->acc = 0.0;
}
} else if (fabs(motion->dis)
< fabs(linear->targetDistance
- linear->deaccelDistance)) {
//等速
motion->vel = linear->targetMaxVelocity;
motion->acc = 0.0;
} else if (fabs(motion->dis)
< fabs(linear->targetDistance)) {
//減速
if (linear->triFlag == 1) {
motion->vel =
linear->targetMaxVelocity;
motion->acc = 0.0;
linear->triFlag = 0;
}
if (linear->targetMaxVelocity
!= linear->targetEndVelocity) {
motion->acc = -1.0
* linear->targetAccel;
} else {
motion->acc = 0.0;
}
} else if (fabs(motion->dis)
>= fabs(linear->targetDistance)) {
//計算終了
motion->acc = 0.0;
motion->vel = linear->targetEndVelocity;
linear->stateFlag = 0;
} else { //計算しない
linear->stateFlag = 0;
}
} else { //計算しない
linear->stateFlag = 0;
}
}



・最適化オプションを変更した

色々と変更を加えたおかげでmain文の待機whileループが1段となりました。
嬉しい。


void waitMotion(void) {
while( (g_linearAccel.stateFlag
| g_linearAngularAccel.stateFlag) == 1) {
}
}


とはじめは良かったのですが、
SW4STM32からTrueSTUDIOに移行するにあたりこの関数が問題となりました。
移植したらこのループが抜けられないんですね。
2つのフラグを見ていても値は変わっているのに、
ループが抜けられません、、、

と泣きそうになりながら原因を探っていたところ
どうやら最適化のせいみたいです。
なので、最適化のオプションを変更しました。
→最適化を完全に切るとFLASH容量が危ないので最適化のオプションのみ変更しました

プロジェクト→プロパティ→C Compiler→Optimization→Optimization Level
Optimize for debugging(-Og)に変更しました。
→色々試しましたがこれしか動かなかった

身に覚えのないバグがあったら変えてみてください。


・三角関数加速を導入してみた

今まで3年間台形加速のみでやってきましたが、
先輩にターンが滑ると相談したところ台形加速から卒業しようと話があったので作ってみました。

三角関数を使った加減速についての参考はこちら

滑らかな加速がしたい(三角関数編) -taniho's blog

今回、三角関数加速を導入するにあたりテーマを決めました。
台形加速の線形部分のみを三角関数加速に置換する

具体的にはこういうこと。

三角関数加速

こうするメリットとしては、
せっかく台形加速で作った関数たちを使いまわせる
ことが挙げられます。
→加減速区間が同じ
→台形加速からの変更の評価が行いやすい

デメリットとしては、
最大加速度は台形加速よりも大きくなってしまう
ことですね。

三角関数の加速度


とりあえずやっていきましょう。
唯一変更する点は加速度の変更関数です。

三角関数加速を台形加速に置き換える場合加速度は式(1)のようになります。
→計算すれば求まる

\[a(t)=\frac{\pi a}{2}\sin{\frac{\pi a}{v_{max}-v_{start}}t} \tag{1}\]

式(1)の厄介な部分は台形加速とは異なり、時間により加速度が変化していくところです。
つまり、割り込み回数を数えておかないと加速度の計算ができません。
目標値構造体に計算回数を加えます。

これらに注意して加速度関数を書き換えました。


if (fabs(motion->dis) < fabs(linear->accelDistance)) { //加速
motion->n++;
if (linear->targetMaxVelocity
!= linear->targetStartVelocity) {
motion->acc = linear->targetAccel * M_PI / 2.0
* sinf( linear->targetAccel * M_PI
/ (linear->targetMaxVelocity
- linear->targetStartVelocity)
* (float) motion->n * DT);
} else {
motion->acc = 0.0;
}
}


これで三角関数加速ができました。


・sinf関数の処理時間

上の式を見て気になる人がいるかもしれないので補足として調べました。

sinf関数やらは処理に時間がかかるため組み込みでの多用は注意!!
と教えてもらってきました。

→テーブルを作って使用するなど

実際にどのくらい処理に時間がかかるか簡単に調べてみました。
割り込み周期は1 msですので、1 ms間に何回sinf関数が行えるか原始的に調べます。
→わからないことはやってみる


float sin = 0.0;
int i = 0;
g_timCount = 0;
HAL_TIM_Base_Start_IT(&htim6);
while (g_timCount < 1) {
sin = sinf((float) i * M_PI / 180.0);
i++;
}
printf("%d,%f\n", i, sin);


これを実行したところ、出力結果は62回でした。
今後の実用を考えた場合、
1回の割り込みでsinf関数の使用は並進方向と回転方向分の2回くらいでしょうか?

余裕ですね。
じゃんじゃんsinf関数を使っていくことにします。


Copyright © うむ夫の歩み. all rights reserved.
上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。