JASALMAのプログラム解読Part1 走行系1

お久しぶりです。

 

やばいテストを4日後に控えて余裕がないのに現実逃避するために書いているブログです(-_-;)

 

さて、今日は前々から書こ書こうと思っていたJASALMAのプログラム解読、そのPart1として走行関連、主に台形加速付近までを書ければと思っています。

 

注意

ブログ名でも記しているように独り言ブログなので、あくまでも自分個人の考え、実装であり正しいor最適なものだとは1ミリも思っておりませんのでその点はあらかじめご了承ください。

というかこうしたほうがいいんじゃない?等々あればコメントでも見かけたときにでもTwitterにでも書いて教えてください。

 

まずJASALMAで使用しているモータはステッピングモーター(オリエンタルモーターさんのPKE243A-L)です。

ステッピングモーターの詳しい原理等は省きますが簡単に言うとパルス波が入力されると定められた角度変化するよ、というものだそうです。(ほんまか?)

このパルス波形の生成にはMTUを用いました。

MTUの初期設定やその仕組み(?)等々に関してはデータシート読んでもらえればと思います。

(自分は沢山先輩に助けていただいたので偉そうなこと言えませんが・・・)

書いてあるのはピン設定・カウントのタイミング(ここではMPΦ/1でカウント)・カウントのタイミング・TCNTのクリアのタイミングの設定・割り込み許可・モード2に設定etcです。

そんなこんなでMTUで矩形波が出せるようになるとモータが回るのももう少しです。(ホンマか?)

 

 

さあさあ、ここから実際にモータを動かすためのプログラムを解読していきます。

 

まず大前提としてこのプログラムではCMTで1msごとに速度や壁制御の制御量等々を計算し、その値をMTUの割り込みのタイミングでTGRAの値を変更することでモーターの回転速度を変更しています。

   ↓↓↓

つまり、任意の速度を入力した際にその速度に対応したモーターの回転速度となるようなTGRAの値を出力させる計算式が必要!

 

そのためにすごく参考になったサイト(pdf)がこちら

(正直上のリンクですべて事足りるのでは?とも思いますが頑張って書いていきます・・・)

 

それでは自分の計算用紙を載せとくので説明は省略します。

f:id:kt33-JASALMA:20171222001859j:plain
f:id:kt33-JASALMA:20171222001853j:plain

 

はい、ごめんなさい、これでわかるわけないですよね。

(↑書いた本人もわかってないです...)

 

まずTGRBの値はステッピングモーターがパルスを認識するのにに必要な時間が確保できる最小の値に固定しておきました。

このソースコードでは内部クロック(25[MHz])のタイミングでカウントしています。

つまり1カウントあたりにかかる時間は0.00000004[s]です。

また、1パルスで0.9°回転するモーターであるためステップ距離[mm/1パルス]は

     ステップ距離=直径×π×(0.9/360)[mm/1パルス]

で求められます。ここでタイヤの直径を51.0とすればこの値は0.400…となります。

また上記のサイトよりパルス周波数をfとすればこれは

     f=速度/ステップ距離[1/s] -①

となります。

さらにパルス周波数とはTGRAによって生じるパルスの周期の逆数によって与えられるものであるためその値は以下のように与えられます。

     f=1/(0.00000004×TGRA) -②

ここで①と②よりTGRAの値について整理すれば

     TGRA=ステップ距離/(0.00000004×速度)

          ≒10013/速度 ←なんか3桁くらいずれてる説あります、ご注意ください

程度となります。のちのちタイヤ径の調整をすることも考えられますからその値は変数として変更できるように書いておくといいかと思います。

(その計算はお任せします)

 

さあ、これで任意の速度でモーターを回転させることができるようになりました。 

 

 

加減速についてです。

台形加速とはその名の通り(?)時刻t-速度vのグラフの概形が台形となるように速度を変化させることを指しています。

なぜそんな加速のさせかたをしなければならないかというと、ステッピングモーターが急激な速度変化に対応しきれずに脱調を起こしてしまうためです。

 つまりいきなり全速力では走り出せないからしっかり遅い速度から加速してやろうな!、という話です。

 

基本的に加減速等の流れとしてはCMTの割り込みの度に速度を計算し、この計算によって得られた値をMTU割り込みにタイミングで先ほど求めた式に突っ込んであげる、という形です。

まず速度の計算には

     v=v0+αt

の式を用います。(厳密にはこの積分としてのしての考え方のほうが近いと思います)

CMTの割り込みを1[ms](=0.001[s])毎と設定しているため速度vは

     v+=0.001×α

と表現できると思います。

 

 これで任意の加速度で速度を変更出来るようになったと言えるでしょう。

 

さらにここからは台形加速や三角加速についてです。

 

・・・と言いたいところですがもう疲れたのと皆さんが突っ込みたいことも溜まってきたころだと思うので今回のブログはここまでとします。

 

それではまた(^.^)/~~~

G

 

P.S.  そういえばテスト前とか書いてましたが、気づいたらテスト終わってました。

明日はcheese杯に向けtの調整、明後日以降はまた新作の設計をやろうと思っています。