本チュートリアルでは前のチュートリアル(C-5)で開発済みのソースコードを再使用し、標準的サーボモータの制御方法を説明する。ここで言う「標準的サーボモータ」は、ラジコン販売店で購入可能なものを指す。

サーボモータについて

サーボモータは角度制御付きモータである。つまり位置制御可能なモータである。

Servo原理は次の通り:減速によりトルクを増加させるギアモータを使用し、軸の端でポテンショメータが角度位置を測定する。コンパレータ・アンプリファイアに角度値が伝送され、アンプリファイアは角度がコマンドと一致するまでモータを回転させる。

これは完全にアナログでも作製可能である。この場合、ポテンショメータのアナログ値はアナログコマンドを受信するアナログコンパレータに入力される。

部分的にデジタルも可能なので、REOボードで1つ(または複数)のサーボを作製することが可能である。この場合、ポテンショメータ及びアナログコマンドの測定値はREOボードを使用し読み込まれる。コマンドと測定値は比較され、チュートリアル6で説明された方法により適切な方向にモータが回転可能となる。

しかしここではサーボ制御に絞って説明する。

HYP DH13ラジコンの世界では小型サーボがたくさん存在する。ここで使用するのは中型のメタルギアサーボモータであるが、他の安価なサーボ(1000円以下の物)でも使用可能である。

既存モーターを使用する利点は、制御ループが組み込まれていることである。このようなサーボには電線が3本ある。それぞれ、電力用、グランド用、信号用に使用される。その信号は、LEDや動作制御のチュートリアルでの解説通りに、ポテンショメーターの位置測定により生成されサーボに転送される。

信号はPWMの一種でその生成方法は既知であるので、今回の実装が容易にできる。

下の概略図はタイミングの詳細を示している。この種のPWMは20ms周期。オン幅は1〜2msで、その中央値は1.5msである。この値はメーカーによりわずかに異なる(例:双葉の規格では中央値が1.52ms)。

 

HYP Timing

設定がわかりやすいように、20ms間隔を20000分割する解決方法がある。この場合、タイマーのCCR0値を20000(正確には19999)に設定する。そして、サーボ信号に対応するCCRxは1000(最小値)と2000(最大値)の間に設定される。その場合、CCRxの値はms単位となっている。

ポテンショメーターの値(val)は0〜4095である。

目標の幅は1000〜2000である。

従って、目標の値は大体1000 +(val / 4)で算出可能である。

 

 方法のまとめ

  • 20ms間隔を20000分割すると、1部分は1μsとなる。結果的に、タイマーを1MHzクロックで設定する必要がある。
  • タイマー周期用の値として19999を使用する
  • PWM信号のパルス幅として1000+command_valueを使用する

プログラム

当プログラムの新規部分は1MHzのタイマークロックを生成することである。前述の通り、32kHzのACLKか24MkHzのMCLK(マスタークロック)のいずれかが選択可能である。タイマークロックはタイマーを入れる前に予め分割される。クロックを24で分割すると、1MHzのタイマーが得られる。

デバイダは2つある。1つ目は2,3,4,5,6,7,8分割に設定可能で、2つ目は2,4,8分割に設定可能である。24で割る場合、1つ目は3に設定し、2つ目は8に設定(或いは6/4に設定)すれば、24分割が実現できる。

サーボ制御用PWMの設定方法は以下の通り

--------------------

void EnableServoPWM(void) {
    P4DIR |= 0x02;                            //    P4.2 output
    P4SEL |= 0x02;                            //    P4.2 as timer output
    TB0CCR0 = 19999;                          //    PWM Period
    TB0CCTL1 = OUTMOD_7;                      //    CCR1 reset/set
    TB0CCR1 = 1500;                           //    CCR1 average vakye
    TB0EX0 = 2;                               //    Divider by 3
    TB0CTL = TBSSEL_2 + MC_1 + TBCLR + ID_3;  //    SMCLK, up mode, clear TAR, divider by 8
}

--------------------

前述のLED制御との違いは、最後の2行のクロック分割である。

次の新しい関数は、角度の設定のためのものである。12bitADコンバータからの値を受信するので、0〜4095の値となる。0〜1000の値を設定したい場合の方法は、まず2bitでシフトし、0〜1023の値を作ることである。

--------------------void SetAngle(uint16 angle) {
    angle >>= 2;
    if(angle > 1023) angle = 1023;
    TB0CCR1 = 1000 + angle;
}

--------------------

以下に全ソースコードを示す。

ソースコード

--------------------

#include <msp430F5659.h>
#include "F5659Utils.h"

void EnableServoPWM(void);
void EnableADCTimer(void);
void EnableADC();
void SetAngle(uint16 angle);
uint16 ReadADCVal(void);

int main(void) {
    WDTCTL = WDTPW + WDTHOLD;                //    Stop WDT
    SetCoreVoltage(VCORE19);                 //    Set the core to 1.9V
    SetFLL(24);                              //    Set SMCLK to 24 MHz
    EnableServoPWM();
    EnableADCTimer();
    EnableADC();
    __bis_SR_register(LPM0_bits + GIE);      //    Enter LPM0, enable interrupts
}

void EnableServoPWM(void) {
    P4DIR |= 0x02;                           //    P4.2 output
    P4SEL |= 0x02;                           //    P4.2 as timer output
    TB0CCR0 = 19999;                         //    PWM Period
    TB0CCTL1 = OUTMOD_7;                     //    CCR1 reset/set
    TB0CCR1 = 1500;                          //    CCR1 PWM duty cycle
    TB0EX0 = 2;                              //    Divider by 3
    TB0CTL = TBSSEL_2 + MC_1 + TBCLR + ID_3; //    SMCLK, up mode, clear TAR, divider by 8
}

void SetAngle(uint16 angle) {
    angle >>= 2;
    if(angle > 1023) angle = 1023;
    TB0CCR1 = 1000 + angle;
}

void EnableADCTimer(void) {
  TA1CCTL0 = CCIE;                          // CCR0 interrupt enabled
  TA1CCR0 = 511;                            //    Check ADC every 15.6 ms (64 Hz)
  TA1CTL = TASSEL_1 + MC_1 + TACLR;         // SMCLK, upmode, clear TAR
}

void EnableADC() {
    ADC12CTL0 = ADC12ON+ADC12SHT0_2;        // Turn on ADC12, set sampling time
    ADC12CTL1 = ADC12SHP;                   // Use sampling timer
    ADC12CTL0 |= ADC12ENC;                  // Enable conversions
}

uint16 ReadADCVal(void) {
    uint16 retval;                          //    Value for debugging purpose
    ADC12CTL0 |= ADC12SC;                   //    Start conversion-software trigger
    while (!(ADC12IFG & BIT0));             //    Wait for the ADC
    retval = ADC12MEM0;                     //    Set breakpoint here to verify value
    return retval;
}

// Timer0 A0 interrupt service routine
#pragma vector=TIMER1_A0_VECTOR
__interrupt void TIMER1_A0_ISR(void) {
    uint16    adcval;
    adcval = ReadADCVal();
    SetAngle(adcval);
}

--------------------